feat(hardware): 接入 SocketCAN 与智身模组协议,补温度诊断和指令看门狗
- hardware: 新增 can_bus.h / joint_module_protocol.h,实现使能/失能/运行帧与 12 字节反馈帧编解码;ReadHardware/WriteHardware 落地,控制字按协议语义映射 (kErrorReset 按重新使能处理,kGetStatus 无需请求) - hardware: 温度/驱动状态/电压经 /joint_diagnostics 发布(默认 10 Hz,可配置) - hardware: 指令看门狗,100 ms 无指令转阻尼,避免上游挂掉后重发最后一帧 - hardware: 限幅与归一化边界分离——[joint_limits] 是唯一实际限幅,加载期校验 其不超过 [motor.*] 的归一化边界;力矩按峰值扭矩夹紧,kp/kd 越界夹紧并告警 - hardware: require_can 开关,无 CAN 时可只发状态(冒烟测试用) - remote: 1002/4 关节按 M20 的 16 槽上报(swing→HipY、wheel→Wheel,其余占位 0), 1002/5 温度改由 /joint_diagnostics 提供 - deploy: 启动时序等订阅者出现再发(ROS1 无 latch,否则整段丢失) - msgs: 新增 JointDiagnostics - docs: 智身模组 CANFD 协议整理、待办清单
This commit is contained in:
@@ -0,0 +1,88 @@
|
||||
# TODO
|
||||
|
||||
本文件记录**当前会话中确认但本期不做**的事项。按优先级分组,每项尽量写清「为什么」和「验收方式」,
|
||||
避免下次重新推导。已完成的部分见各包 README。
|
||||
|
||||
## P0:安全链路(真机跑 RL 前必须处理)
|
||||
|
||||
- [ ] **异常数据接入 deploy 的保护链路**
|
||||
硬件已在 `/joint_diagnostics` 提供温度/状态字/电压(10 Hz),但 deploy 侧仍是常量:
|
||||
`robot_interface.h` 的 `GetMotorTemperture()` 返回 30 ℃、`GetDriverStatusWord()` 返回全 1,
|
||||
所以 `SafeController` 的过温检查和驱动报错检查恒不触发。
|
||||
做法:`InvictaInterface` 订阅 `/joint_diagnostics` 并覆盖这两个虚函数。
|
||||
注意:协议 Byte9 的 bit0 是「使能中」,而 `IsDriverStatusNormal()` 判断 `status_word != 1` 报错,
|
||||
失能但健康的电机回 `0x00` 会误判;建议在接口层归一化成「无故障位就填 1」。
|
||||
验收:人为把某个关节温度改成 101 ℃,deploy 进入 `safe_control_mode=2` 并切 `kJointDamping`。
|
||||
|
||||
- [ ] **诊断超时判断**
|
||||
诊断话题 10 Hz,`SafeController` 读缓存值。若 hardware 进程死掉,温度会停在最后一帧的「正常」。
|
||||
需要在接口层记录诊断时间戳,超时(如 1 s)按异常处理。
|
||||
验收:杀掉 hardware 节点后 deploy 进入安全状态。
|
||||
|
||||
- [ ] **异常上报 1002/3 的数据源**
|
||||
remote 的 `ErrorDoc` 恒为空列表,因为 `MotionState.msg` 没有错误字段。
|
||||
建议给 `MotionState` 加 `uint32 error_code` + `uint16 error_component`(位域与
|
||||
`SafeController::robot_error_state_` 对齐,component 用 M20 槽号),remote 再映射成
|
||||
`0x8001/0x8002/0x8020/0x8012` 等协议错误码。
|
||||
验收:模拟驱动器故障,App 收到非空 `ErrorList` 且 component 指向正确关节。
|
||||
|
||||
- [ ] **IMU 数据源缺失**
|
||||
`/imu/data` 目前发的是单位四元数、`orientation_covariance[0] = 0`(声称有效),
|
||||
但协议里没有任何 IMU 字段,硬件桥也没有读 IMU。
|
||||
按本项目自己的约定,拿不到姿态时应把 `orientation_covariance[0]` 置 `-1`;
|
||||
改之前要确认 deploy 的 `[rl].require_imu` 会不会因此拒绝进 RL。
|
||||
|
||||
## P1:标定与配置一致性(上真机前)
|
||||
|
||||
- [ ] **`dir`/`offset` 的施加方未定**
|
||||
hardware 现在收发的都是**原始电机量**,`dir`/`offset_deg` 由 deploy 的 `InvictaInterface` 施加;
|
||||
但 hardware 的 README 原设计与 `hardware_bridge.h` 注释写的是「由本节点施加」。
|
||||
两边都施加时 `dir=±1` 会二次相乘抵消、`offset` 会翻倍(当前 offset 全 0,尚未暴露)。
|
||||
二选一并同步修改另一侧配置与文档;同时注意 `[joint_limits]` 在两份 TOML 里重复维护。
|
||||
|
||||
- [ ] **反馈帧仲裁 ID 需真机确认**
|
||||
`ReadHardware` 按「反馈使用关节自身 CANID」实现(手册只写了从站 CANID 1~6)。
|
||||
上电后 `candump can0` 核对;若不是 1~4,需要改匹配逻辑。
|
||||
|
||||
- [ ] **`lie_height = 0` 与位置限位矛盾**
|
||||
`invicta_deploy.toml` 里 `lie_height = 0.00` 反解 `θ = acos(0) = 90° = 1.571 rad`,
|
||||
超过同文件 `[joint_limits].upper = 1.40`。确认是笔误还是限位需要放宽。
|
||||
|
||||
- [ ] **轮的位置限位占位值**
|
||||
`[joint_limits]` 中轮的 `lower/upper = ±100` 既不在归一化边界内,也不表示限幅
|
||||
(轮不支持位置控制,手册 §4.4)。加载校验时已跳过,但语义上应改成 ±12.5 或显式标注「仅占位」。
|
||||
|
||||
- [ ] **摆臂速度限幅复核**
|
||||
`vel_limit = 22.40 rad/s` 是额定 15.5 的 1.45 倍,比轮的 100/81.2(1.23 倍)更激进。
|
||||
如果当初是估的,建议按实测重新定。
|
||||
|
||||
- [ ] **位置指令越界目前静默夹紧**
|
||||
力矩和 kp/kd 越界会告警,位置不会。策略动作顶到限位时可能值得提示一次(注意别刷屏)。
|
||||
|
||||
## P2:协议状态上报的剩余占位
|
||||
|
||||
- [ ] `MotionStatus` 的 `LinearX / LinearY / Height / RemainMile`:需要里程计、运动学估计与电池数据。
|
||||
- [ ] `1002/5` 的电池(电压/电量/温度/serial)、CPU、GPS:无数据源。
|
||||
- [ ] `Charge / HES / Sleep`:没有充电状态、硬急停信号与休眠管理。
|
||||
- [ ] `HipY` 的正负号未与真实 App/M20 对照:如果 App 只是显示数值没问题,若渲染姿态需要核对。
|
||||
- [ ] 未实现指令:照明灯、休眠设置/查询、自主充电、速度指令(导航模式)、导航任务(Pro)。
|
||||
|
||||
## P3:工程与清理
|
||||
|
||||
- [ ] **启动时序的 `kEnable` 可能被采样合并**
|
||||
deploy 已改为「等订阅者再发」且 hardware 在 `kMotorControl` 未使能时会补发使能,功能上已安全;
|
||||
但 hardware 循环线程处理 `kErrorReset` 的使能连发会阻塞约 80 ms,期间可能观察不到 `kEnable`。
|
||||
若要严格逐帧落地,可把连发改成分段非阻塞发送。
|
||||
- [ ] `[motor.gains].bench_*` 已被指令里的 kp/kd 取代,确认是否删除。
|
||||
- [ ] 构建镜像 `localhost/custom-ros-noetic` 缺 toml++,目前靠挂载外部安装树 + `CMAKE_PREFIX_PATH`。
|
||||
可考虑在镜像里装 toml++,或在仓库内 vendored。
|
||||
- [ ] 真机联调清单:`ip link set canX ... fd on` 权限、`roslaunch` 启动顺序、双路差分驱动方向。
|
||||
|
||||
## 已完成(本会话)
|
||||
|
||||
- [x] SocketCAN FD 收发 + 智身模组协议编解码(使能/失能/运行、12 字节反馈解码)
|
||||
- [x] 温度读回 → `/joint_diagnostics`(默认 10 Hz,可配置)→ remote 填 1002/5
|
||||
- [x] 1002/4 的 8 → 16 槽映射(swing→HipY、wheel→Wheel,HipX/Knee 占位 0)
|
||||
- [x] 力矩/速度/位置限幅,kp/kd 越界夹紧并告警;限幅与归一化边界分离 + 加载期校验
|
||||
- [x] 指令看门狗(100 ms 无指令转阻尼)
|
||||
- [x] 容器内构建 + 冒烟测试;修复启动时序丢失与订阅回调阻塞
|
||||
Reference in New Issue
Block a user