diff --git a/xr_rm_bringup/config/peripherals_rm75.yaml b/xr_rm_bringup/config/peripherals_rm75.yaml index 30f39c3..d835fc7 100644 --- a/xr_rm_bringup/config/peripherals_rm75.yaml +++ b/xr_rm_bringup/config/peripherals_rm75.yaml @@ -27,3 +27,4 @@ arms: scissorgripper: 0 right: scissorgripper: 1 + set_initial_tool_state: true diff --git a/xr_rm_teleop/test/test_initial_joint_pose.py b/xr_rm_teleop/test/test_initial_joint_pose.py index 95ec063..5af5649 100644 --- a/xr_rm_teleop/test/test_initial_joint_pose.py +++ b/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"), [ diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py index ae5d924..fa2614d 100644 --- a/xr_rm_teleop/test/test_joint_control.py +++ b/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) diff --git a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py b/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py index 1e65b69..2f25e6c 100644 --- a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py +++ b/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 控制。 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 1531331..818dde6 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 @@ -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()