acRealman_xr/docs/superpowers/plans/2026-07-29-rm75-feedback-recovery.md

# 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]]`：<https://develop.realman-robotics.com/robot/apipython/classes/armState/>
- `rm_movej_canfd(..., follow=False, ...)` 为低跟随：<https://develop.realman-robotics.com/robot/apipython/classes/movePlan/>

## 仓库约束

- 所有构建、测试和启动命令均在工作空间根目录 `/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逻辑仍保留。
- 验证期间未连接真机、移动机械臂或操作夹爪。