feat: 添加手柄主键回初始位姿

xr_rm_teleop/test/test_joint_control.py

@@ -30,6 +30,76 @@ class FakeTime:
         return SimpleNamespace(nanoseconds=0)
 
 
+def _primary_button_teleop(*, move_error=None):
+    events = []
+    errors = []
+    snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
+
+    class Adapter:
+        def move_to_initial_pose(self):
+            events.append("move")
+            if move_error is not None:
+                raise move_error
+
+        def read_joint_state(self):
+            events.append("read")
+            return snapshot
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._adapter = Adapter()
+    teleop._last_primary_pressed = None
+    teleop._grip_rearm_required = False
+    teleop._safe_stop = lambda reset_active: events.append(
+        ("stop", reset_active)
+    )
+    teleop._reset_joint_state = lambda value: events.append(("sync", value))
+    teleop._handle_trigger_gripper = lambda msg: None
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    teleop.get_logger = lambda: SimpleNamespace(
+        info=lambda message: None,
+        error=lambda message: errors.append(message),
+    )
+    return teleop, events, errors, snapshot
+
+
+def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
+    teleop, events, _, snapshot = _primary_button_teleop()
+    released = SimpleNamespace(primary=False)
+    pressed = SimpleNamespace(primary=True)
+
+    teleop._on_controller(released)
+    teleop._on_controller(pressed)
+    teleop._on_controller(pressed)
+    teleop._on_controller(released)
+    teleop._on_controller(pressed)
+
+    expected_once = [
+        ("stop", True),
+        "move",
+        "read",
+        ("sync", snapshot),
+    ]
+    assert events == expected_once * 2
+    assert teleop._grip_rearm_required
+
+
+def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
+    failure = RuntimeError("rm_movej failed")
+    teleop, events, errors, _ = _primary_button_teleop(
+        move_error=failure
+    )
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert events == [("stop", True), "move"]
+    assert teleop._grip_rearm_required
+    assert errors == [
+        "right_rm75 回初始位姿失败：rm_movej failed"
+    ]
+
+
 def test_startup_joint_query_initializes_qp_and_command_history() -> None:
     positions = [0.1] * 7
     pose = np.eye(4)

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -295,6 +295,7 @@ class SingleArmVelocityTeleop(Node):
         self._control_fault_latched = False
         self._stop_sent = True
         self._trigger_tool_open = True
+        self._last_primary_pressed: bool | None = None
         self._last_trigger_pressed: bool | None = None
         self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
         self._tool_worker_stop = threading.Event()
@@ -514,8 +515,32 @@ class SingleArmVelocityTeleop(Node):
     def _on_controller(self, msg: XrController) -> None:
         self._last_msg = msg
         self._last_msg_time = self.get_clock().now()
+        self._handle_initial_pose_button(msg)
         self._handle_trigger_gripper(msg)
 
+    def _handle_initial_pose_button(self, msg: XrController) -> None:
+        if self._last_primary_pressed is None:
+            self._last_primary_pressed = msg.primary
+            return
+
+        rising_edge = msg.primary and not self._last_primary_pressed
+        self._last_primary_pressed = msg.primary
+        if not rising_edge:
+            return
+
+        self._grip_rearm_required = True
+        self._safe_stop(reset_active=True)
+        try:
+            self._adapter.move_to_initial_pose()
+            self._reset_joint_state(self._adapter.read_joint_state())
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 回初始位姿失败：{exc}"
+            )
+            return
+
+        self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
+
     def _handle_trigger_gripper(self, msg: XrController) -> None:
         if not self._enable_tool_control or not self._enable_trigger_gripper_control:
             return