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

• 步骤1：先写失败测试

在 test_initial_joint_pose.py 增加：

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 的最终断言改为：

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 帧保留最后已知角度，但立即禁止这些角度继续参与运动。

• 步骤2：运行测试并确认 RED

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 帧仍被标记为可运动。

• 步骤3：实现最小同步查询

给 MockRealManAdapter 增加：

def read_joint_state(self) -> JointStateSnapshot:
    return self.get_latest_joint_state()

给 RealManAdapter 增加：

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 回调中重复的角度转换替换为：

positions = self._joint_positions_from_degrees(
    data.joint_status.joint_position,
    "RM75 UDP feedback",
)

在 UDP 回调的异常分支中，保留最后角度但标记为不可运动：

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,
        )

• 步骤4：运行适配器测试并确认 GREEN

source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v

预期：该文件全部测试通过。

• 步骤5：检查本任务差异

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

• 步骤1：先写启动同步失败测试

增加：

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]

• 步骤2：运行测试并确认 RED

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() 尚不存在而失败。

• 步骤3：增加启动同步

增加：

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

在现有适配器连接之后、外设初始化之前调用：

self._adapter = self._make_adapter()
self._adapter.connect()
self._initialize_joint_state()
self._setup_tool_control()

同步查询结果不得写入 RealManAdapter._latest_joint_state；该缓存继续只代表 UDP 反馈。

• 步骤4：运行启动与适配器测试

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

预期：所选控制测试和全部适配器测试通过。

• 步骤5：检查本任务差异

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

• 步骤1：先写超时行为测试

在测试文件增加最小构造器：

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

增加：

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 中补齐：

teleop._control_fault_latched = False
teleop._feedback_resync_attempted = False

• 步骤2：运行测试并确认 RED

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

预期：测试因超时处理和锁存状态尚不存在而失败。

• 步骤3：增加参数与最小状态

参数默认值：

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)

读取并初始化：

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() 中增加：

if self._feedback_resync_timeout_sec <= self._command_timeout_sec:
    raise ValueError(
        "feedback_resync_timeout_sec must be greater than command_timeout_sec"
    )

• 步骤4：增加保持、重新同步和锁存逻辑

增加：

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)

• 步骤5：在 QP 之前处理反馈状态

在 _control_tick() 开头用以下逻辑替换现有 _fresh_joint_state() 分支：

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) 后，用以下逻辑替换现有首次反馈日志：

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()：

@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逻辑、工作空间/圆柱限位、姿态限速和关节限速保持原样。

• 步骤6：运行超时及反馈安全测试

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 和同步查询，机械臂未就绪仍立即停止。

• 步骤7：检查本任务差异

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

• 步骤1：先写 CANFD 恢复测试

用以下测试替换旧的 test_joint_send_failure_requests_slow_stop_and_resets_control：

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

• 步骤2：运行测试并确认 RED

source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
  -k 'canfd_error' -v

预期：测试失败；当前发送错误只会 slow-stop，不会查询实际关节角或锁存查询失败。

• 步骤3：增加统一 CANFD 恢复路径

增加：

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() 的异常分支替换为：

except Exception as exc:
    self._recover_from_canfd_error(exc)
    return False

让短暂超时重发也走相同错误恢复：

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)

• 步骤4：运行全部关节控制测试

source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v

预期：全部关节控制测试通过。

• 步骤5：检查日志与差异

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

• 步骤1：只修改请求中的控制参数

三份配置的每个机械臂条目统一为：

control_rate_hz: 90.0
command_timeout_sec: 0.12
feedback_resync_timeout_sec: 0.5

保留低跟随：

follow: false

不得修改：

• 工作空间与圆柱限位。

• TCP线速度、角速度及关节速度/加速度限制。

• configure_safety_limits: true。

• move_to_initial_pose_on_connect: false。

• 机械臂IP、端口、初始位姿和末端工具配置。

• 双臂节点名 left_arm_teleop、right_arm_teleop。

• 步骤2：机械检查三份配置

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。

• 步骤3：运行相关测试

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退出且无失败。

• 步骤4：构建ROS2工作空间

source /opt/ros/humble/setup.bash
colcon build --symlink-install

预期：xr_rm_interfaces、xr_rm_input、xr_rm_teleop、xr_rm_bringup 构建成功。

• 步骤5：通过统一入口进行mock启动验证

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。

• 步骤6：最终范围与安全审计

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逻辑仍保留。
• 验证期间未连接真机、移动机械臂或操作夹爪。