# RM75 关节反馈与故障恢复实施计划 > **执行要求：** 使用 `superpowers:executing-plans` 按任务逐项实施。只有用户明确授权 subagent 后，才允许使用 `superpowers:subagent-driven-development`、独立 worktree 或本地分支。所有步骤使用复选框跟踪。 **目标：** 启动时用 `rm_get_joint_degree()` 初始化 RM75 QP；运行时以 UDP `joint_position` 作为实际反馈；短暂丢包时保持最后安全目标；持续丢包或 CANFD 错误时安全同步、停止或等待人工重新使能。 **实现方式：** 继续复用现有唯一 `RealManAdapter` 连接，只增加一个同步读取关节角的方法。恢复决策仍放在 `SingleArmVelocityTeleop`，用少量布尔状态复用现有 slow-stop、Grip 重新使能和关节限速逻辑，不新增状态机类、线程、连接或依赖。 **技术栈：** Python 3.10、ROS2 Humble `rclpy`、睿尔曼 Python API2、Placo、pytest、ament/colcon。 **设计文档：** `docs/superpowers/specs/2026-07-29-rm75-feedback-recovery-design.md` **厂商接口依据：** - `rm_get_joint_degree() -> tuple[int, list[float]]`： - `rm_movej_canfd(..., follow=False, ...)` 为低跟随： ## 仓库约束 - 所有构建、测试和启动命令均在工作空间根目录 `/home/robot/WS_xr` 执行。 - 所有 Git 命令均在仓库根目录 `/home/robot/WS_xr/src` 执行。 - 每次 ROS2 构建、测试或启动前先执行 `source /opt/ros/humble/setup.bash`。 - 不自动提交、推送、创建分支或 worktree；只有用户明确要求后才执行。 - 验证只通过 `xr_rm_bringup/launch/arm_debug.launch.py use_mock:=true`。 - 不连接真机，不发送真实 CANFD，不移动机械臂，不操作夹爪或末端外设。 - 不修改依赖、锁文件、CI、格式化配置、公开入口和无关代码。 ## 文件范围 - 修改 `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py` - 增加同步关节查询。 - 复用 UDP 与同步查询的角度校验。 - 修改 `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` - 启动同步、UDP保持/恢复、CANFD恢复和故障锁存。 - 修改 `src/xr_rm_teleop/test/test_initial_joint_pose.py` - 适配器同步查询测试。 - 修改 `src/xr_rm_teleop/test/test_joint_control.py` - 启动、超时、恢复和锁存测试。 - 同步修改： - `src/xr_rm_bringup/config/dual_arm_rm75.yaml` - `src/xr_rm_bringup/config/left_arm_rm75.yaml` - `src/xr_rm_bringup/config/right_arm_rm75.yaml` --- ## 任务一：给现有适配器增加同步关节查询 **修改文件：** - `src/xr_rm_teleop/test/test_initial_joint_pose.py` - `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py` - [x] **步骤1：先写失败测试** 在 `test_initial_joint_pose.py` 增加： ```python 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]] ) ``` 把现有 `test_invalid_udp_feedback_does_not_replace_snapshot` 的最终断言改为： ```python 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 ``` 该断言要求无效 UDP 帧保留最后已知角度，但立即禁止这些角度继续参与运动。 - [x] **步骤2：运行测试并确认 RED** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \ -k 'joint_degree_query or mock_joint_query or invalid_udp_feedback' -v ``` 预期：测试失败；原因是适配器还没有 `read_joint_state()`，且无效 UDP 帧仍被标记为可运动。 - [x] **步骤3：实现最小同步查询** 给 `MockRealManAdapter` 增加： ```python def read_joint_state(self) -> JointStateSnapshot: return self.get_latest_joint_state() ``` 给 `RealManAdapter` 增加： ```python def read_joint_state(self) -> JointStateSnapshot: self._require_arm() started_at = time.monotonic() result = self._arm.rm_get_joint_degree() finished_at = time.monotonic() if not isinstance(result, tuple) or len(result) != 2: raise RuntimeError( f"rm_get_joint_degree returned invalid result: {result!r}" ) self._check_return(result, "rm_get_joint_degree") return JointStateSnapshot( self._joint_positions_from_degrees( result[1], "rm_get_joint_degree", ), finished_at, (finished_at - started_at) * 1000.0, ) @staticmethod def _joint_positions_from_degrees( values: Any, source: str, ) -> list[float]: try: degrees = list(values) except TypeError as exc: raise ValueError(f"{source} must contain 7 numeric joints") from exc if len(degrees) != 7 or not all( isinstance(value, Number) for value in degrees ): raise ValueError(f"{source} must contain 7 numeric joints") positions = [math.radians(float(value)) for value in degrees] if not all(math.isfinite(value) for value in positions): raise ValueError(f"{source} contains NaN/Inf") return positions ``` 把 UDP 回调中重复的角度转换替换为： ```python positions = self._joint_positions_from_degrees( data.joint_status.joint_position, "RM75 UDP feedback", ) ``` 在 UDP 回调的异常分支中，保留最后角度但标记为不可运动： ```python with self._joint_state_lock: if self._latest_joint_state is not None: current = self._latest_joint_state self._latest_joint_state = JointStateSnapshot( list(current.positions), current.received_at, current.read_duration_ms, current.update_interval_ms, False, ) ``` - [x] **步骤4：运行适配器测试并确认 GREEN** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v ``` 预期：该文件全部测试通过。 - [x] **步骤5：检查本任务差异** ```bash git diff --check git diff -- \ xr_rm_teleop/xr_rm_teleop/realman_adapter.py \ xr_rm_teleop/test/test_initial_joint_pose.py ``` 预期：只有同步查询、共用角度校验、无效反馈安全标记及对应测试。 --- ## 任务二：用启动查询结果初始化 QP 和关节命令历史 **修改文件：** - `src/xr_rm_teleop/test/test_joint_control.py` - `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` - [x] **步骤1：先写启动同步失败测试** 增加： ```python def test_startup_joint_query_initializes_qp_and_command_history() -> None: positions = [0.1] * 7 pose = np.eye(4) teleop = object.__new__(SingleArmVelocityTeleop) teleop._arm_name = "right_rm75" teleop._adapter = SimpleNamespace( read_joint_state=lambda: JointStateSnapshot( positions, time.monotonic(), ) ) teleop._ik_solver = SimpleNamespace( update_joint_state=lambda joints: pose ) teleop.get_logger = lambda: FakeLogger() teleop._initialize_joint_state() assert teleop._latest_joint_positions == positions assert teleop._last_valid_joint_target == positions assert teleop._last_joint_command_target == positions assert teleop._last_joint_command_velocity == [0.0] * 7 assert teleop._last_current_pose is pose def test_startup_joint_query_failure_closes_adapter() -> None: class FailingAdapter: def __init__(self): self.close_calls = 0 def read_joint_state(self): raise RuntimeError("rm_get_joint_degree failed with code 7") def close(self): self.close_calls += 1 errors = [] teleop = object.__new__(SingleArmVelocityTeleop) teleop._arm_name = "left_rm75" teleop._adapter = FailingAdapter() teleop.get_logger = lambda: SimpleNamespace( error=lambda message: errors.append(message) ) with pytest.raises(RuntimeError, match="code 7"): teleop._initialize_joint_state() assert teleop._adapter.close_calls == 1 assert "left_rm75" in errors[0] assert "启动关节同步失败" in errors[0] ``` - [x] **步骤2：运行测试并确认 RED** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \ -k 'startup_joint_query' -v ``` 预期：测试因 `_initialize_joint_state()` 尚不存在而失败。 - [x] **步骤3：增加启动同步** 增加： ```python def _initialize_joint_state(self) -> None: try: self._reset_joint_state(self._adapter.read_joint_state()) except Exception as exc: self.get_logger().error( f"{self._arm_name} 启动关节同步失败：{exc}" ) self._adapter.close() raise def _reset_joint_state( self, snapshot: JointStateSnapshot, ) -> np.ndarray: current_pose = self._ik_solver.update_joint_state(snapshot.positions) positions = list(snapshot.positions) self._latest_joint_positions = positions self._last_current_pose = current_pose self._last_valid_joint_target = list(positions) self._last_joint_command_target = list(positions) self._last_joint_command_velocity = [0.0] * 7 return current_pose ``` 在现有适配器连接之后、外设初始化之前调用： ```python self._adapter = self._make_adapter() self._adapter.connect() self._initialize_joint_state() self._setup_tool_control() ``` 同步查询结果不得写入 `RealManAdapter._latest_joint_state`；该缓存继续只代表 UDP 反馈。 - [x] **步骤4：运行启动与适配器测试** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \ -k 'startup_joint_query or first_feedback' -v python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v ``` 预期：所选控制测试和全部适配器测试通过。 - [x] **步骤5：检查本任务差异** ```bash git diff --check git diff -- \ xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \ xr_rm_teleop/test/test_joint_control.py ``` 预期：启动阶段只增加一次同步读取，并初始化现有 QP/关节命令字段。 --- ## 任务三：UDP 短暂超时保持，持续超时重新同步 **修改文件：** - `src/xr_rm_teleop/test/test_joint_control.py` - `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` - [x] **步骤1：先写超时行为测试** 在测试文件增加最小构造器： ```python def _timeout_teleop(adapter) -> SingleArmVelocityTeleop: teleop = object.__new__(SingleArmVelocityTeleop) teleop._adapter = adapter teleop._arm_name = "right_rm75" teleop._follow = False teleop._active = True teleop._joint_feedback_ready = True teleop._grip_rearm_required = False teleop._feedback_resync_attempted = False teleop._control_fault_latched = False teleop._last_joint_command_target = [0.1] * 7 teleop._last_joint_command_velocity = [0.0] * 7 teleop._latest_joint_positions = [0.1] * 7 teleop._last_valid_joint_target = [0.1] * 7 teleop._last_current_pose = np.eye(4) teleop._controller_start = None teleop._controller_orientation_start = None teleop._robot_start_transform = None teleop._filtered_target = None teleop._filtered_orientation_target = None teleop._last_sent_target = None teleop._last_sent_orientation = None teleop._last_command_time = None teleop._ik_solver = SimpleNamespace( update_joint_state=lambda joints: np.eye(4) ) teleop._stop_sent = False teleop._feedback_resync_timeout_sec = 0.5 teleop._publish_stop_debug = lambda: None teleop.get_logger = lambda: FakeLogger() return teleop ``` 增加： ```python def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None: assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None) assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready( JointStateSnapshot( [0.0] * 7, time.monotonic(), motion_ready=False, ) ) def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None: sends = [] adapter = SimpleNamespace( send_joint_target=lambda joints, follow: sends.append( (list(joints), follow) ), read_joint_state=lambda: pytest.fail("query must not run"), stop=lambda: pytest.fail("stop must not run"), ) teleop = _timeout_teleop(adapter) teleop._handle_stale_joint_feedback(0.2) assert sends == [([0.1] * 7, False)] assert teleop._last_joint_command_target == [0.1] * 7 assert teleop._grip_rearm_required def test_short_udp_timeout_without_active_target_stays_stopped() -> None: stop_calls = [] adapter = SimpleNamespace( send_joint_target=lambda joints, follow: pytest.fail( "inactive control must not start CANFD output" ), read_joint_state=lambda: pytest.fail("query must not run"), stop=lambda: stop_calls.append(True), ) teleop = _timeout_teleop(adapter) teleop._active = False teleop._handle_stale_joint_feedback(0.2) assert len(stop_calls) == 1 def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None: sends = [] query_calls = [] adapter = SimpleNamespace( send_joint_target=lambda joints, follow: sends.append(list(joints)), read_joint_state=lambda: ( query_calls.append(True) or JointStateSnapshot([0.2] * 7, time.monotonic()) ), stop=lambda: None, ) teleop = _timeout_teleop(adapter) teleop._handle_stale_joint_feedback(0.5) teleop._handle_stale_joint_feedback(0.6) assert len(query_calls) == 1 assert sends == [[0.2] * 7, [0.2] * 7] assert teleop._last_valid_joint_target == [0.2] * 7 assert teleop._last_joint_command_velocity == [0.0] * 7 def test_persistent_udp_timeout_query_failure_latches_control() -> None: stop_calls = [] adapter = SimpleNamespace( send_joint_target=lambda joints, follow: pytest.fail( "CANFD must stop after query failure" ), read_joint_state=lambda: (_ for _ in ()).throw( RuntimeError("rm_get_joint_degree failed with code 7") ), stop=lambda: stop_calls.append(True), ) teleop = _timeout_teleop(adapter) teleop._handle_stale_joint_feedback(0.5) teleop._handle_stale_joint_feedback(0.6) assert teleop._control_fault_latched assert len(stop_calls) == 1 ``` 删除旧的 `_fresh_joint_state()` 直接测试，并用 `test_short_udp_timeout_without_active_target_stays_stopped` 替换旧的 `test_stale_feedback_stops_before_active_control`。 在 `test_feedback_fault_blocks_grip_until_release` 中补齐： ```python teleop._control_fault_latched = False teleop._feedback_resync_attempted = False ``` - [x] **步骤2：运行测试并确认 RED** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \ -k 'udp_timeout or joint_snapshot' -v ``` 预期：测试因超时处理和锁存状态尚不存在而失败。 - [x] **步骤3：增加参数与最小状态** 参数默认值： ```python self.declare_parameter("control_rate_hz", 90.0) self.declare_parameter("command_timeout_sec", 0.12) self.declare_parameter("feedback_resync_timeout_sec", 0.5) ``` 读取并初始化： ```python self._feedback_resync_timeout_sec = float( self.get_parameter("feedback_resync_timeout_sec").value ) self._feedback_resync_attempted = False self._control_fault_latched = False ``` 在 `_validate_parameters()` 中增加： ```python if self._feedback_resync_timeout_sec <= self._command_timeout_sec: raise ValueError( "feedback_resync_timeout_sec must be greater than command_timeout_sec" ) ``` - [x] **步骤4：增加保持、重新同步和锁存逻辑** 增加： ```python def _handle_stale_joint_feedback(self, age: float) -> None: if self._control_fault_latched: return self._grip_rearm_required = True if self._joint_feedback_ready: self.get_logger().warn( f"{self._arm_name} UDP关节反馈超时，保持最后安全目标。" ) self._joint_feedback_ready = False if ( age >= self._feedback_resync_timeout_sec and not self._feedback_resync_attempted ): self._feedback_resync_attempted = True self.get_logger().warn( f"{self._arm_name} UDP关节反馈持续超时，" "尝试rm_get_joint_degree重新同步。" ) try: self._reset_joint_state(self._adapter.read_joint_state()) except Exception as exc: self._latch_control_fault( f"UDP关节反馈持续超时且重新同步失败：{exc}" ) return self.get_logger().info( f"{self._arm_name} 已通过rm_get_joint_degree重新同步，" "继续保持并等待UDP恢复。" ) if self._active and self._last_joint_command_target is not None: self._repeat_last_joint_target() else: self._safe_stop(reset_active=True) def _repeat_last_joint_target(self) -> None: target = self._last_joint_command_target if target is None: return self._adapter.send_joint_target(list(target), self._follow) self._stop_sent = False def _latch_control_fault(self, message: str) -> None: if self._control_fault_latched: return self._control_fault_latched = True self._grip_rearm_required = True self.get_logger().error( f"{self._arm_name} 控制故障已锁存：{message}" ) self._safe_stop(reset_active=True) ``` - [x] **步骤5：在 QP 之前处理反馈状态** 在 `_control_tick()` 开头用以下逻辑替换现有 `_fresh_joint_state()` 分支： ```python if self._control_fault_latched: return snapshot = self._adapter.get_latest_joint_state() if not self._joint_snapshot_is_motion_ready(snapshot): self._grip_rearm_required = True if self._joint_feedback_ready: self.get_logger().warn( f"{self._arm_name} 关节反馈无效或机械臂未就绪，机械臂停止。" ) self._joint_feedback_ready = False self._safe_stop(reset_active=True) return feedback_age = time.monotonic() - snapshot.received_at if feedback_age < 0.0: self._grip_rearm_required = True self._joint_feedback_ready = False self._safe_stop(reset_active=True) return if feedback_age > self._command_timeout_sec: self._handle_stale_joint_feedback(feedback_age) return self._feedback_resync_attempted = False ``` 在成功执行 `_sync_joint_feedback(snapshot)` 后，用以下逻辑替换现有首次反馈日志： ```python if not self._joint_feedback_ready: if self._grip_rearm_required: message = ( f"{self._arm_name} UDP关节反馈已恢复，" "等待Grip松开后重新使能。" ) else: message = ( f"{self._arm_name} 已收到首帧有效关节反馈，QP可以启用。" ) self.get_logger().info(message) self._joint_feedback_ready = True ``` 用以下静态校验替换 `_fresh_joint_state()`： ```python @staticmethod def _joint_snapshot_is_motion_ready( snapshot: JointStateSnapshot | None, ) -> bool: return ( snapshot is not None and len(snapshot.positions) == 7 and all(math.isfinite(value) for value in snapshot.positions) and snapshot.motion_ready ) ``` XR手柄消息超时、Grip逻辑、工作空间/圆柱限位、姿态限速和关节限速保持原样。 - [x] **步骤6：运行超时及反馈安全测试** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \ -k 'udp_timeout or feedback_fault or joint_snapshot' -v ``` 预期：所选测试通过；短暂超时不调用 QP 和同步查询，机械臂未就绪仍立即停止。 - [x] **步骤7：检查本任务差异** ```bash git diff --check git diff --stat ``` 预期：没有新增状态机类、线程、依赖、连接或常态轮询。 --- ## 任务四：CANFD 错误后停止、查询并等待人工恢复 **修改文件：** - `src/xr_rm_teleop/test/test_joint_control.py` - `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` - [x] **步骤1：先写 CANFD 恢复测试** 用以下测试替换旧的 `test_joint_send_failure_requests_slow_stop_and_resets_control`： ```python def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None: class RecoveringAdapter: def __init__(self): self.stop_calls = 0 self.read_calls = 0 def send_joint_target(self, joints, follow): raise RuntimeError("rm_movej_canfd failed with code 9") def stop(self): self.stop_calls += 1 def read_joint_state(self): self.read_calls += 1 return JointStateSnapshot([0.2] * 7, time.monotonic()) teleop = _timeout_teleop(RecoveringAdapter()) teleop._joint_command_max_speed = math.radians(180.0) teleop._joint_command_max_acceleration = math.radians(300.0) teleop._dt = 1.0 / 90.0 sent = teleop._send_joint_target([0.3] * 7) assert not sent assert teleop._adapter.stop_calls == 1 assert teleop._adapter.read_calls == 1 assert not teleop._control_fault_latched assert teleop._grip_rearm_required assert teleop._last_joint_command_target == [0.2] * 7 def test_canfd_error_latches_when_joint_query_also_fails() -> None: class FailingAdapter: def __init__(self): self.stop_calls = 0 def send_joint_target(self, joints, follow): raise RuntimeError("rm_movej_canfd failed with code 9") def stop(self): self.stop_calls += 1 def read_joint_state(self): raise RuntimeError("rm_get_joint_degree failed with code 7") teleop = _timeout_teleop(FailingAdapter()) teleop._joint_command_max_speed = math.radians(180.0) teleop._joint_command_max_acceleration = math.radians(300.0) teleop._dt = 1.0 / 90.0 assert not teleop._send_joint_target([0.3] * 7) assert teleop._control_fault_latched assert teleop._adapter.stop_calls == 1 ``` - [x] **步骤2：运行测试并确认 RED** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \ -k 'canfd_error' -v ``` 预期：测试失败；当前发送错误只会 slow-stop，不会查询实际关节角或锁存查询失败。 - [x] **步骤3：增加统一 CANFD 恢复路径** 增加： ```python def _recover_from_canfd_error(self, send_error: Exception) -> None: self.get_logger().error( f"{self._arm_name} rm_movej_canfd发送失败：{send_error}" ) self._grip_rearm_required = True self._send_stop_once() self._safe_stop(reset_active=True) try: snapshot = self._adapter.read_joint_state() self._reset_joint_state(snapshot) except Exception as query_error: self._latch_control_fault( "CANFD错误后关节同步失败：" f"send={send_error}; query={query_error}" ) return self.get_logger().info( f"{self._arm_name} CANFD错误后已同步实际关节角，" "等待UDP恢复及Grip重新使能。" ) ``` 把 `_send_joint_target()` 的异常分支替换为： ```python except Exception as exc: self._recover_from_canfd_error(exc) return False ``` 让短暂超时重发也走相同错误恢复： ```python def _repeat_last_joint_target(self) -> None: target = self._last_joint_command_target if target is None: return try: self._adapter.send_joint_target(list(target), self._follow) self._stop_sent = False except Exception as exc: self._recover_from_canfd_error(exc) ``` - [x] **步骤4：运行全部关节控制测试** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v ``` 预期：全部关节控制测试通过。 - [x] **步骤5：检查日志与差异** ```bash rg -n "rm_movej_canfd发送失败|控制故障已锁存|rm_get_joint_degree" \ src/xr_rm_teleop/xr_rm_teleop git diff --check ``` 预期： - CANFD错误和查询错误都包含机械臂名称及失败阶段。 - 锁存分支不会每周期重复打印错误。 - 正常QP输出和超时保持使用同一个CANFD恢复入口。 --- ## 任务五：同步90 Hz配置并完成mock验证 **修改文件：** - `src/xr_rm_bringup/config/dual_arm_rm75.yaml` - `src/xr_rm_bringup/config/left_arm_rm75.yaml` - `src/xr_rm_bringup/config/right_arm_rm75.yaml` - [x] **步骤1：只修改请求中的控制参数** 三份配置的每个机械臂条目统一为： ```yaml control_rate_hz: 90.0 command_timeout_sec: 0.12 feedback_resync_timeout_sec: 0.5 ``` 保留低跟随： ```yaml follow: false ``` 不得修改： - 工作空间与圆柱限位。 - TCP线速度、角速度及关节速度/加速度限制。 - `configure_safety_limits: true`。 - `move_to_initial_pose_on_connect: false`。 - 机械臂IP、端口、初始位姿和末端工具配置。 - 双臂节点名 `left_arm_teleop`、`right_arm_teleop`。 - [x] **步骤2：机械检查三份配置** ```bash rg -n "control_rate_hz|command_timeout_sec|feedback_resync_timeout_sec|follow:" \ src/xr_rm_bringup/config/dual_arm_rm75.yaml \ src/xr_rm_bringup/config/left_arm_rm75.yaml \ src/xr_rm_bringup/config/right_arm_rm75.yaml ``` 预期：共四个机械臂配置条目，每个条目均为90.0、0.12、0.5和`follow: false`；三份文件不再出现125.0。 - [x] **步骤3：运行相关测试** ```bash source /opt/ros/humble/setup.bash python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -v ``` 预期：三个命令均以0退出且无失败。 - [x] **步骤4：构建ROS2工作空间** ```bash source /opt/ros/humble/setup.bash colcon build --symlink-install ``` 预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`、`xr_rm_bringup` 构建成功。 - [x] **步骤5：通过统一入口进行mock启动验证** ```bash source /opt/ros/humble/setup.bash source install/setup.bash if timeout --signal=INT 10s ros2 launch xr_rm_bringup arm_debug.launch.py \ arm:=right use_mock:=true udp_port:=15123 then true else launch_status=$? test "$launch_status" -eq 124 fi ``` 预期： - `udp_controller_receiver` 与 `single_arm_velocity_teleop` 正常启动。 - 遥操作节点报告90 Hz、低跟随，并完成mock关节状态初始化。 - 不导入厂商SDK，不建立RealMan连接，不发送CANFD，不移动机械臂，不操作夹爪。 - 10秒后由`timeout`结束；仅该超时允许退出码124。 - [x] **步骤6：最终范围与安全审计** ```bash git diff --check git status --short git diff --stat git diff -- \ xr_rm_teleop/xr_rm_teleop/realman_adapter.py \ xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \ xr_rm_teleop/test/test_initial_joint_pose.py \ xr_rm_teleop/test/test_joint_control.py \ xr_rm_bringup/config/dual_arm_rm75.yaml \ xr_rm_bringup/config/left_arm_rm75.yaml \ xr_rm_bringup/config/right_arm_rm75.yaml ``` 逐项确认： - 没有无关文件或格式化改动。 - 没有提交、推送、分支、worktree、锁文件、CI、格式化规则或依赖变化。 - 没有新增线程、ROS包、launch入口、并发RealMan连接或常态SDK轮询。 - mock模式不导入、不依赖厂商SDK。 - `configure_safety_limits` 默认仍为开启。 - `move_to_initial_pose_on_connect` 默认仍为关闭。 - `left_arm_teleop`、`right_arm_teleop` 节点名不变。 - 工作空间/圆柱限位、TCP与关节限速、XR命令超时和slow-stop逻辑仍保留。 - 验证期间未连接真机、移动机械臂或操作夹爪。