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)

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -225,9 +225,12 @@ def peripheral_cfg(
             time.sleep(0.5)
 
         if set_initial_tool_state:
-            set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
-            time.sleep(1.5)
-            set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
+            set_tool_position(
+                robot,
+                percent=1.0,
+                device=1,
+                scissorgripper=scissorgripper,
+            )
 
     elif scissorgripper == 2:
         # 小型剪刀夹爪通过控制器 DO3/DO4 控制。

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -297,6 +297,11 @@ class SingleArmVelocityTeleop(Node):
         self._control_fault_latched = False
         self._stop_sent = True
         self._trigger_tool_open = True
+        self._tool_state_lock = threading.Lock()
+        self._tool_target_open = True
+        self._tool_state_open: bool | None = None
+        self._tool_command_pending = False
+        self._tool_command_failed = False
         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
@@ -430,11 +435,20 @@ class SingleArmVelocityTeleop(Node):
 
     def _setup_tool_control(self) -> None:
         peripheral_arm = self._peripheral_arm_name()
-        if self._bool_parameter("configure_peripheral_on_connect"):
+        configure_on_connect = self._bool_parameter(
+            "configure_peripheral_on_connect"
+        )
+        if configure_on_connect:
             self._adapter.configure_peripheral(
                 self._peripheral_config,
                 peripheral_arm,
             )
+            if self._peripheral_config.set_initial_tool_state:
+                with self._tool_state_lock:
+                    self._tool_target_open = True
+                    self._tool_state_open = True
+                    self._tool_command_pending = False
+                    self._tool_command_failed = False
 
         if not self._enable_tool_control:
             if self._enable_trigger_gripper_control:
@@ -484,6 +498,10 @@ class SingleArmVelocityTeleop(Node):
             )
             return
 
+        with self._tool_state_lock:
+            self._tool_target_open = open_tool
+            self._tool_command_pending = True
+
         item = (open_tool, source)
         while True:
             try:
@@ -512,15 +530,34 @@ class SingleArmVelocityTeleop(Node):
                 try:
                     self._adapter.set_tool_enabled(open_tool)
                 except Exception as exc:
+                    with self._tool_state_lock:
+                        self._tool_command_failed = True
                     self.get_logger().error(
                         f"{self._arm_name} tool {action} failed from {source}: {exc}"
                     )
                     continue
+                with self._tool_state_lock:
+                    self._tool_state_open = open_tool
+                    self._tool_command_failed = False
                 self.get_logger().info(
                     f"{self._arm_name} tool {action} command sent from {source}"
                 )
             finally:
                 self._tool_command_queue.task_done()
+                if self._tool_command_queue.empty():
+                    with self._tool_state_lock:
+                        self._tool_command_pending = False
+
+    def _tool_state_snapshot(
+        self,
+    ) -> tuple[bool, bool | None, bool, bool]:
+        with self._tool_state_lock:
+            return (
+                self._tool_target_open,
+                self._tool_state_open,
+                self._tool_command_pending,
+                self._tool_command_failed,
+            )
 
     def _peripheral_arm_name(self) -> str:
         configured = str(self.get_parameter("peripheral_arm").value).strip().lower()