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

2026-07-31 15:31:35 +08:00
parent 5267da14c2
commit 4e068ce637
2 changed files with 95 additions and 0 deletions
@@ -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)