diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 7ec46f337..ce9e518b1 100755 --- 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 index c8e9d6175..99ddaa49b 100755 --- 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