ROS 1 velocity smoother (#129)

* add velocity somoother

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: 2.7
+acc_lim_y: 2.7
+acc_lim_theta: 5.74
+decel_lim_x: 2.7
+decel_lim_y: 2.7
+decel_lim_theta: 5.74
+emergency_decel_lim_x: 2.7
+emergency_decel_lim_y: 2.7
+emergency_decel_lim_theta: 5.74

panther_driver/README.md

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

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,96 @@
+#!/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:
+        # When changing the sign of the command velocity, the robot should stop 
+        # (approach a velocity of 0.0) by applying a deceleration factor. 
+        # Then, it should use acceleration factor to reach the desired velocity value.
+        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)
+
+    @staticmethod
+    def _clamp(value, min_value, max_value):
+        return max(min(value, max_value), min_value)