ROS 1 velocity smoother

panther_bringup/config/panther_common.yaml

@@ -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

panther_driver/README.md

@@ -36,12 +36,24 @@ For a `/joint_states` message is carrying given data:
 
 #### Parameters
 
+- `~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.
 - `~base_link_frame` [*string*, default: **base_link**]: the name of the base link frame.
 - `~can_interface` [*string*, default: **panther_can**]: the name of the socket CAN interface.
+- `~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.
 - `~eds_file` [*string*, default: **None**]: required path to eds file containing CANopen configuration for Roboteq motor controllers.
+- `~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.
 - `~encoder_resolution` [*int*, default: **1600**]: resolution of motor encoder in **[PPR]**.
 - `~gear_ratio` [*float*, default: **30.08**]: wheel gear ratio.
 - `~kinematics` [*string*, default: **differential**]: kinematics type, possible are: differential, mecanum.
+- `~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.
 - `~motor_torque_constant` [*float*, default: **2.6149**]: constant used to estimate torque.
 - `~odom_frame` [*string*, default: **odom**]: the name of the odom frame.
 - `~publish_joints` [*bool*, default: **true**]: whether to publish robot joints states.

panther_driver/src/driver_node.py

@@ -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:
@@ -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',
@@ -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
@@ -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])
 
@@ -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(
@@ -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:

panther_driver/src/velocity_smoother.py

@@ -0,0 +1,102 @@
+#!/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_velocity = [0.0, 0.0, 0.0]  # vel_x, vel_y, vel_theta
+
+    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._velocity_limiter(
+            self._accel_limiter(
+                cmd_vel.linear.x, self._robot_velocity[0], acc_x_factor, decel_x_factor
+            ),
+            self._max_vel_x,
+        )
+        output_vel.linear.y = self._velocity_limiter(
+            self._accel_limiter(
+                cmd_vel.linear.y, self._robot_velocity[1], acc_y_factor, decel_y_factor
+            ),
+            self._max_vel_y,
+        )
+        output_vel.angular.z = self._velocity_limiter(
+            self._accel_limiter(
+                cmd_vel.angular.z,
+                self._robot_velocity[2],
+                acc_theta_factor,
+                decel_theta_factor,
+            ),
+            self._max_vel_theta,
+        )
+
+        self._robot_velocity[0] = output_vel.linear.x
+        self._robot_velocity[1] = output_vel.linear.y
+        self._robot_velocity[2] = output_vel.angular.z
+
+        return output_vel
+
+    @staticmethod
+    def _accel_limiter(cmd_vel: float, robot_vel: float, acc_factor: float, decel_factor) -> float:
+        if cmd_vel > robot_vel:
+            if robot_vel >= 0.0:
+                return min(cmd_vel, robot_vel + acc_factor)
+            else:
+                return min(cmd_vel, robot_vel + decel_factor)
+        elif cmd_vel < robot_vel:
+            if robot_vel <= 0.0:
+                return max(cmd_vel, robot_vel - acc_factor)
+            else:
+                return max(cmd_vel, robot_vel - decel_factor)
+        else:
+            return cmd_vel
+
+    @staticmethod
+    def _velocity_limiter(cmd_vel: float, max_vel) -> float:
+        if cmd_vel >= 0:
+            return min(cmd_vel, max_vel)
+        else:
+            return max(cmd_vel, -max_vel)