Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ROS 1 velocity smoother #129

Merged
merged 9 commits into from
Jun 29, 2023
Merged
Show file tree
Hide file tree
Changes from 7 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions panther_bringup/config/panther_common.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -9,3 +9,17 @@ motor_torque_constant: 2.6149
gear_ratio: 30.08
encoder_resolution: 1600 # 400 * 4
robot_length: 0.44

# velocity smoother params
max_vel_x: 2.0
max_vel_y: 2.0
max_vel_theta: 4.0
acc_lim_x: 1.0
acc_lim_y: 1.0
acc_lim_theta: 1.57
decel_lim_x: 1.5
decel_lim_y: 1.5
decel_lim_theta: 2.3
emergency_decel_lim_x: 2.7
emergency_decel_lim_y: 2.7
emergency_decel_lim_theta: 5.74
15 changes: 15 additions & 0 deletions panther_driver/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,21 @@ For a `/joint_states` message is carrying given data:
- `~wheel_radius` [*float*, default: **0.1825**]: wheel radius in **[m]**.
- `~wheel_separation` [*float*, default: **0.697**]: separation of wheels alongside y axis in **[m]**.

#### Velocity smoother parameters

- `~max_vel_x` [*float*, default: **2.0**]: maximum linear vlelocity in **X** direction.
- `~max_vel_y` [*float*, default: **2.0**]: maximum linear vlelocity in **Y** direction.
- `~max_vel_theta` [*float*, default: **4.0**]: maximum angular velocity.
- `~acc_lim_x` [*float*, default: **1.0**]: maximum linear acceleration in **X** direction.
- `~acc_lim_y` [*float*, default: **1.0**]: maximum linear acceleration in **Y** direction.
- `~acc_lim_theta` [*float*, default: **1.57**]: maximum angular acceleration.
- `~decel_lim_x` [*float*, default: **1.5**]: maximum linear decelaration in **X** direction.
- `~decel_lim_y` [*float*, default: **1.5**]: maximum linear decelaration in **Y** direction.
- `~decel_lim_theta` [*float*, default: **2.3**]: maximum angular deceleration.
- `~emergency_decel_lim_x` [*float*, default: **2.7**]: maximum linear decelaration in **X** direction when a timeout is reached for velocity commands.
- `~emergency_decel_lim_y` [*float*, default: **2.7**]: maximum linear decelaration in **Y** direction when a timeout is reached for velocity commands.
- `~emergency_decel_lim_theta` [*float*, default: **5.74**]: maximum angular deceleration when a timeout is reached for velocity commands.

#### Kinematics type - explanation

The Panther robot can be configured with different wheels to match your needs, we provide 2 different kinematics types `differential`/`mecanum`. You can change the wheel type by providing an appropriate launch parameter with a path to the wheel configuration file - `wheel_config_file`. Basic wheel configuration files (*WH01.yaml, WH02.yaml, WH04.yaml*): are located in `panther_description` package.
60 changes: 45 additions & 15 deletions panther_driver/src/driver_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
from panther_msgs.msg import DriverState, FaultFlag, IOState, RuntimeError, ScriptFlag
from panther_can import PantherCANSDO, PantherCANPDO
from panther_kinematics import PantherDifferential, PantherMecanum
from velocity_smoother import VelocitySmoother


class DriverFlagLogger:
Expand Down Expand Up @@ -87,6 +88,19 @@ def __init__(self, name: str) -> None:
robot_length = rospy.get_param('~robot_length', 0.44)
wheel_radius = rospy.get_param('~wheel_radius', 0.1825)

max_vel_x = rospy.get_param('~max_vel_x', 2.0)
max_vel_y = rospy.get_param('~max_vel_y', 2.0)
max_vel_theta = rospy.get_param('~max_vel_theta', 4.0)
acc_lim_x = rospy.get_param('~acc_lim_x', 1.0)
acc_lim_y = rospy.get_param('~acc_lim_y', 1.0)
acc_lim_theta = rospy.get_param('~acc_lim_theta', 1.57)
decel_lim_x = rospy.get_param('~decel_lim_x', 1.5)
decel_lim_y = rospy.get_param('~decel_lim_y', 1.5)
decel_lim_theta = rospy.get_param('~decel_lim_theta', 2.3)
emergency_decel_lim_x = rospy.get_param('~emergency_decel_lim_x', 2.7)
emergency_decel_lim_y = rospy.get_param('~emergency_decel_lim_y', 2.7)
emergency_decel_lim_theta = rospy.get_param('~emergency_decel_lim_theta', 5.74)

self._wheels_joints_names = [
'fl_wheel_joint',
'fr_wheel_joint',
Expand All @@ -111,7 +125,22 @@ def __init__(self, name: str) -> None:

self._e_stop_cliented = False
self._motor_on = False
self._stop_cmd_vel_cb = True
self._stop_motors = True

self._velocity_smoother = VelocitySmoother(
max_vel_x,
max_vel_y,
max_vel_theta,
acc_lim_x,
acc_lim_y,
acc_lim_theta,
decel_lim_x,
decel_lim_y,
decel_lim_theta,
emergency_decel_lim_x,
emergency_decel_lim_y,
emergency_decel_lim_theta,
)

# -------------------------------
# Kinematic type
Expand Down Expand Up @@ -269,12 +298,18 @@ def _main_timer_cb(self, *args) -> None:
dt = (time_now - self._time_last).to_sec()
self._time_last = time_now

if (time_now - self._cmd_vel_command_last_time) < rospy.Duration(
secs=self._cmd_vel_timeout
):
self._panther_can.write_wheels_enc_velocity(
self._panther_kinematics.wheels_enc_speed
if (time_now - self._cmd_vel_command_last_time) > rospy.Duration(self._cmd_vel_timeout):
self._cmd_vel = Twist()
emergency_breaking = True
else:
emergency_breaking = False

if not self._stop_motors:
cmd_vel = self._velocity_smoother.get_smooth_velocity(
self._cmd_vel, self._main_timer_period, emergency_breaking
)
self._panther_kinematics.inverse_kinematics(cmd_vel)
self._panther_can.write_wheels_enc_velocity(self._panther_kinematics.wheels_enc_speed)
else:
self._panther_can.write_wheels_enc_velocity([0.0, 0.0, 0.0, 0.0])

Expand Down Expand Up @@ -361,7 +396,7 @@ def _safety_timer_cb(self, *args) -> None:
if any(self._panther_can.can_connection_error()):
if not e_stop_cliented:
self._trigger_panther_e_stop()
self._stop_cmd_vel_cb = True
self._stop_motors = True

if not self._motor_on:
rospy.logwarn_throttle(
Expand All @@ -377,21 +412,16 @@ def _safety_timer_cb(self, *args) -> None:
f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure)',
)
elif self._e_stop_cliented:
self._stop_cmd_vel_cb = True
self._stop_motors = True
else:
self._stop_cmd_vel_cb = False
self._stop_motors = False

def _e_stop_cb(self, data: Bool) -> None:
with self._lock:
self._e_stop_cliented = data.data

def _cmd_vel_cb(self, data: Twist) -> None:
# Block all motors if any Roboteq controller returns a fault flag or runtime error flag
if not self._stop_cmd_vel_cb:
self._panther_kinematics.inverse_kinematics(data)
else:
self._panther_kinematics.inverse_kinematics(Twist())

self._cmd_vel = data
self._cmd_vel_command_last_time = rospy.Time.now()

def _io_state_cb(self, io_state: IOState) -> None:
Expand Down
93 changes: 93 additions & 0 deletions panther_driver/src/velocity_smoother.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
#!/usr/bin/python3

from geometry_msgs.msg import Twist


class VelocitySmoother:
def __init__(
self,
max_vel_x,
max_vel_y,
max_vel_theta,
acc_lim_x,
acc_lim_y,
acc_lim_theta,
decel_lim_x,
decel_lim_y,
decel_lim_theta,
emergency_decel_lim_x,
emergency_decel_lim_y,
emergency_decel_lim_theta,
) -> None:

self._max_vel_x = max_vel_x
self._max_vel_y = max_vel_y
self._max_vel_theta = max_vel_theta
self._acc_lim_x = acc_lim_x
self._acc_lim_y = acc_lim_y
self._acc_lim_theta = acc_lim_theta
self._decel_lim_x = decel_lim_x
self._decel_lim_y = decel_lim_y
self._decel_lim_theta = decel_lim_theta
self._emergency_decel_lim_x = emergency_decel_lim_x
self._emergency_decel_lim_y = emergency_decel_lim_y
self._emergency_decel_lim_theta = emergency_decel_lim_theta

self._robot_vel_x = 0.0
self._robot_vel_y = 0.0
self._robot_vel_theta = 0.0

def get_smooth_velocity(
self, cmd_vel: Twist, period: float, emergency_breaking: bool = False
) -> Twist:
acc_x_factor = self._acc_lim_x * period
acc_y_factor = self._acc_lim_y * period
acc_theta_factor = self._acc_lim_theta * period
if not emergency_breaking:
decel_x_factor = self._decel_lim_x * period
decel_y_factor = self._decel_lim_y * period
decel_theta_factor = self._decel_lim_theta * period
else:
decel_x_factor = self._emergency_decel_lim_x * period
decel_y_factor = self._emergency_decel_lim_y * period
decel_theta_factor = self._emergency_decel_lim_theta * period

output_vel = Twist()
output_vel.linear.x = self._clamp(
self._accel_limiter(cmd_vel.linear.x, self._robot_vel_x, acc_x_factor, decel_x_factor),
-self._max_vel_x,
self._max_vel_x,
)
output_vel.linear.y = self._clamp(
self._accel_limiter(cmd_vel.linear.y, self._robot_vel_y, acc_y_factor, decel_y_factor),
-self._max_vel_y,
self._max_vel_y,
)
output_vel.angular.z = self._clamp(
self._accel_limiter(
cmd_vel.angular.z,
self._robot_vel_theta,
acc_theta_factor,
decel_theta_factor,
),
-self._max_vel_theta,
self._max_vel_theta,
)

self._robot_vel_x = output_vel.linear.x
self._robot_vel_y = output_vel.linear.y
self._robot_vel_theta = output_vel.angular.z

return output_vel

def _accel_limiter(
self, cmd_vel: float, robot_vel: float, acc_factor: float, decel_factor
) -> float:
if robot_vel >= 0.0:
return robot_vel + self._clamp(cmd_vel - robot_vel, -decel_factor, acc_factor)
else:
return robot_vel + self._clamp(cmd_vel - robot_vel, -acc_factor, decel_factor)
macstepien marked this conversation as resolved.
Show resolved Hide resolved

@staticmethod
def _clamp(value, min_value, max_value):
return max(min(value, max_value), min_value)