import math import sys from types import ModuleType, SimpleNamespace import pytest from xr_rm_teleop import 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 def test_initial_pose_uses_joint_move_only() -> None: class FakeArm: def __init__(self) -> None: self.calls = [] def rm_movej(self, *args): self.calls.append(args) return 0 joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51] adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, initial_joint_pose=joints, ) adapter._arm = FakeArm() adapter._move_to_initial_pose() assert adapter._arm.calls == [(joints, 20, 0, 0, 1)] def test_peripheral_config_exposes_selected_tool() -> None: config = PeripheralConfig( scissorgripper=1, tools_in_ee={ "first": [[0.0] * 7, [0.0] * 7], "second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7], }, ) assert config.tool_name == "second" assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0] @pytest.mark.parametrize( ("existing", "expected_operation"), [(False, "create"), (True, "update")], ) def test_tool_frame_is_created_or_updated(existing, expected_operation) -> None: class FakeArm: def __init__(self) -> None: self.calls = [] def rm_get_total_tool_frame(self): self.calls.append(("get",)) names = ["omnipic"] if existing else [] return {"return_code": 0, "tool_names": names} def rm_set_manual_tool_frame(self, *, frame): self.calls.append(("create", frame)) return 0 def rm_update_tool_frame(self, *, frame): self.calls.append(("update", frame)) return 0 def rm_change_tool_frame(self, tool_name): self.calls.append(("change", tool_name)) return 0 arm = FakeArm() frame = object() _configure_tool_frame(arm, frame, "omnipic") assert arm.calls == [ ("get",), (expected_operation, frame), ("change", "omnipic"), ] @pytest.mark.parametrize( ("existing", "failure", "operation"), [ (False, "query", "rm_get_total_tool_frame"), (False, "create", "rm_set_manual_tool_frame"), (True, "update", "rm_update_tool_frame"), (False, "change", "rm_change_tool_frame"), ], ) def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> None: class FakeArm: def rm_get_total_tool_frame(self): names = ["omnipic"] if existing else [] return { "return_code": 1 if failure == "query" else 0, "tool_names": names, } def rm_set_manual_tool_frame(self, *, frame): del frame return 1 if failure == "create" else 0 def rm_update_tool_frame(self, *, frame): del frame return 1 if failure == "update" else 0 def rm_change_tool_frame(self, tool_name): del tool_name return 1 if failure == "change" else 0 with pytest.raises(RuntimeError, match=operation): _configure_tool_frame(FakeArm(), object(), "omnipic") def _udp_state( *, robot_ip: str = "127.0.0.1", joints=None, error_code: int = 0, joint_enabled=None, joint_error_codes=None, arm_error_codes=None, arm_current_status: int = 0, ): if joints is None: joints = [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0] if joint_enabled is None: joint_enabled = [True] * 7 if joint_error_codes is None: joint_error_codes = [0] * 7 if arm_error_codes is None: arm_error_codes = [] return SimpleNamespace( errCode=error_code, arm_ip=robot_ip.encode(), joint_status=SimpleNamespace( joint_position=joints, joint_en_flag=joint_enabled, joint_err_code=joint_error_codes, ), err=SimpleNamespace( err_len=len(arm_error_codes), err=list(arm_error_codes), ), arm_current_status=arm_current_status, ) def _install_fake_sdk(monkeypatch, *, push_return=0, send_feedback=True): class FakeThreadMode: RM_TRIPLE_MODE_E = 2 class FakePushConfig: def __init__(self, *args): self.args = args class FakeArm: instance = None def __init__(self, mode): self.mode = mode self.callback = None self.config = None self.delete_calls = 0 FakeArm.instance = self def rm_create_robot_arm(self, robot_ip, robot_port): self.robot_ip = robot_ip self.robot_port = robot_port return SimpleNamespace(id=1) def rm_realtime_arm_state_call_back(self, callback): self.callback = callback def rm_set_realtime_push(self, config): self.config = config if push_return == 0 and send_feedback: self.callback(_udp_state()) return push_return def rm_delete_robot_arm(self): self.delete_calls += 1 return 0 sdk = ModuleType("Robotic_Arm.rm_robot_interface") sdk.RoboticArm = FakeArm sdk.rm_thread_mode_e = FakeThreadMode sdk.rm_realtime_push_config_t = FakePushConfig sdk.rm_realtime_arm_state_callback_ptr = lambda callback: callback 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) return SimpleNamespace(RoboticArm=FakeArm) def test_joint_degree_query_returns_validated_radians() -> None: class FakeArm: def rm_get_joint_degree(self): return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0] adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._arm = FakeArm() snapshot = adapter.read_joint_state() assert snapshot.positions == pytest.approx( [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]] ) assert snapshot.read_duration_ms is not None @pytest.mark.parametrize( "result", [ (7, [0.0] * 7), (0, [0.0] * 6), (0, [0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]), ], ) def test_joint_degree_query_rejects_sdk_errors_and_invalid_values(result) -> None: adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._arm = SimpleNamespace(rm_get_joint_degree=lambda: result) with pytest.raises((RuntimeError, ValueError)): adapter.read_joint_state() def test_mock_joint_query_uses_current_mock_positions() -> None: adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) snapshot = adapter.read_joint_state() assert snapshot.positions == pytest.approx( [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]] ) def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None: monotonic = iter([10.0, 10.005]) monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic)) adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._accept_realtime_feedback = True adapter._on_realtime_arm_state(_udp_state()) first = adapter.get_latest_joint_state() adapter._on_realtime_arm_state(_udp_state()) second = adapter.get_latest_joint_state() assert first is not None assert first.positions == pytest.approx( [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]] ) assert first.read_duration_ms is None assert first.update_interval_ms is None assert second is not None assert second.read_duration_ms is None assert second.update_interval_ms == pytest.approx(5.0) @pytest.mark.parametrize( "state", [ _udp_state(error_code=-3), _udp_state(robot_ip="192.168.192.18"), _udp_state(joints=[0.0] * 6), _udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]), ], ) def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None: adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._accept_realtime_feedback = True adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7)) before = adapter.get_latest_joint_state() adapter._on_realtime_arm_state(state) after = adapter.get_latest_joint_state() assert after is not None assert before is not None assert after.positions == before.positions assert after.received_at == before.received_at assert after.motion_ready is False def test_udp_joint_fault_marks_snapshot_unready() -> None: adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._accept_realtime_feedback = True adapter._on_realtime_arm_state( _udp_state( joint_enabled=[True, True, False, True, True, True, True], joint_error_codes=[0, 0, 17, 0, 0, 0, 0], arm_error_codes=[42], arm_current_status=9, ) ) snapshot = adapter.get_latest_joint_state() assert snapshot is not None assert snapshot.motion_ready is False def test_udp_stop_status_marks_snapshot_unready_without_joint_error() -> None: adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._accept_realtime_feedback = True adapter._on_realtime_arm_state(_udp_state(arm_current_status=9)) snapshot = adapter.get_latest_joint_state() assert snapshot is not None assert snapshot.motion_ready is False def test_udp_zero_arm_error_code_is_motion_ready() -> None: adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._accept_realtime_feedback = True adapter._on_realtime_arm_state(_udp_state(arm_error_codes=[0])) snapshot = adapter.get_latest_joint_state() assert snapshot is not None assert snapshot.motion_ready is True def test_udp_fault_and_recovery_are_logged_once_per_transition() -> None: class FakeLogger: def __init__(self) -> None: self.infos = [] self.warnings = [] def info(self, message): self.infos.append(message) def warn(self, message): self.warnings.append(message) logger = FakeLogger() adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, logger=logger, ) adapter._accept_realtime_feedback = True adapter._on_realtime_arm_state(_udp_state()) fault = _udp_state( joint_enabled=[False] * 7, joint_error_codes=[17, 0, 0, 0, 0, 0, 0], arm_error_codes=[42], arm_current_status=9, ) adapter._on_realtime_arm_state(fault) adapter._on_realtime_arm_state(fault) adapter._on_realtime_arm_state(_udp_state()) assert len(logger.warnings) == 1 assert "joint_errors=[17, 0, 0, 0, 0, 0, 0]" in logger.warnings[0] assert len([message for message in logger.infos if "恢复正常" in message]) == 1 @pytest.mark.parametrize( ("cycle_ms", "sdk_cycle"), [(5, 1), (10, 2)], ) def test_connect_converts_udp_feedback_cycle_to_sdk_units( monkeypatch, cycle_ms, sdk_cycle, ) -> None: fake_sdk = _install_fake_sdk(monkeypatch) adapter = RealManAdapter( "127.0.0.1", 8080, 0, "192.168.192.148", 8090, realtime_push_cycle_ms=cycle_ms, configure_safety_limits=False, ) adapter.connect() arm = fake_sdk.RoboticArm.instance assert arm is not None assert arm.config.args == ( sdk_cycle, True, 8090, 0, "192.168.192.148", ) assert arm.callback is adapter._realtime_callback assert adapter.get_latest_joint_state() is not None assert not hasattr(adapter, "_feedback_thread") def test_udp_configuration_failure_cleans_up_robot_handle(monkeypatch) -> None: fake_sdk = _install_fake_sdk(monkeypatch, push_return=1) adapter = RealManAdapter( "127.0.0.1", 8080, 0, "192.168.192.148", 8090, configure_safety_limits=False, ) with pytest.raises(RuntimeError, match="rm_set_realtime_push"): adapter.connect() assert fake_sdk.RoboticArm.instance.delete_calls == 1 assert adapter._arm is None def test_udp_first_frame_timeout_cleans_up_robot_handle(monkeypatch) -> None: fake_sdk = _install_fake_sdk(monkeypatch, send_feedback=False) adapter = RealManAdapter( "127.0.0.1", 8080, 0, "192.168.192.148", 8090, configure_safety_limits=False, ) adapter._feedback_ready = SimpleNamespace( clear=lambda: None, set=lambda: None, wait=lambda timeout: False, ) with pytest.raises(RuntimeError, match="within 2 seconds"): adapter.connect() assert fake_sdk.RoboticArm.instance.delete_calls == 1 assert adapter._arm is None def test_joint_target_uses_movej_canfd_in_degrees() -> None: class FakeArm: def __init__(self) -> None: self.calls = [] def rm_movej_canfd(self, *args): self.calls.append(args) return 0 adapter = RealManAdapter( "127.0.0.1", 8080, 0, "127.0.0.1", 8090, ) adapter._arm = FakeArm() target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]] adapter.send_joint_target(target, follow=False) assert len(adapter._arm.calls) == 1 degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0] assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0) def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None: adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) adapter.connect() snapshot = adapter.get_latest_joint_state() assert snapshot is not None assert snapshot.positions == pytest.approx( [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]] )