6.3 KiB
RM75 关节反馈与故障恢复设计
目标
将当前 RM75 QP 遥操作链路调整为：
- 启动时使用
rm_get_joint_degree()获取实际关节角并初始化 QP。 - 运行时只把 RealMan UDP
joint_position作为连续实际关节反馈。 - UDP 短暂超时时保持最后一次已限速的安全关节目标，不生成新运动。
- UDP 持续超时或 CANFD 发送错误时，按明确的同步、停止和人工恢复流程处理。
rm_movej_canfd()保持低跟随，控制频率使用xr_rm_teleop分支的 90 Hz。
不改变现有工作空间/圆柱限位、TCP与关节速度和加速度限制、XR命令超时、安全停止、外设控制及双臂节点名。
控制数据源
启动初始化与运行反馈使用不同的数据源：
RealManAdapter建立现有唯一厂商连接。- 节点同步调用一次
rm_get_joint_degree()。 - 查询成功后把7个角度转换为弧度，用于初始化 Placo QP、最后安全目标和关节限速历史。
- 查询失败时关闭适配器、打印错误并使节点启动失败，不发送 CANFD。
- 正常运行后，QP 的连续实际状态只来自已校验且运动状态正常的 UDP
joint_position。
同步查询只用于启动、持续反馈超时恢复和 CANFD 错误恢复，不新增连接，不进行常态轮询。
状态与转换
正常运行
控制定时器以 90 Hz 执行。每个周期读取最新 UDP 关节快照，同步 QP，执行现有目标生成、安全限位、单步 QP、关节速度/加速度限制，然后调用：
rm_movej_canfd(target_degrees, follow=false, ...)
只有已经通过关节限速并成功发送的目标才能成为“最后安全目标”。
UDP 短暂超时
UDP 快照年龄超过现有 command_timeout_sec=0.12 秒、但未达到 feedback_resync_timeout_sec=0.5 秒时：
- 不使用过期反馈同步 QP。
- 不运行目标生成和 QP。
- 不更新任何目标、滤波器或限速历史。
- 若超时前正在遥操作且已有成功发送的安全目标，以 90 Hz 原样重发该目标。
- 若超时前未在遥操作或没有成功发送的目标，保持停止，不开始 CANFD 输出。
- 首次进入时打印节流后的警告。
机械臂报警、关节掉使能、关节错误或非有限关节值不是普通超时，仍立即执行安全停止。
UDP 持续超时
UDP 快照年龄达到 0.5 秒时，每次中断只同步调用一次 rm_get_joint_degree()：
- 查询成功：用实际角度重置 QP、最后安全目标和关节限速历史；不生成新运动，继续保持并等待 UDP 恢复。
- 查询失败：调用 slow-stop，停止 CANFD，进入锁存故障并打印错误。
同一次中断不会反复查询。收到新的有效 UDP 反馈后，查询标志才复位。
UDP 恢复
UDP 恢复后先持续同步实际关节状态，但不能直接恢复运动：
- 当前 Grip 必须松开。
- 节点清除重新使能要求。
- 操作者再次按下 Grip，节点以新的手柄和机械臂实际位姿建立相对控制起点。
CANFD 错误
rm_movej_canfd() 返回错误或抛出异常时：
- 立即停止后续 CANFD 发送。
- 调用 slow-stop。
- 打印包含机械臂名称、命令名称和原始错误的终端错误日志。
- 调用
rm_get_joint_degree()查询实际关节角。 - 查询成功时重置 QP和关节命令历史，但不再发送保持命令；等待有效 UDP 和 Grip 松开后重新按下。
- 查询失败时进入锁存故障并打印查询错误。
锁存故障
锁存故障只作用于发生错误的机械臂节点：
- 控制定时器不再查询、运行 QP或发送 CANFD。
- slow-stop 只发送一次。
- 后续 UDP 恢复或 Grip 操作不能自动解锁。
- 终端保留明确错误信息，但不在每个周期重复刷屏。
- 操作者检查后必须重启对应遥操作节点才能恢复。
代码边界
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
- 给真实与 mock 适配器增加同步关节角查询能力。
- 复用现有厂商连接。
- 校验返回码、数量和有限值，统一返回弧度。
- 保留 UDP 回调作为运行时快照来源。
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
- 启动时查询并初始化 QP。
- 增加
feedback_resync_timeout_sec参数，默认 0.5 秒。 - 校验
feedback_resync_timeout_sec > command_timeout_sec > 0。 - 在现有控制周期内加入保持、一次性重新同步、等待 Grip 重使能和锁存判断。
- 复用现有
_safe_stop()、Grip 重使能和关节限速逻辑，不新增状态机类。
配置
以下配置的 control_rate_hz 从 125 Hz 改为 90 Hz，并增加相同的 0.5 秒持续超时参数：
xr_rm_bringup/config/dual_arm_rm75.yamlxr_rm_bringup/config/left_arm_rm75.yamlxr_rm_bringup/config/right_arm_rm75.yaml
三份配置继续使用 follow: false，双臂节点名保持 left_arm_teleop 和 right_arm_teleop。
错误日志
以下转换必须写入 ROS2 终端日志：
- 启动关节查询失败：
error。 - 首次进入 UDP 短暂超时：
warn。 - 持续超时查询开始及成功：
warn/info。 - 持续超时查询失败并锁存：
error。 - CANFD 发送失败：
error。 - CANFD 后关节查询失败并锁存：
error。 - UDP 恢复并等待 Grip 人工重使能：
info。
日志包含机械臂名称和失败阶段；周期性路径使用状态转换或节流避免刷屏。
测试与验证
使用现有 mock 和单元测试完成，不连接真机：
- 适配器正确解析
rm_get_joint_degree()成功结果，并拒绝错误码、错误数量和 NaN/Inf。 - 启动查询结果初始化 QP 和安全目标；查询失败时节点不能进入控制。
- 0.12～0.5 秒反馈超时期间不调用 QP，只重发同一安全目标。
- 0.5 秒持续超时只查询一次；成功后等待 UDP 与 Grip，失败后锁存。
- CANFD 错误后停止发送并查询；查询成功要求 Grip 重使能，查询失败锁存。
- 机械臂报警或掉使能仍立即停止，不能进入保持路径。
- 三份配置均使用 90 Hz、0.5 秒持续超时和低跟随。
在工作空间根目录 /home/robot/WS_xr 执行：
source /opt/ros/humble/setup.bash
pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
pytest src/xr_rm_teleop/test/test_joint_control.py
pytest src/xr_rm_teleop/test/test_orientation_control.py
colcon build --symlink-install