From 4e068ce6377157abb8a578c2ba553e3ab837279b Mon Sep 17 00:00:00 2001
From: YikaiFu-cart <yikai.fu@nicecart.ai>
Date: Fri, 31 Jul 2026 15:31:35 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0=E6=89=8B=E6=9F=84?=
 =?UTF-8?q?=E4=B8=BB=E9=94=AE=E5=9B=9E=E5=88=9D=E5=A7=8B=E4=BD=8D=E5=A7=BF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 xr_rm_teleop/test/test_joint_control.py       | 70 +++++++++++++++++++
 .../single_arm_velocity_teleop.py             | 25 +++++++
 2 files changed, 95 insertions(+)

diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py
index aefea68..25c68f1 100644
--- a/xr_rm_teleop/test/test_joint_control.py
+++ b/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)
diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
index 9a81dcd..2913d06 100755
--- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
+++ b/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