Clear acceleration on e-stop

husarion · Jul 24, 2023 · 149e106 · 149e106
1 parent 561766b
commit 149e106
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 0 deletions.
diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py
@@ -428,6 +428,8 @@ def _safety_timer_cb(self, *args) -> None:
 
     def _e_stop_cb(self, data: Bool) -> None:
         with self._lock:
+            if data.data:
+                self._velocity_smoother.reset()
             self._e_stop_cliented = data.data
 
     def _cmd_vel_cb(self, data: Twist) -> None:

diff --git a/panther_driver/src/velocity_smoother.py b/panther_driver/src/velocity_smoother.py
@@ -79,6 +79,11 @@ def get_smooth_velocity(
         self._robot_vel_theta = output_vel.angular.z
 
         return output_vel
+
+    def reset(self) -> None:
+        self._robot_vel_x = 0.0
+        self._robot_vel_y = 0.0
+        self._robot_vel_theta = 0.0
 
     def _accel_limiter(
         self, cmd_vel: float, robot_vel: float, acc_factor: float, decel_factor