feat: Enhance single arm velocity teleoperation with zero velocity handling and new parameters

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -40,6 +40,7 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("kp_linear", 2.0)
         self.declare_parameter("deadband_m", 0.002)
         self.declare_parameter("low_pass_alpha", 0.35)
+        self.declare_parameter("velocity_zero_epsilon", 1e-6)
         self.declare_parameter("max_linear_speed", 0.05)
         self.declare_parameter("enable_position_axes", [True, True, True])
         self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
@@ -82,6 +83,7 @@ class SingleArmVelocityTeleop(Node):
         self._kp_linear = float(self.get_parameter("kp_linear").value)
         self._deadband_m = float(self.get_parameter("deadband_m").value)
         self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
+        self._velocity_zero_epsilon = float(self.get_parameter("velocity_zero_epsilon").value)
         self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
         self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
         self._workspace_min = self._float_list_parameter("workspace_min", 3)
@@ -217,6 +219,7 @@ class SingleArmVelocityTeleop(Node):
             alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
             for i in range(3)
         ]
+        self._filtered_velocity = self._zero_tiny_velocity(self._filtered_velocity)
         cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
         self._publish_debug(robot_pose, target, cartesian_velocity)
         if self._adapter_uses_pose_targets():
@@ -290,10 +293,18 @@ class SingleArmVelocityTeleop(Node):
     def _safe_stop(self) -> None:
         # 所有断连、松手、退出路径都走这里，确保发送零速度。
         self._filtered_velocity = [0.0, 0.0, 0.0]
+        self._publish_stop_debug()
         if self._adapter_uses_pose_targets():
             return
         self._send_cartesian_velocity([0.0] * 6)
 
+    def _publish_stop_debug(self) -> None:
+        try:
+            current_pose = self._adapter.get_current_pose()
+        except Exception:
+            return
+        self._publish_debug(current_pose, current_pose.xyz(), [0.0] * 6)
+
     def _send_cartesian_velocity(self, velocity: list[float]) -> None:
         try:
             self._adapter.send_cartesian_velocity(velocity, self._follow)
@@ -318,6 +329,14 @@ class SingleArmVelocityTeleop(Node):
         uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None)
         return bool(uses_pose_targets is not None and uses_pose_targets())
 
+    def _zero_tiny_velocity(self, velocity: list[float]) -> list[float]:
+        if self._velocity_zero_epsilon <= 0.0:
+            return velocity
+        return [
+            0.0 if abs(value) < self._velocity_zero_epsilon else value
+            for value in velocity
+        ]
+
     def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
         stamp = self.get_clock().now().to_msg()
         current_msg = self._pose_msg(stamp, current_pose)
@@ -332,6 +351,7 @@ class SingleArmVelocityTeleop(Node):
                 rz=current_pose.rz,
             ),
         )
+        velocity = self._zero_tiny_velocity(velocity)
         velocity_msg = TwistStamped()
         velocity_msg.header.stamp = stamp
         velocity_msg.header.frame_id = "rm_base"
@@ -406,6 +426,8 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("deadband_m must be >= 0")
         if not 0.0 <= self._low_pass_alpha <= 1.0:
             raise ValueError("low_pass_alpha must be in [0, 1]")
+        if self._velocity_zero_epsilon < 0.0:
+            raise ValueError("velocity_zero_epsilon must be >= 0")
         if self._max_linear_speed < 0.0:
             raise ValueError("max_linear_speed must be >= 0")
         for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):