feat: 记录夹爪逻辑执行状态

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -6,13 +6,14 @@ from types import ModuleType, SimpleNamespace
 import pytest
 import yaml
 
-from xr_rm_teleop import realman_adapter
+from xr_rm_teleop import fun_peripheral, realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
 from xr_rm_teleop.realman_adapter import MockRealManAdapter
 from xr_rm_teleop.fun_peripheral import (
     PeripheralConfig,
     _configure_tool_frame,
     load_peripheral_config,
+    peripheral_cfg,
 )
 
 
@@ -82,6 +83,78 @@ def test_deployed_peripheral_config_selects_left_and_right_tools() -> None:
     assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
 
 
+def test_right_tool_initializes_open_only_for_right_arm() -> None:
+    config_file = CONFIG_DIR / "peripherals_rm75.yaml"
+    left = load_peripheral_config(str(config_file), "left")
+    right = load_peripheral_config(str(config_file), "right")
+
+    assert not left.set_initial_tool_state
+    assert right.set_initial_tool_state
+
+
+def test_omnipic_initial_state_opens_fully(monkeypatch) -> None:
+    calls = []
+
+    class FakeArm:
+        def rm_set_voltage(self, *args):
+            del args
+
+        def rm_set_io_mode(self, *args):
+            del args
+
+        def rm_algo_quaternion2euler(self, quaternion):
+            del quaternion
+            return [0.0, 0.0, 0.0]
+
+        def rm_get_total_tool_frame(self):
+            return {"return_code": 0, "tool_names": []}
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            del frame
+            return 0
+
+        def rm_change_tool_frame(self, tool_name):
+            del tool_name
+            return 0
+
+        def rm_set_modbus_mode(self, **kwargs):
+            del kwargs
+            return 0
+
+        def rm_write_single_register(self, params, value):
+            del params, value
+            return 0
+
+    sdk = ModuleType("Robotic_Arm.rm_robot_interface")
+    sdk.rm_frame_t = lambda *args: object()
+    sdk.rm_peripheral_read_write_params_t = lambda *args: object()
+    package = ModuleType("Robotic_Arm")
+    package.rm_robot_interface = sdk
+    monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
+    monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
+    monkeypatch.setattr(fun_peripheral.time, "sleep", lambda seconds: None)
+    monkeypatch.setattr(
+        fun_peripheral,
+        "set_tool_position",
+        lambda robot, percent, device, scissorgripper: calls.append(
+            (percent, device, scissorgripper)
+        ),
+    )
+    tools = {
+        "scissor": [[0.0] * 7, [0.0] * 7],
+        "omnipic": [[0.0] * 7, [0.0] * 7],
+    }
+
+    peripheral_cfg(
+        FakeArm(),
+        1,
+        tools,
+        set_initial_tool_state=True,
+    )
+
+    assert calls == [(1.0, 1, 1)]
+
+
 @pytest.mark.parametrize(
     ("config_name", "node_names"),
     [

xr_rm_teleop/test/test_joint_control.py

@@ -1,4 +1,5 @@
 import math
+import threading
 import time
 from types import SimpleNamespace
 
@@ -42,6 +43,69 @@ class FakePublisher:
         self.messages.append(message)
 
 
+def _tool_state_teleop(*, command_error=None):
+    started = threading.Event()
+    release = threading.Event()
+
+    class Adapter:
+        def set_tool_enabled(self, open_tool):
+            del open_tool
+            started.set()
+            assert release.wait(timeout=1.0)
+            if command_error is not None:
+                raise command_error
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._adapter = Adapter()
+    teleop._tool_command_queue = None
+    teleop._tool_worker_stop = threading.Event()
+    teleop._tool_worker_thread = None
+    teleop._tool_state_lock = threading.Lock()
+    teleop._tool_target_open = True
+    teleop._tool_state_open = True
+    teleop._tool_command_pending = False
+    teleop._tool_command_failed = False
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._start_tool_worker()
+    return teleop, started, release
+
+
+def test_tool_state_changes_only_after_command_succeeds() -> None:
+    teleop, started, release = _tool_state_teleop()
+    try:
+        teleop._enqueue_tool_command(False, "test")
+        assert started.wait(timeout=1.0)
+
+        assert teleop._tool_state_snapshot() == (False, True, True, False)
+
+        release.set()
+        assert teleop._tool_command_queue is not None
+        teleop._tool_command_queue.join()
+
+        assert teleop._tool_state_snapshot() == (False, False, False, False)
+    finally:
+        release.set()
+        teleop._shutdown_tool_worker()
+
+
+def test_tool_failure_keeps_previous_state_and_is_reported() -> None:
+    teleop, started, release = _tool_state_teleop(
+        command_error=RuntimeError("modbus failed")
+    )
+    try:
+        teleop._enqueue_tool_command(False, "test")
+        assert started.wait(timeout=1.0)
+        release.set()
+        assert teleop._tool_command_queue is not None
+        teleop._tool_command_queue.join()
+
+        assert teleop._tool_state_snapshot() == (False, True, False, True)
+    finally:
+        release.set()
+        teleop._shutdown_tool_worker()
+
+
 def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
     names = [f"omnipic_joint_{index}" for index in range(1, 8)]
     teleop = object.__new__(SingleArmVelocityTeleop)