LinkNexus/huoyi_deploy
20
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: LinkNexus/huoyi_deploy:a17338375640c7557b45252e94880543bb750895
Branches Tags
LinkNexus/huoyi_deploy:master LinkNexus/huoyi_deploy:invicta/ros1
..
compare: LinkNexus/huoyi_deploy:invicta/ros1
Branches Tags
LinkNexus/huoyi_deploy:invicta/ros1 LinkNexus/huoyi_deploy:master
4 Commits
a173383756 .. invicta/ros1
Author SHA1 Message Date
caowangrenbo 7ba2bba450 feat(safety): 错误位上报 1002/3；IMU 断流判定；移除硬件假 IMU 
- MotionState 增加 error_code，新增 invicta_msgs/error_code.h 共用位定义；SafeController 写入、remote 展开成协议 ErrorList（component 暂为 0）
- InvictaInterface 增加 IMU 断流/恢复判定（imu_timeout_s / imu_recover_s），covariance=-1 视为不可用；RL 丢失 IMU 走正常趴下
- invicta_hardware 不再发布硬编码单位四元数 IMU（含错误的 covariance[0]=0）
- 更新 README 与 docs/TODO.md：记录已完成项、反馈帧 CANID 1~4 已确认、[joint_limits] 重复维护入待办
 2026-10-23 23:32:57 +08:00
caowangrenbo 944774eab3 refactor(log): 日志文案改 ASCII 英文，修 ROS_INFO 中文乱码 
ROS_INFO/WARN/ERROR 走的不是 printf，而是 rosconsole -> log4cxx，后者按进程当前的
C 语言环境对消息做一次窄/宽字符转码。节点从未调用 setlocale，LC_CTYPE 停在默认的
"C"（ASCII），于是非 ASCII 的每个字节被替换成一个 '?'——一个汉字 3 字节正好 3 个
'?'。而 [config] 摘要那条 std::cout 路径直接写字节、不转码，所以之前只有 ROS_* 的
日志乱码。备选方案是 setlocale(LC_CTYPE, "")（3 行即可修），这里采用"运行时输出一律
英文"：英文日志不依赖语言环境，systemd/robot_upstart 拉起时也不会因缺 LANG 复发。

改动与校验：
- invicta_hardware / invicta_remote / invicta_deploy / invicta_msgs 共 20 个文件、
  202 处字符串字面量改为 ASCII 英文。中文代码注释、标识符、话题名、JSON/TOML 键名、
  日志前缀一律未动（把字面量内容挖空后与改前做骨架比对，20 个文件零差异）。
- printf 格式说明符（%s/%d/%zu/%.1f/%02X ...）的数量、类型、顺序逐文件与改前一致。
- 顺带 ASCII 化非中文字符：° -> deg、± -> +/-、× -> x、→ -> ->、框线 -> "+==+"，
  并修正了启动横幅框线宽度差 2 列的问题。
- motion_defs.h 的 GaitName / MotionParamName 显示名改英文（只用于日志与回包文案）；
  协议字段仍传整数（bs["MotionState"] = int(...)），不影响上位机协议。

注意：invicta_remote 回给手柄的 Unsupported / "Not allowed operation" 文案也变为英文，
会出现在上位机错误列表里。

已在 SBC（aarch64, Noetic）编译确认。
 2026-10-23 15:39:55 +08:00
caowangrenbo 46bbb06978 feat(hardware): 标定归硬件；按 name 选关节 + kSetZero；toml++ 改 vendored 
标定边界上移到 invicta_hardware：
- raw↔model 的 dir/offset 由 hardware 独家施加（话题收发都是模型帧），
  deploy 的 InvictaInterface 退化为透传，invicta_deploy.toml 不再有 dir/offset_deg。
  这样 [joint_limits]（模型帧）与被限幅的指令同帧，/joint_states 也是模型角。

关节选择（部分接入 / 单腿调试）：
- JointCommandArray 的 name 即本次要控制的关节子集（空 = 全部已配置关节）。
- 未选中的关节写「零力矩自由」块，而不是留全 0——按协议 (0,0,0,0,0) 会解码成
  P_MIN/V_MIN/T_MIN，其中 tau=T_MIN（摆臂 -100 N·m）是危险的前馈力矩。
- 新增 control_word kSetZero(6) -> 0xEF，只作用于选中的摆臂（轮毂不支持，跳过）。

调试脚本 invicta_hardware/scripts/hw_debug_test.py：
- 只起 hardware 节点调试：monitor/damping/angle/wheel/offset/zero 六种模式 + --joints。
- 限幅参数硬编码；发指令时默认回显模型角。

构建：toml++ 改为 vendored single-header（三个包各 third_party/tomlplusplus/toml++/toml.hpp），
   去掉 find_package(tomlplusplus) 与 -DTOML_HEADER_ONLY（单头自带 header-only）。

已在容器（localhost/custom-ros-noetic）中全新 catkin_make + smoke_test 验证通过。
 2026-10-23 13:44:27 +08:00
caowangrenbo 594885b9e8 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 协议整理、待办清单
 2026-10-22 11:05:49 +08:00
55 changed files with 56252 additions and 454 deletions
Expand all files Collapse all files
README.md
+25 -25
View File
@@ -5,6 +5,7 @@
- 上游参考代码：`sdk_deploy/src/M20_sdk_deploy`（ROS2 / ament / drdds）
- 本工程：ROS1 / catkin / Noetic / Ubuntu 20.04
- 协议依据：`proto.md`（山猫 M20 basic_server V1.2.0，UDP + JSON）
- 关节模组 CANFD 协议：[`docs/zhishen_joint_module_canfd.md`](docs/zhishen_joint_module_canfd.md)（智身模组手册 V1.2）；文档索引见 [`docs/README.md`](docs/README.md)
> 代码源自 [DeepRoboticsLab/sdk_deploy](https://github.com/DeepRoboticsLab/sdk_deploy) 的 M20 sim2real 部分，遵循 BSD-3-Clause（见 `LICENSE`）；已按 ROS1、厂家完整协议与本机器人形态重构。
@@ -70,7 +71,9 @@ invicta_hardware ──/joint_states (200 Hz)──→ invicta_deploy ──/joi
| `invicta_hardware/config/invicta_hardware.toml` | 硬件节点 | 关节表与硬件标定、每条腿挂的总线/CANID、**电机量程与 CAN 参数**、本节点话题与发布频率 |
launch 只传一个路径：`<param name="config_file" value="$(find <本包>)/config/<本包>.toml"/>`；
不传则默认取本包 `config/` 下的同名文件。TOML 解析用 toml++，以 **header-only 方式**使用（`-DTOML_HEADER_ONLY=1`），CMake 里只有一句 `find_package(tomlplusplus REQUIRED)`——库源码不进仓库，也不引入运行时动态库依赖。
不传则默认取本包 `config/` 下的同名文件。TOML 解析用 toml++ 的 **single-header**（`toml.hpp`，
vendor 在三个包各自的 `third_party/tomlplusplus/toml++/` 下）。单头版本本身就是 header-only，
实现直接编进节点：**不需要 `find_package`、不需要系统装库、也不需要任何宏**，目标机无运行时 `.so` 依赖。
改动节奏：**改 TOML 不用重编**；改各节点的配置头（`deploy_config.h` / `remote_config.h` / `hardware_config.h`）会自动触发该节点重编。
@@ -84,18 +87,24 @@ launch 只传一个路径：`<param name="config_file" value="$(find <本包>)/c
- 趴下：h = `[leg].lie_height`（默认 0.10 m → ±78.5°），**前腿向前倒、后腿向后倒**（由 `[[legs]].splay_sign` 决定，与 M20 相反）。
- 轮关节指令 `kp` 恒为 0（速度/力矩模式，`wheel_kp_zero`）。
硬件约定（电机方向、机械零点）在 `[[legs]]` 里逐关节给：
硬件约定（电机方向、机械零点）由 **`invicta_hardware`** 独家承担，写在它自己的
`config/invicta_hardware.toml` 的 `[[legs]]` 里，逐关节给：
```toml
[[legs]]
name = "fl"
splay_sign = 1.0 # 模型姿态：趴下时向前倒
swing = { dir = 1.0, offset_deg = 0.0 } # 硬件：电机正方向 / 机械零点
wheel = { dir = 1.0, offset_deg = 0.0 }
swing_joint = "fl_swing"
wheel_joint = "fl_wheel"
swing = { dir = 1.0, offset_deg = 0.0, bus = "can0", canid = 1 } # 硬件：电机正方向 / 机械零点
wheel = { dir = 1.0, offset_deg = 0.0, bus = "can0", canid = 3 }
```
指令换算：`q_cmd = dir · θ_model + offset`，反馈反向（`invicta_interface` 一处完成）。
第一次上机先按默认值跑，用 idle 打印的关节角与实测对照，再填 `dir`/`offset_deg`。
指令换算 `q_motor = (q_model − offset)·dir`、反馈 `q_model = q_motor·dir + offset`（速度/力矩只乘 `dir`）
都在该节点一处完成；话题上收发的都是**模型帧**，`invicta_deploy` 的 `invicta_interface` 只做透传。
deploy 侧的 `[[legs]]` 只保留模型姿态约定 `splay_sign`。
第一次上机先按默认值（`dir` 见配置、`offset_deg=0`）跑，用调试脚本或 idle 打印的关节角与实测对照，
再填 `dir`/`offset_deg`；也可用调试脚本的置零（`--mode zero`）把当前位姿写为零位后置 `offset_deg=0`。
## 步态 = RL 模型
厂家协议里的 `GaitParam` 不是简单参数，**每个步态对应一个 RL 模型**：
@@ -118,28 +127,19 @@ catkin_make -j4 # 交叉编译：-DBUILD_PLATFORM=arm
source devel/setup.bash
```
依赖：`roscpp`、`roslib`、`sensor_msgs`、`std_msgs` + 自建 `invicta_msgs`。`eigen`、`onnxruntime 1.22`（x86/aarch64）、`policy.onnx` 已 vendor，无需联网。
依赖：`roscpp`、`roslib`、`sensor_msgs`、`std_msgs` + 自建 `invicta_msgs`。`eigen`、`onnxruntime 1.22`（x86/aarch64）、`policy.onnx`、`toml++` 均已 vendor，无需联网、无需系统装库。
### 一次性依赖：toml++（系统安装，源码不入仓库）
### toml++（已 vendor 进仓库，无需安装）
三个节点都用 `find_package(tomlplusplus REQUIRED)` 找它，**以 header-only 方式编译**（实现编进节点里），
因此目标机不需要额外部署 `libtomlplusplus.so`。
`toml.hpp`（single-header，上游 https://github.com/marzer/tomlplusplus，v3.4.0）放在三个包各自的
`third_party/tomlplusplus/toml++/toml.hpp`。**单头版本本身就是 header-only（"drop it in, there is no
step two"），不需要任何宏**，实现直接编进节点：
```bash
# 方式一：Ubuntu 22.04+ 有官方包
sudo apt install libtomlplusplus-dev
- **不需要 `find_package(tomlplusplus)`、不需要 `apt install`、不需要 `-DTOML_HEADER_ONLY`**，`catkin_make` 开箱即用；
- 目标机不需要 `libtomlplusplus.so`；
- 升级：下载新的 `toml.hpp` 覆盖三个包里的副本（保持同版本）即可，见各 `third_party/tomlplusplus/README.md`。
# 方式二：目标机是 Ubuntu 20.04（Noetic 基线）或要交叉编译 aarch64 —— 源里没有此包，源码编译安装到系统
# 源码自行获取（不放进本仓库），例如 tomlplusplus 3.4.0
cmake -S tomlplusplus -B build -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local
cmake --build build -j
sudo cmake --install build # 装头文件 + CMake 配置（tomlplusplusConfig.cmake）
# 方式三：离线镜像里预装 .deb（只含头文件与 CMake 配置，123 KB）
sudo dpkg -i libtomlplusplus-dev_3.4.0+ds-0.2build2_amd64.deb
```
装完 `cmake --find-package ... tomlplusplus` 能找到即可；`catkin_make` 会在 configure 阶段报缺库并停下。
（早先的「系统安装 / 离线 .deb / 源码装到 /usr/local」三种方式已不再需要。）
## 运行
docs/README.md
+8
View File
@@ -0,0 +1,8 @@
# 文档索引
| 文档 | 内容 |
| --- | --- |
| [zhishen_joint_module_canfd.md](zhishen_joint_module_canfd.md) | 智身关节模组 CANFD 通信协议（整理自厂家手册 V1.2）：通信参数、帧格式、使能/失能/运行/置零指令、状态与故障位、数值编码、各型号阈值 |
| [TODO.md](TODO.md) | 待办清单：每项写清原因与验收方式，已完成项在文末 |
厂家原始资料：《智身关节模组使用手册 V1.2（CANFD 版）》，2026-04-22。
docs/TODO.md
+82
View File
@@ -0,0 +1,82 @@
# TODO
本文件记录**当前会话中确认但本期不做**的事项。按优先级分组，每项尽量写清「为什么」和「验收方式」，
避免下次重新推导。已完成的部分见各包 README 与文末清单。
## P0：安全链路（真机跑 RL 前必须处理）
- [ ] **异常数据接入 deploy 的保护链路**
硬件已在 `/joint_diagnostics` 提供温度/状态字/电压（10 Hz），但 deploy 侧仍未接：
`InvictaInterface` 没有订阅该话题，`GetMotorTemperture()` 走基类常量 30 ℃；
`GetDriverStatusWord()` 虽已重写为返回成员 `driver_status_`，但该成员只被初始化成全 1、无人更新，
所以 `SafeController` 的过温检查与驱动报错检查依然恒不触发。
做法：`InvictaInterface` 订阅 `/joint_diagnostics`，按关节名填 `driver_status_`、覆盖 `GetMotorTemperture()`。
注意：协议 Byte9 的 bit0 是「使能中」，而 `IsDriverStatusNormal()` 判断 `status_word != 1` 报错，
失能但健康的电机回 `0x00` 会误判；建议在接口层归一化成「无故障位就填 1」。
验收：人为把某个关节温度改成 101 ℃，deploy 进入 `safe_control_mode=2` 并切 `kJointDamping`。
- [ ] **诊断超时判断**
诊断话题 10 Hz，`SafeController` 读缓存值。若 hardware 进程死掉，温度会停在最后一帧的「正常」。
需要在接口层记录诊断时间戳，超时（如 1 s）按异常处理。
验收：杀掉 hardware 节点后 deploy 进入安全状态。
（对照：IMU 的同类问题已解，走 `IsImuHealthy()` 的断流判定——本条可照抄那套「时间戳 + 超时 + 恢复去抖」。）
## P1：标定与配置一致性（上真机前）
- [ ] **`[joint_limits]` 在两份 TOML 里重复维护**
`invicta_deploy.toml` 用一份做状态机自检，`invicta_hardware.toml` 用一份做下发夹紧。
两份手改容易漂移，且改限位时要同步改两处。可考虑合并成单一来源（hardware 读 deploy 的，或从一处生成另一处）。
- [ ] **`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 倍）更激进。
如果当初是估的，建议按实测重新定。
- [ ] **位置指令越界目前静默夹紧**
`invicta_hardware_node.cpp` 的 `PackRunBlocks` 里力矩和 kp/kd 越界会告警，位置不会。
策略动作顶到限位时可能值得提示一次（`ROS_WARN_THROTTLE`，注意别刷屏）。
## P2：协议状态上报的剩余占位
- [ ] `MotionStatus` 的 `LinearX / LinearY / Height / RemainMile`：需要里程计、运动学估计与电池数据。
- [ ] `1002/5` 的电池（电压/电量/温度/serial）、CPU、GPS：无数据源。
- [ ] `Charge / HES / Sleep`：没有充电状态、硬急停信号与休眠管理。
- [ ] `HipY` 的正负号未与真实 App/M20 对照：如果 App 只是显示数值没问题，若渲染姿态需要核对。
- [ ] 未实现指令：照明灯、休眠设置/查询、自主充电、速度指令（导航模式）、导航任务（Pro）。
- [ ] 错误上报的 `component` 恒为 0（见文末遗留）：要定位到具体关节，需在 deploy 侧把错误位与关节号关联。
## P3：工程与清理
- [ ] **启动时序的 `kEnable` 可能被采样合并**
deploy 已改为「等订阅者再发」且 hardware 在 `kMotorControl` 未使能时会补发使能，功能上已安全；
但 hardware 循环线程处理 `kErrorReset` 的使能连发会阻塞约 80 ms，期间可能观察不到 `kEnable`。
若要严格逐帧落地，可把连发改成分段非阻塞发送。
- [ ] `[motor.gains].bench_*` 已被指令里的 kp/kd 取代（现仅被加载、无消费方），确认是否删除。
- [ ] 真机联调清单：`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] **`dir`/`offset` 的施加方已定：hardware**（2026-10-22）
raw↔model 换算由 `invicta_hardware` 独家承担（`[[legs]].swing/wheel` 的 `dir`/`offset_deg`），话题上收发都是模型帧；
deploy 的 `InvictaInterface` 退化为纯透传，`invicta_deploy.toml` 不再有 `dir`/`offset_deg`。
- [x] **反馈帧仲裁 ID 已确认为 1~4**（2026-10-23）：`ReadHardware` 按关节自身 CANID 匹配，与实测一致，无需改动。
- [x] **toml++ 改为 vendored**（`third_party/tomlplusplus`），不再依赖镜像或 `CMAKE_PREFIX_PATH`。
- [x] **容器内构建 + 冒烟测试**（`scripts/smoke_test.sh`）；修复启动时序丢失与订阅回调阻塞。
- [x] **异常上报 1002/3 的数据源**：`MotionState.error_code` + 共用位定义 `invicta_msgs/error_code.h`；
`SafeController` 每周期写 `UserCommand::robot_error_code`，remote 经 `RobotErrorList()` 展开成协议 `ErrorList`。
遗留：`component` 恒填 0（未按关节定位），已单列在 P2。
- [x] **IMU 数据源缺失**：`invicta_hardware` 不再发假 IMU（删掉单位四元数 + `covariance[0]=0`）；
deploy 把 `orientation_covariance[0]==-1` 视为不可用，并做断流/恢复判定（`imu_timeout_s`/`imu_recover_s`），
RL 丢失 IMU 走「正常趴下」而非就地垮下。实际数据源 `odin_imu_bridge` 不在本仓库，契约见 `invicta_deploy/README.md`。
docs/zhishen_joint_module_canfd.md
+282
View File
@@ -0,0 +1,282 @@
# 智身关节模组 CANFD 通信协议
> 整理自《智身关节模组使用手册 V1.2（CANFD 版）》，智身新创（苏州）智能科技有限公司，2026-04-22。
> 本文覆盖手册第 3 章协议主体，并补充第 1 章阻抗控制模型与第 2 章中影响协议使用的约束。
> 手册 V1.2 的变更：修改 P65、P85MAX-S 转矩阈值；新增 W110-S、P100MAX-S 关节通信阈值。
## 1. 适用范围
- 硬件：支持 CAN FD 的关节模组控制器与主控设备。
- 场景：电机控制、状态监测、参数配置。
- 控制模式：**阻抗控制模式**。电机使能后默认进入该模式，内部最终执行力矩闭环：
```
力矩指令 = Kp × (位置指令 − 反馈位置)
+ Kd × (速度指令 − 反馈速度)
+ 前馈力矩指令
```
该模型无积分项。Kp 越大位置刚度越高；Kd 越大阻尼越强、动态响应越慢。位置、速度、转矩、阻尼四种“模式”只是同一模型在不同参数配置下的表现：
| 控制方式 | 参数配置 | 说明 |
| --- | --- | --- |
| 位置控制 | 给定 `P_cmd`，`Kp > 0`，建议 `Kd > 0` | 围绕目标位置的弹性约束，允许位置偏差；轮毂电机不支持 |
| 速度控制 | 给定 `V_cmd`，`Kd > 0`，`Kp = 0` | Kd 相当于比例系数，存在稳态速度误差 |
| 转矩控制 | 给定 `T_ff`，其余为 0 | 直接输出恒定转矩；负载小于给定转矩时会持续加速 |
| 阻尼控制 | 仅 `Kd > 0`，其余为 0 | 输出与速度反向、大小成比例的阻力，不约束位置 |
## 2. CANFD 通信参数
| 类别 | 参数项 | 参数说明 |
| --- | --- | --- |
| 参数配置 | 波特率（仲裁段） | 1 Mbps |
| 参数配置 | 波特率（数据段） | 5 Mbps |
| 参数配置 | 帧格式 | 标准帧 |
| 参数配置 | 数据长度（主站发送） | 64 字节（可配置） |
| 参数配置 | 数据长度（关节返回） | 12 字节 |
| CANID | 主站 CANID | `0x00` |
| CANID | 从站关节 CANID | `0x0X`（X = 1~6） |
| CANID | 关节出厂默认 CANID | `0x03` |
| 通信速率 | 主机与关节通信速率 | 最大支持 1 kHz |
多字节字段全部为 **小端（低字节在前）**。下文示例中的 `00 80` 即 uint16 的 `0x8000`。
## 3. 帧格式
### 3.1 主机下发帧
一帧最多控制 6 个关节，每个关节占 10 字节，按 CANID 顺序排列：
| Byte | 功能 | 说明 |
| --- | --- | --- |
| Byte0–Byte1 | MSGID | 主机发送，从机原样返回；用于匹配请求帧，不用可填 0 |
| Byte2–Byte11 | CANID=1 指令数据 | 见第 4 节 |
| Byte12–Byte21 | CANID=2 指令数据 | 同上 |
| Byte22–Byte31 | CANID=3 指令数据 | 同上 |
| Byte32–Byte41 | CANID=4 指令数据 | 同上 |
| Byte42–Byte51 | CANID=5 指令数据 | 同上 |
| Byte52–Byte61 | CANID=6 指令数据 | 同上 |
| Byte62–Byte63 | 保留 | 手册未定义 |
每个关节块的起始字节为 `2 + 10 × (CANID − 1)`。
**下发长度**可按最大 CANID 裁剪：
| 最大关节 CANID | 下发最小字节数 |
| ---: | ---: |
| 1 | 12 |
| 2 | 24 |
| 3 | 32 |
| 4 | 48 |
| 5 / 6 | 64 |
- 最大 CANID=3 时下发 Byte0–Byte31（32 字节）；最大 CANID=1 时下发 Byte0–Byte11（12 字节）。
- 只控制不连续关节时，仍需按最大 CANID 的长度下发：例如只控 CANID=3，则发 64 字节，仅给 Byte22–Byte31 赋值，其余填 0 或保持上次值；控 CANID=1 和 4 时，给 Byte2–Byte11 与 Byte32–Byte41 赋值。
- CANID=4 时手册要求 48 字节，而实际数据只用到 Byte41（42 字节），以手册要求为准。
### 3.2 关节反馈帧
对所有指令，关节均返回固定的 12 字节状态帧：
| Byte | 功能 | 说明 |
| --- | --- | --- |
| Byte0–Byte1 | MSGID | 等于主机发送值 |
| Byte2–Byte3 | 关节输出端当前位置 | uint16，见第 5 节映射 |
| Byte4–Byte5 | 关节输出端当前速度 | uint16，rad/s |
| Byte6–Byte7 | 关节输出端当前扭矩 | uint16，N·m |
| Byte8 | 电机当前温度 | uint8，`0~255` 对应 `−40~215 °C`（即 `T = raw − 40`） |
| Byte9 | 电机当前状态码 | 位定义见第 6 节 |
| Byte10 | 电机当前电压 | uint8，`0~255` 直接对应 `0~255 V` |
| Byte11 | 预留 | — |
## 4. 指令数据（每关节 10 字节）
### 4.1 电机使能
| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
| Byte11 | 使能指令码 | `0xFC` |
示例（使能所有关节，MSGID = `0x0100`）：
```
Byte0-1 : 00 01
Byte2-11 : FF FF FF FF FF FF FF FF FF FC
Byte12-21 : FF FF FF FF FF FF FF FF FF FC
...
Byte52-61 : FF FF FF FF FF FF FF FF FF FC
Byte62-63 : xx xx
```
### 4.2 电机失能
| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
| Byte11 | 使能指令码 | `0xFD` |
失能后所有 MOS 管关闭，电机线圈处于断开状态。
### 4.3 电机运行
必须在使能之后发送。运行指令块内容：
| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte3 | 位置指令 | uint16，`0~65535` 映射到 `P_MIN~P_MAX`（rad），`0x8000` = 0 rad |
| Byte4–Byte5 | 速度指令 | uint16，`0~65535` 映射到 `V_MIN~V_MAX`（rad/s），`0x8000` = 0 rad/s |
| Byte6–Byte7 | 刚度 Kp 指令 | uint16，`0~65535` 映射到 `0~KP_MAX`，`0x0000` = 0 |
| Byte8–Byte9 | 阻尼 Kd 指令 | uint16，`0~65535` 映射到 `0~KD_MAX`，`0x0000` = 0 |
| Byte10–Byte11 | 前馈力矩指令 | uint16，`0~65535` 映射到 `T_MIN~T_MAX`（N·m），`0x8000` = 0 N·m |
示例（所有关节 0 位置、0 速度、0 Kp、0 Kd、0 力矩）：
```
Byte0-1 : 00 01
Byte2-11 : 00 80 00 80 00 00 00 00 00 80
Byte12-21 : 00 80 00 80 00 00 00 00 00 80
...
Byte52-61 : 00 80 00 80 00 00 00 00 00 80
```
### 4.4 设置当前位置为机械零位
| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
| Byte11 | 零位指令码 | `0xEF` |
设置成功且关节角度未变化时，当前反馈位置为 0。电机在任意位置的上电角度上报范围为 `(−π, π)` rad。
> 完成机械零点标定后，关节具备单圈绝对位置记忆：断电及人为转动输出端后重新上电，仍可恢复当前单圈绝对位置。**轮毂电机不支持该功能，也不支持位置控制。**
## 5. 数值编码
所有物理量以 uint16 线性映射。手册写作 `0~65535 → MIN~MAX`。
编码（物理量 → 原始值）：
```
raw = round((x - MIN) / (MAX - MIN) × 65535)
```
解码（原始值 → 物理量）：
```
x = MIN + raw / 65535 × (MAX - MIN)
```
说明：
- 位置、速度、前馈力矩为对称范围，因此 `0x8000`（32768）对应 0。按上式，`raw = 32768` 时 `x ≈ MIN + 0.5000076 × (MAX − MIN)`，与 0 的偏差来自 65536 个码值无法整除；工程上通常直接按 `32768` 作为零点，或用 `(raw − 32768) / 32768 × MAX` 计算对称量。
- Kp、Kd 为非负范围，`0x0000` 对应 0。
- 下发前必须按关节型号的 `P/V/T/KP/KD` 阈值限幅，见第 8 节。
Python 参考实现：
```python
def encode(x, lo, hi):
r = round((x - lo) / (hi - lo) * 65535)
return max(0, min(65535, r))
def decode(raw, lo, hi):
return lo + raw / 65535 * (hi - lo)
def pack_u16(v):
return bytes((v & 0xFF, (v >> 8) & 0xFF)) # 小端
def run_block(p, v, kp, kd, t, lim):
return b"".join((
pack_u16(encode(p, lim["p_min"], lim["p_max"])),
pack_u16(encode(v, lim["v_min"], lim["v_max"])),
pack_u16(encode(kp, 0.0, lim["kp_max"])),
pack_u16(encode(kd, 0.0, lim["kd_max"])),
pack_u16(encode(t, lim["t_min"], lim["t_max"])),
))
```
## 6. 状态码与故障保护
Byte9 状态码按位定义（1 有效）：
| bit | 功能 | 触发条件 | 恢复措施 |
| ---: | --- | --- | --- |
| 0 | 电机使能 | 使能为 1，失能为 0 | — |
| 1 | 过压故障 | 母线电压超过阈值 | 重启或重新使能 |
| 2 | 过流故障 | 电流超过阈值 | 重启或重新使能 |
| 3 | 过温故障 | 电机温度超过阈值 | 重启或重新使能；状态位低于 85 °C 自动恢复 |
| 4 | 过速故障 | 输出端速度超过阈值 | 重启或重新使能 |
| 5 | 编码器故障 | 编码器测量失效 | 重启或重新使能 |
| 6 | 欠压故障 / 预驱故障 | 母线电压低于阈值，或检测到预驱故障 | 重启或重新使能 |
| 7 | 预留 | — | — |
发生故障时电机会自动失能，同时对应状态位置 1。主站应持续检查 Byte9 的 bit1–bit6，非零即进入故障处理。
## 7. 关节型号与阈值
| 关节型号 | P_MAX | P_MIN | V_MAX | V_MIN | T_MAX | T_MIN | KP_MAX | KD_MAX |
| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
| P55 | 12.5 | −12.5 | 65 | −65 | 30 | −30 | 500 | 5 |
| P65 | 12.5 | −12.5 | 65 | −65 | 50 | −50 | 500 | 5 |
| P65MAX-S | 12.5 | −12.5 | 65 | −65 | 100 | −100 | 500 | 5 |
| P85MAX-S | 12.5 | −12.5 | 65 | −65 | 300 | −300 | 500 | 50 |
| W150-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
| W190-S | 12.5 | −12.5 | 200 | −200 | 50 | −50 | 500 | 5 |
| W110-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
| P100MAX-S | 12.5 | −12.5 | 65 | −65 | 650 | −650 | 500 | 50 |
单位：位置 rad，速度 rad/s，转矩 N·m；Kp、Kd 无量纲。轮毂电机（W 系列）速度阈值明显高于摆臂电机。
### 7.1 额定参数与额定速度（厂家参数页补充）
| 型号 | 额定功率 (W) | 额定扭矩 (N·m) | 峰值扭矩 (N·m) | 推算额定速度 (rad/s) |
| --- | ---: | ---: | ---: | ---: |
| P55 | 78.5 | 6.5 | 21 | 12.1 |
| P65 | 330 | 11.5 | 48 | 28.7 |
| P65MAX-S | 330 | 21.3 | 75 | 15.5 |
| P85MAX-S | 706 | 43 | 180 | 16.4 |
| W150-S | 495 | 3.5 | 11 | 141.4 |
| W190-S | 706 | 8.7 | 28.68 | 81.2 |
额定速度 = 额定功率 / 额定扭矩。它和上表的 `V_MAX` 不是一回事：`V_MAX` 是**通信编码阈值**（决定 uint16 的满量程），额定速度是持续工作能力。本机取的三层关系：
| 关节 | 额定速度 | 人为限幅（`[joint_limits].vel_limit`） | 通信阈值（`[motor.*].v_max`） |
| --- | ---: | ---: | ---: |
| 摆臂 P65MAX-S | 15.5 | 22.4 | 65 |
| 轮 W190-S | 81.2 | 100 | 200 |
人为限幅略高于额定、远低于阈值；实际下发按 `min(阈值, 限幅)` 饱和。注意阈值/额定的比值并不统一（P65MAX-S ≈ 4.2×，W190-S ≈ 2.5×），不能用其中一个推另一个。
## 8. 调试给定值参考
手册给出的典型增益（`P_cmd`, `V_cmd`, `T_ff`, `Kp`, `Kd`）：
| 关节型号 | 位置控制 | 速度控制 | 转矩控制 |
| --- | --- | --- | --- |
| P55 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
| P65 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
| P65MAX-S | `3.14, 0, 0, 3, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
| P85MAX-S | `3.14, 0, 0, 5, 1` | `0, 5, 0, 0, 2` | `0, 0, 3, 0, 0` |
| W150-S | — | `0, 5, 0, 0, 0.1` | `0, 0, 0.3, 0, 0` |
| W190-S | — | `0, 5, 0, 0, 0.5` | `0, 0, 0.2, 0, 0` |
阻尼控制只需 `Kd > 0`，其余参数置 0。
使用注意：
1. 目标位置与当前位置差值不宜过大，否则会产生瞬时大转矩和大电流；建议给定值缓慢增加。
2. `Kp`、`Kd` 的比值不宜过大；速度控制时 `Kd` 不宜过大。
3. 速度项启动转矩由速度给定与 `Kd` 决定，突变会产生电流峰值或振荡。
4. 转矩控制下若负载小于给定转矩，转速会一直上升到最大转速甚至失控。
5. 写入参数后需断电重启才能生效；上位机装入参数后需再次“写入参数”。
## 9. 通信时序要点
1. 上电后先发使能（`0xFC`），再发运行指令；失能（`0xFD`）后线圈断开。
2. 需要绝对零位时，在装配完成后发一次置零（`0xEF`），之后反馈位置以该点为 0。
3. 主机发送周期最大支持 1 kHz；实际控制周期按上层需求设定，并保证每帧携带同一 MSGID 以匹配反馈。
4. 一帧可同时控制最多 6 个关节；不同 CANID 的关节共用一个 64 字节帧，按 `2 + 10 × (CANID − 1)` 定位各自数据块。
5. 每个关节独立返回 12 字节状态帧，主站需按 MSGID 关联请求与反馈。
6. 反馈中的位置、速度、转矩均使用与下发相同的 `MIN~MAX` 映射，解码时必须使用对应关节型号的阈值。
7. ⚠️ 编码满量程只能取第 7 节的型号阈值，不能取摇杆/工作范围。台架脚本曾用 `float_to_uint(v, V_MAX=10, ...)` 下发轮速，而 W190-S 的阈值是 ±200：同一 raw 值固件按 ±200 解码，实际轮速被放大 20 倍。工作上限（如 100）应作为独立限幅先夹紧，再按阈值编码。
src/invicta_deploy/CMakeLists.txt
+3 -6
View File
@@ -31,12 +31,9 @@ catkin_package(
CATKIN_DEPENDS roscpp roslib sensor_msgs invicta_msgs
)
# TOML 解析库：系统安装（见 README「依赖」），CMake 里体现为 required
find_package(tomlplusplus REQUIRED)
get_target_property(TOMLPLUSPLUS_INCLUDE_DIR tomlplusplus::tomlplusplus INTERFACE_INCLUDE_DIRECTORIES)
include_directories(${TOMLPLUSPLUS_INCLUDE_DIR})
# 以 header-only 方式使用：实现编进本节点，避免引入运行时动态库依赖（目标机无需部署 .so）
add_definitions(-DTOML_HEADER_ONLY=1)
# TOML 解析库：single-header，vendored 在 third_party/tomlplusplus/toml++/toml.hpp
# 单头版本本身就是 header-only（自带 TOML_HEADER_ONLY），实现直接编进本节点：无 find_package、无运行时库依赖
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/tomlplusplus)
set(THIRD_PARTY ${CMAKE_CURRENT_SOURCE_DIR}/third_party)
set(ORT_ROOT ${THIRD_PARTY}/onnxruntime/${BUILD_PLATFORM})
src/invicta_deploy/README.md
+30 -1
View File
@@ -59,11 +59,40 @@ config/ launch/ 参数与启动文件
| 关节角到达站立位姿（轮关节除外） | `rl/require_pose`、`rl/joint_tolerance` | true / 0.15 rad |
| 关节速度足够小 | `rl/velocity_tolerance` | 1.0 rad/s |
| IMU 数据有效（无 NaN、加速度在 0.1~3 g） | `rl/require_imu` | true |
| IMU 连续（有新鲜可用帧、且断流后已恢复） | `rl/imu_timeout_s`、`rl/imu_recover_s` | 0.2 s / 1.0 s |
| 到达站立位姿后稳定一段时间 | `rl/enter_delay` | 0.5 s |
自检不通过时保持站立，并每秒打印一次原因（`[RL] 暂不进入 RL：...`），`/motion_state` 如实报 1。
另外推理输出若出现 NaN/Inf，会立刻停发指令并落回关节阻尼。
### IMU 断流：退出 RL 走「正常趴下」，恢复前不再进 RL
RL 运行中 IMU 断流（超过 `rl/imu_timeout_s` 没有新鲜可用的姿态帧）时，**按正常退出去趴下**：
`RLControlState` 返回 `kLieDown`，由 `LieDownState` 做受控下降（关节空间样条，不依赖 IMU），
而不是就地转阻尼垮下来。恢复需要连续 `rl/imu_recover_s` 的新鲜可用帧，
在此之前 `ReadyForRl()` 拒绝重新进入 RL。
注意这**不**通过 `safe_control_mode` 表达：那个标志会让 `LieDownState` 自己也立刻转阻尼
（`liedown_state.hpp` 里 `safe_control_mode != 0 → kJointDamping`），把受控下降打断成就地垮掉。
IMU 解算结果含 NaN 仍走 `safe_control_mode`（急停语义），两者刻意分开。
### IMU 话题契约
姿态/角速度由**独立的 IMU 节点**发布（`invicta_hardware` 不再发布 `/imu/data`），
本节点与 `invicta_remote` 按 `[topics].imu` 订阅。数据源必须满足：
- `header.stamp` 用 ROS 时间（本节点把它当 `imu_ts_` 转给状态机）；
- `frame_id` 本节点**不检查**，但姿态必须是**机体轴**下的姿态。当前数据源 `src/odin_imu_bridge`
发的是 `base_link_com`（与 `base_link` 同轴、平移到质心），与策略的训练约定一致；
- **姿态必须真给**：`orientation_covariance[0]` 不能是 `-1`（`-1` = 不提供姿态）。给 `-1` 时本节点
丢弃该帧并告警，且视为**不可用** → 等同断流（不满足连续性，见上一节）。
⚠️ `odin_imu_bridge` 的 `orientation_covariance` 是从里程计的 `pose.covariance` 转出来的，
如果上游里程计在该位置填 `-1`，本节点就会认为姿态不可用——首次接入时用
`rostopic echo /imu/data -n1` 确认一下这几个字段；
- 线加速度 `|acc|` 要落在 0.1~3 g，否则 `rl/require_imu` 自检过不去；
- 姿态用于观测里的重力轴夹角（`projected_gravity = R(rpy)⁻¹ ·(0,0,-1)`，见 `invicta_policy_runner.hpp`），
角速度进观测，线加速度**不进观测**、只用于自检。
> 无硬件台架（`invicta_hardware` 只发零位状态）时必须关掉位姿与 IMU 自检，否则永远进不了 RL：
> `roslaunch invicta_deploy invicta_deploy.launch rl_require_pose:=false rl_require_imu:=false`
@@ -110,7 +139,7 @@ gait:
| `[robot]` | 关节名与顺序（8 个）、机型标识 |
| `[joint_limits]` | 逐关节位置/速度限位（顺序同 joint_names） |
| `[leg]` `[leg.timing]` `[leg.gains]` | 2 自由度腿的几何（摆臂长度、站/收腿/趴下高度）、时序、各状态 PD 增益 |
| `[[legs]]` ×4 | 每腿的摆臂/轮关节名、`splay_sign`（倒向）、`dir`/`offset_deg`（硬件标定） |
| `[[legs]]` ×4 | 每腿的摆臂/轮关节名、`splay_sign`（模型倒向）。硬件标定 `dir`/`offset_deg` 在 invicta_hardware |
| `[rl]` | 站立完成后自动进 RL 的开关与自检阈值 |
| `[policy]` | 模型路径、decimation、观测缩放、关节顺序、动作尺度、默认姿态、kp/kd |
| `[gait.policies]` | 步态 → 模型文件（空 = 该步态不可用） |
src/invicta_deploy/config/invicta_deploy.toml
+6 -12
View File
@@ -59,45 +59,39 @@ enter_delay_s = 0.5 # 到站立位姿后再稳定多久 [s]
require_pose = true # 是否检查摆臂角已到站立位姿（无硬件台架时可关）
joint_tolerance = 0.15 # 摆臂角容差 [rad]
velocity_tolerance = 1.0 # 摆臂角速度容差 [rad/s]
require_imu = true # 是否检查 IMU 数据有效
require_imu = true # 是否检查 IMU 数据有效（含下面的连续性检查）
imu_timeout_s = 0.2 # 多久没有新鲜可用的 IMU 帧判为断流 → 退出 RL 走正常趴下
imu_recover_s = 1.0 # 断流后需连续这么久有新鲜可用帧才重新判健康（恢复去抖）
# ------------------------------------------------------------------ 每腿标定
# 摆臂指令角 = dir · θ_model + offset_deg，θ_model = splay_sign · acos(h/L)
# 模型约定：摆臂角 θ_model = splay_sign · acos(h/L)。
# splay_sign：模型姿态约定。趴下/收腿时摆臂往哪边倒——本题要求「前腿向前、后腿向后」，
# 即前腿 +1、后腿 −1（与 M20 相反）
# dir ：硬件约定，电机正方向与模型正方向的关系（±1）
# offset_deg：机械零点偏置 [deg]，第一次上机按 idle 打印角与实测对照后填写
# 硬件标定（电机方向 dir、机械零点 offset_deg）**不在本文件**：raw↔model 的换算由
# invicta_hardware 独家承担（见该节点 config 与 README「标定边界」），本节点只处理模型量。
[[legs]]
name = "fl"
swing_joint = "fl_swing"
wheel_joint = "fl_wheel"
splay_sign = 1.0
swing = { dir = 1.0, offset_deg = 0.0 }
wheel = { dir = 1.0, offset_deg = 0.0 }
[[legs]]
name = "fr"
swing_joint = "fr_swing"
wheel_joint = "fr_wheel"
splay_sign = 1.0
swing = { dir = -1.0, offset_deg = 0.0 }
wheel = { dir = -1.0, offset_deg = 0.0 }
[[legs]]
name = "hl"
swing_joint = "hl_swing"
wheel_joint = "hl_wheel"
splay_sign = -1.0
swing = { dir = 1.0, offset_deg = 0.0 }
wheel = { dir = 1.0, offset_deg = 0.0 }
[[legs]]
name = "hr"
swing_joint = "hr_swing"
wheel_joint = "hr_wheel"
splay_sign = -1.0
swing = { dir = -1.0, offset_deg = 0.0 }
wheel = { dir = -1.0, offset_deg = 0.0 }
# ------------------------------------------------------------------ 策略模型
[policy]
src/invicta_deploy/include/config/deploy_config.h
+20 -24
View File
@@ -18,15 +18,13 @@
namespace invicta {
/// 单个关节：限位与硬件标定
/// 单个关节：限位（模型帧）。硬件标定 dir/offset 在 invicta_hardware，见该节点 README
struct JointSpec {
std::string name;
bool is_wheel = false;
int leg = -1;
double lower = 0., upper = 0.; ///< 位置限位 [rad]
double vel_limit = 0.; ///< 速度限位 [rad/s]
double dir = 1.; ///< 硬件方向 ±1
double offset_deg = 0.; ///< 硬件零点偏置 [deg]
};
/// 一条腿：摆臂 + 轮
@@ -64,6 +62,8 @@ public:
bool rl_require_pose = true;
double rl_joint_tolerance = 0.15, rl_velocity_tolerance = 1.0;
bool rl_require_imu = true;
double rl_imu_timeout_s = 0.2; ///< 多久没有新鲜可用 IMU 帧判为断流（RL 退出、拒绝再进）
double rl_imu_recover_s = 1.0; ///< 断流后需连续这么久有新鲜可用帧才重新判健康（恢复去抖）
// ---- 策略 ----
std::string policy_path; ///< 相对本包目录、或绝对路径
@@ -165,17 +165,17 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
try {
tbl = toml::parse_file(path);
} catch (const toml::parse_error& e) {
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
return fail(std::string("TOML parse failed: ") + std::string(e.description()) + " @ " + path);
}
const toml::table* robot = detail::Section(tbl, "robot");
if (!robot) return fail("缺少 [robot] 段");
if (!robot) return fail("missing [robot] section");
detail::GetStr(robot, "name", robot_name);
detail::GetStr(robot, "version", robot_version);
detail::GetStrArr(robot, "joint_names", joint_names);
if (joint_names.empty()) return fail("[robot].joint_names 为空");
if (joint_names.empty()) return fail("[robot].joint_names is empty");
dof = int(joint_names.size());
if (dof % 2 != 0) return fail("joint_names 数量必须是偶数（每腿 摆臂+轮）");
if (dof % 2 != 0) return fail("joint_names count must be even (one swing + one wheel per leg)");
std::vector<double> lower, upper, vel_limit;
const toml::table* limits = detail::Section(tbl, "joint_limits");
@@ -183,7 +183,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
detail::GetDoubleArr(limits, "upper", upper);
detail::GetDoubleArr(limits, "vel_limit", vel_limit);
if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
return fail("[joint_limits] lower/upper/vel_limit length must equal joint count");
}
joints.assign(size_t(dof), JointSpec{});
for (int i = 0; i < dof; ++i) {
@@ -206,22 +206,16 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
leg.swing_index = IndexOf(leg.swing_joint);
leg.wheel_index = IndexOf(leg.wheel_joint);
if (leg.swing_index < 0 || leg.wheel_index < 0) {
return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
return fail("[[legs]] " + leg.name + " swing_joint/wheel_joint not in joint_names");
}
const toml::table* sw = detail::Section(*lt, "swing");
const toml::table* wh = detail::Section(*lt, "wheel");
detail::Get(sw, "dir", joints[size_t(leg.swing_index)].dir);
detail::Get(sw, "offset_deg", joints[size_t(leg.swing_index)].offset_deg);
detail::Get(wh, "dir", joints[size_t(leg.wheel_index)].dir);
detail::Get(wh, "offset_deg", joints[size_t(leg.wheel_index)].offset_deg);
joints[size_t(leg.swing_index)].leg = int(legs.size());
joints[size_t(leg.wheel_index)].leg = int(legs.size());
joints[size_t(leg.wheel_index)].is_wheel = true;
legs.push_back(leg);
}
}
if (legs.empty()) return fail("缺少 [[legs]] 段");
if (int(legs.size()) * 2 != dof) return fail("腿数与关节数不匹配（每腿 2 个关节）");
if (legs.empty()) return fail("missing [[legs]] section");
if (int(legs.size()) * 2 != dof) return fail("leg count does not match joint count (2 joints per leg)");
const toml::table* gc = detail::Section(tbl, "leg");
detail::Get(gc, "swing_link_len", swing_link_len);
@@ -243,12 +237,12 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
detail::Get(gains, "damping_kd_wheel", damping_kd_wheel);
detail::GetBool(gains, "wheel_kp_zero", wheel_kp_zero);
if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len 必须 > 0");
if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len must be > 0");
for (const auto& pair : {std::make_pair("stand_height", stand_height),
std::make_pair("pre_height", pre_height),
std::make_pair("lie_height", lie_height)}) {
if (pair.second > swing_link_len) {
return fail(std::string("[leg].") + pair.first + " 大于 swing_link_len，物理上不可达");
return fail(std::string("[leg].") + pair.first + " greater than swing_link_len, physically unreachable");
}
}
@@ -259,6 +253,8 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
detail::Get(rl, "joint_tolerance", rl_joint_tolerance);
detail::Get(rl, "velocity_tolerance", rl_velocity_tolerance);
detail::GetBool(rl, "require_imu", rl_require_imu);
detail::Get(rl, "imu_timeout_s", rl_imu_timeout_s);
detail::Get(rl, "imu_recover_s", rl_imu_recover_s);
if (const toml::table* pol = detail::Section(tbl, "policy")) {
detail::GetStr(pol, "path", policy_path);
@@ -272,14 +268,14 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
detail::GetDoubleArr(pol, "kp", policy_kp);
detail::GetDoubleArr(pol, "kd", policy_kd);
if (policy_joint_order.empty()) policy_joint_order = joint_names;
if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order 长度必须等于关节数量");
if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order length must equal joint count");
for (const auto& n : policy_joint_order) {
if (IndexOf(n) < 0) return fail("[policy].joint_order 里的 " + n + " 不在 joint_names 里");
if (IndexOf(n) < 0) return fail("[policy].joint_order entry " + n + " not in joint_names");
}
action_dim = dof;
for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) {
if (int(arr->size()) != action_dim) {
return fail("[policy] 的 action_scale/default_joint_pos/kp/kd 长度必须等于关节数量");
return fail("[policy] action_scale/default_joint_pos/kp/kd length must equal joint count");
}
}
obs_dim = 9 + 3 * action_dim; // 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角 + 关节速度 + 上一步动作
@@ -293,7 +289,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
try {
gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{});
} catch (const std::exception&) {
return fail("[gait.policies] 的键必须能转成整数步态值");
return fail("[gait.policies] key must convert to an integer gait value");
}
}
}
@@ -328,7 +324,7 @@ inline std::string DeployConfig::Summary() const {
for (size_t i = 0; i < legs.size(); ++i) {
const double sw = SwingAngleForHeight(stand_height, int(i));
const double li = SwingAngleForHeight(lie_height, int(i));
snprintf(buf, sizeof(buf), "[config] %s splay=%+.0f 站立摆臂=%+.1f° 趴下=%+.1f°\n",
snprintf(buf, sizeof(buf), "[config] %s splay=%+.0f stand swing=%+.1f deg lie down=%+.1f deg\n",
legs[i].name.c_str(), legs[i].splay_sign, sw * 180. / M_PI, li * 180. / M_PI);
s += buf;
}
src/invicta_deploy/include/types/common_types.h
+3
View File
@@ -96,6 +96,9 @@ struct UserCommand {
float reserved_scale;
uint32_t gait = 0; // 请求/生效的步态（协议 GaitParam），0 = 未请求
uint8_t usage_mode = 0; // 协议使用模式：0 常规 / 1 导航 / 2 辅助
// 控制器 → 上报通道：SafeController 每周期写入内部错误位掩码，
// MotionStateReporter 读它填 MotionState.error_code（位定义见 invicta_msgs/error_code.h）
uint32_t robot_error_code = 0;
};
enum FromType {
src/invicta_deploy/include/utils/motion_state_feedback.hpp
+3 -15
View File
@@ -23,14 +23,6 @@ private:
Vec3f cmd_vel_;
Vec3f max_vel_;
union{
struct{
uint32_t joint_pos_limit : 1;
uint32_t posture_limit : 1;
uint32_t reserved : 30;
}error_code_bit_;
uint32_t error_code_;
};
public:
MotionStateFeedback(/* args */){
memset(this, 0, sizeof(MotionStateFeedback));
@@ -54,13 +46,9 @@ public:
}
}
void ClearMotionError(){error_code_ = 0;}
void SetJointPosLimitError(){
error_code_bit_.joint_pos_limit = 1;
}
void SetPostureLimitError(){
error_code_bit_.posture_limit = 1;
}
// 错误码不再从这里走出去：内部错误位掩码的定义与展开见 invicta_msgs/error_code.h，
// 由 SafeController 每周期写入 UserCommand::robot_error_code，
// MotionStateReporter 读它填 MotionState.error_code（remote 再映射成协议 ErrorList）。
inline uint8_t GetCurrentState(){return current_state_;}
inline uint32_t GetCurrentGait(){return current_gait_;}
src/invicta_deploy/include/utils/safe_controller.hpp
+19 -20
View File
@@ -36,9 +36,6 @@ private:
std::thread judge_thread_;
int run_cnt_ = 0;
double last_imu_ts_, imu_check_time_;
bool imu_check_flag_ = true;
double ri_check_time_, last_ri_ts_;
VecXf last_joint_pos_, last_joint_vel_, last_joint_tau_;
std::vector<int> joint_data_same_cnt_;
@@ -60,27 +57,24 @@ private:
std::map<uint8_t, uint16_t> error_driver_map_;
uint32_t joint_update_state_;
/**
* IMU 检查。两件事分开：
* ① 解算结果含 NaN → 返回 false，调用方置 safe_control_mode（急停语义）
* ② 断流/姿态不可信 → 只置 imu_update_overtime 错误位，**不改 safe_control_mode**：
* RL 退出要走「正常切出→趴下」（RLControlState::GetNextStateName），
* 而趴下状态自己会在 safe_control_mode != 0 时转阻尼（liedown_state），
* 所以这里一旦置 mode 就会把受控下山打断成就地垮下来。
* 连续性判定（阈值 rl_imu_timeout_s / 恢复 rl_imu_recover_s）由 RobotInterface 提供。
*/
bool IsImuDataNormal() {
double current_imu_ts_ = ri_ptr_->GetImuTimestamp();
Vec3f rpy = ri_ptr_->GetImuRpy();
Vec3f acc = ri_ptr_->GetImuAcc();
Vec3f omg = ri_ptr_->GetImuOmega();
if (imu_check_flag_) {
last_imu_ts_ = current_imu_ts_;
imu_check_time_ = current_time_;
imu_check_flag_ = false;
}
robot_error_state_.imu_update_overtime = ri_ptr_->IsImuHealthy() ? 0 : 1;
bool res = true;
robot_error_state_.imu_update_overtime = 0;
if (current_imu_ts_ != last_imu_ts_) {
last_imu_ts_ = current_imu_ts_;
imu_check_time_ = current_time_;
}
if (current_time_ - imu_check_time_ == 0.2) {
std::cout << "========== imu lag warning ========>> " << current_time_ - imu_check_time_ << std::endl;
}
robot_error_state_.imu_num_error = 0;
for (int i = 0; i < 3; ++i) {
if (std::isnan(rpy(i)) || std::isnan(acc(i)) || std::isnan(omg(i))) {
res = false;
@@ -199,7 +193,8 @@ private:
}
if (robot_error_state_.imu_update_overtime) {
std::cout << "IMU update overtime: no IMU data received since timestamp " << last_imu_ts_ << std::endl;
std::cout << "IMU update overtime: no usable IMU frame, age " << ri_ptr_->GetImuAgeSec() << " s"
<< std::endl;
}
if (robot_error_state_.command_heartbeat_overtime) {
@@ -216,7 +211,7 @@ private:
}
if (robot_error_state_.motor_heat_warn) {
std::cout << "Motor overheat warning: motor " << max_temp_idx_ << " reached " << max_temp_ << "°C"
std::cout << "Motor overheat warning: motor " << max_temp_idx_ << " reached " << max_temp_ << " C"
<< std::endl;
}
@@ -317,6 +312,10 @@ public:
std::cout << "IMU error!" << std::endl;
usr_cmd_->safe_control_mode = 2;
}
// 把内部错误位掩码交给上报通道（MotionStateReporter 读它填 MotionState.error_code）
usr_cmd_->robot_error_code = robot_error_state_.error_code;
if (last_error_code_ != robot_error_state_.error_code) {
if (robot_error_state_.error_code != 0) {
std::cout << "========== error_code ========>> " << std::hex << robot_error_state_.error_code
src/invicta_deploy/interface/motion_state_reporter.hpp
+2
View File
@@ -74,6 +74,8 @@ private:
msg.charge = 0; // 待硬件：充电状态
msg.hes = 0; // 待硬件：硬急停信号
msg.sleep = 0; // 待实现：休眠管理
// 内部错误位掩码（SafeController 每周期写入）；位定义见 invicta_msgs/error_code.h
msg.error_code = user_cmd_ ? user_cmd_->robot_error_code : 0u;
msg.version = version_;
pub_.publish(msg);
std::this_thread::sleep_until(next);
src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp
+75 -28
View File
@@ -1,15 +1,14 @@
/**
* @file invicta_interface.hpp
* @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题、标定全部来自 invicta.toml
* @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题来自 invicta_deploy.toml
*
* 硬件约定（方向 dir、零点 offset、单位）在本层一次性归一：
* 反馈：q_model = q_robot · dir + offset
* 指令：q_robot = (q_des − offset) · dir
* 上层的状态机与策略只看到「模型约定」下的量（rad / rad/s / Nm）。
* 本层只做**模型帧**的透传：/joint_states 与 /joint_commands 上都是模型量（rad / rad/s / Nm）。
* 硬件的方向 `dir` 与零点 `offset` 由 invicta_hardware 在 raw↔model 的唯一边界处施加
* （见 invicta_hardware/README.md「标定边界」），deploy 不再参与标定。
* 上层状态机与策略只看到模型约定下的量。
*
* 相比之前：删掉了 auto_calibrate / ±360° 回绕标定（那是 M20 三关节腿的绝对编码器问题，
* 本构型摆臂行程只有 ±80°，无回绕；标定统一由 invicta.toml 的 dir/offset 表达），
* 轮关节判定也从 i%4==3 改为按配置的 is_wheel。
* 本构型摆臂行程只有 ±80°，无回绕），也删掉了本地 dir/offset 换算（已上移到 hardware）。
*/
#pragma once
@@ -17,6 +16,7 @@
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <limits>
#include <string>
#include <unordered_map>
#include <vector>
@@ -35,7 +35,7 @@ public:
InvictaInterface(const std::string& robot_name, int dof_num)
: RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
if (dof_num_ != cfg_.dof) {
ROS_WARN("[InvictaInterface] 传入 dof=%d 与配置的 %d 不一致，按配置执行", dof_num_, cfg_.dof);
ROS_WARN("[InvictaInterface] passed dof=%d does not match configured %d, using config value", dof_num_, cfg_.dof);
}
joint_pos_ = VecXf::Zero(dof_num_);
joint_vel_ = VecXf::Zero(dof_num_);
@@ -59,12 +59,7 @@ public:
SendStartupSequence();
std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> "
<< cfg_.topic_joint_commands << std::endl;
std::cout << "[InvictaInterface] 关节标定（invicta.toml）: ";
for (int i = 0; i < dof_num_; ++i) {
std::cout << cfg_.joint_names[size_t(i)] << "(dir=" << cfg_.Joint(i).dir << ",off="
<< cfg_.Joint(i).offset_deg << "°) ";
}
std::cout << std::endl;
std::cout << "[InvictaInterface] model frame passthrough: dir/offset applied by invicta_hardware" << std::endl;
}
virtual void Stop() {}
@@ -78,6 +73,13 @@ public:
virtual Vec3f GetImuOmega() { return omega_body_; }
virtual VecXf GetContactForce() { return VecXf::Zero(4); }
virtual double GetImuTimestamp() { return imu_ts_; }
/// 距最近一帧**可用** IMU 的时长 [s]；从未收到过返回极大值（安全线程用，纯读）
virtual double GetImuAgeSec() {
if (!has_imu_valid_) return std::numeric_limits<double>::max();
return (ros::Time::now() - imu_valid_arrival_).toSec();
}
/// IMU 是否健康（控制线程用；状态由 RefreshRobotData() 每周期更新）
virtual bool IsImuHealthy() { return imu_healthy_; }
virtual std::vector<uint16_t> GetJointDataID() { return joint_data_id_; }
virtual std::vector<uint16_t> GetDriverStatusWord() { return driver_status_; }
@@ -93,9 +95,10 @@ public:
msg.kd.resize(size_t(dof_num_));
for (int i = 0; i < dof_num_; ++i) {
const auto& j = cfg_.Joint(i);
msg.position[i] = (input(i, 1) - Deg2Rad(float(j.offset_deg))) * j.dir;
msg.velocity[i] = input(i, 3) * j.dir;
msg.effort[i] = input(i, 4) * j.dir;
// 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
msg.position[i] = input(i, 1);
msg.velocity[i] = input(i, 3);
msg.effort[i] = input(i, 4);
msg.kp[i] = (cfg_.wheel_kp_zero && j.is_wheel) ? 0.f : input(i, 0);
msg.kd[i] = input(i, 2);
}
@@ -103,7 +106,8 @@ public:
cmd_pub_.publish(msg);
}
virtual void RefreshRobotData() {}
/// 每个控制周期由状态机调一次：刷新 IMU 连续性判定（断流判坏 + 恢复去抖，见 RefreshImuHealth）
virtual void RefreshRobotData() { RefreshImuHealth(); }
protected:
// ------------------------------------------------------------------ 启动序列
@@ -121,20 +125,29 @@ protected:
}
/// 使能时序：disable → error reset → enable；此后运行期恒为 motor control
/// ⚠ ROS1 的发布没有 latch：订阅端连接建立前发出去的消息会被直接丢掉。
/// 若 hardware 节点后启动（或本节点重启），一次性时序帧会整段丢失，
/// 所以这里先等订阅者出现再发。
void SendStartupSequence() {
for (int i = 0; i < 200 && cmd_pub_.getNumSubscribers() == 0 && ros::ok(); ++i) {
ros::Duration(0.01).sleep();
}
if (cmd_pub_.getNumSubscribers() == 0) {
std::cout << "[InvictaInterface] warning: joint_commands has no subscribers yet, startup sequence may be lost" << std::endl;
}
const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
for (uint16_t word : seq) {
PublishRawCommand(word);
ros::Duration(0.01).sleep();
}
std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)" << std::endl;
std::cout << "[InvictaInterface] startup control sequence complete (disable -> error_reset -> enable)" << std::endl;
}
// ------------------------------------------------------------------ 回调
void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) {
const size_t n = size_t(dof_num_);
if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 长度应为 %d，收到 pos=%zu vel=%zu eff=%zu，丢帧",
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s length should be %d, got pos=%zu vel=%zu eff=%zu, dropping frame",
cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
msg->effort.size());
return;
@@ -148,18 +161,18 @@ protected:
++unknown;
continue;
}
const auto& j = cfg_.Joint(i);
joint_pos_(i) = float(msg->position[k]) * j.dir + Deg2Rad(float(j.offset_deg));
joint_vel_(i) = float(msg->velocity[k]) * j.dir;
joint_tau_(i) = float(msg->effort[k]) * j.dir;
// 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
joint_pos_(i) = float(msg->position[k]);
joint_vel_(i) = float(msg->velocity[k]);
joint_tau_(i) = float(msg->effort[k]);
}
if (!by_name && !warned_name_mismatch_) {
warned_name_mismatch_ = true;
ROS_WARN("[InvictaInterface] %s 未按约定给出 %d 个 name，按数组下标解析",
ROS_WARN("[InvictaInterface] %s did not provide the agreed %d names, resolving by array index",
cfg_.topic_joint_states.c_str(), dof_num_);
}
if (unknown > 0) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 有 %d 个关节名不认识，已忽略",
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s has %d unknown joint names, ignored",
cfg_.topic_joint_states.c_str(), unknown);
}
@@ -174,11 +187,15 @@ protected:
}
void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
imu_arrival_ = ros::Time::now();
has_imu_ = true;
// orientation_covariance[0] == -1 表示「不提供姿态」
if (msg->orientation_covariance[0] == -1.0) {
// 帧在来但没有可用姿态：对 RL 而言等同断流（base_rpy 会停在旧值，不能当有效观测）
has_imu_valid_ = false;
if (!warned_imu_no_orientation_) {
warned_imu_no_orientation_ = true;
ROS_WARN("[InvictaInterface] %s 没有提供 orientation（covariance=-1），姿态观测无效",
ROS_WARN("[InvictaInterface] %s did not provide orientation (covariance=-1), attitude observation invalid",
cfg_.topic_imu.c_str());
}
return;
@@ -186,7 +203,8 @@ protected:
Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y),
float(msg->orientation.z));
if (q.norm() < 1e-6f) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 四元数模长近零，忽略该帧", cfg_.topic_imu.c_str());
has_imu_valid_ = false;
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s quaternion norm near zero, ignoring this frame", cfg_.topic_imu.c_str());
return;
}
q.normalize();
@@ -194,6 +212,29 @@ protected:
omega_body_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
acc_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
imu_ts_ = msg->header.stamp.toSec();
imu_valid_arrival_ = imu_arrival_;
has_imu_valid_ = true;
}
/**
* IMU 连续性判定（只在控制线程里由 RefreshRobotData() 调一次）：
* 判坏：最近一帧可用 IMU 的龄期 > rl_imu_timeout_s（或从未收到过、或帧不提供姿态）
* 恢复：断流之后需要连续 rl_imu_recover_s 的新鲜可用帧才重新判为健康（去抖）
* 之所以要恢复去抖：否则 IMU 抖动一帧就会立刻放行重新进 RL。
*/
void RefreshImuHealth() {
const ros::Time now = ros::Time::now();
const bool fresh = has_imu_valid_ && (now - imu_valid_arrival_).toSec() <= cfg_.rl_imu_timeout_s;
if (!fresh) {
imu_good_since_valid_ = false;
imu_healthy_ = false;
return;
}
if (!imu_good_since_valid_) {
imu_good_since_ = now;
imu_good_since_valid_ = true;
}
if ((now - imu_good_since_).toSec() >= cfg_.rl_imu_recover_s) imu_healthy_ = true;
}
protected:
@@ -202,6 +243,12 @@ protected:
std::unordered_map<std::string, int> name2slot_;
double ri_ts_ = 0., imu_ts_ = 0.;
// ---- IMU 连续性（见 RefreshImuHealth）----
ros::Time imu_arrival_, imu_valid_arrival_, imu_good_since_;
bool has_imu_ = false; ///< 是否收到过任何 IMU 帧（哪怕不提供姿态）
bool has_imu_valid_ = false; ///< 最近一帧是否提供了可用姿态
bool imu_good_since_valid_ = false, imu_healthy_ = false;
Vec3f omega_body_, rpy_, acc_;
VecXf joint_pos_, joint_vel_, joint_tau_;
uint16_t joint_data_frame_ = 0;
src/invicta_deploy/interface/robot/robot_interface.h
+12
View File
@@ -72,6 +72,18 @@ public:
/// IMU 数据时间戳 [s]
virtual double GetImuTimestamp() { return 0.; }
/// 距最近一帧**可用** IMU 的时长 [s]；从未收到过返回极大值
virtual double GetImuAgeSec() { return 0.; }
/**
* @brief IMU 是否健康：最近有新鲜、可用的姿态帧，且断流之后已连续恢复
*
* 用于两件事：RL 运行中失去 IMU 时走「正常切出→趴下」，
* 以及恢复之前拒绝重新进入 RL（见 rl_control_state / standup_state）。
* 无 IMU 数据源的实现返回 true（本构型 RL 必须要姿态，台架可关 rl_require_imu）。
*/
virtual bool IsImuHealthy() { return true; }
/**
* @brief 关节电机温度 [℃]
*
src/invicta_deploy/interface/user_command/ros_command_interface.hpp
+21 -21
View File
@@ -47,12 +47,12 @@ public:
void Start() override {
if (msfb_) msfb_->UpdateCurrentGait(uint32_t(default_gait_));
std::cout << "\n╔════════════════════════════════════════════════╗\n"
<< "║ INVICTA MOTION COMMAND LINK ║\n"
<< "╚════════════════════════════════════════════════╝\n"
<< " 指令来源: " << topic_ << "（由 invicta_remote 发布）\n"
<< " 默认步态: " << GaitName(uint32_t(default_gait_)) << "\n"
<< " 已支持步态: " << SupportedGaitList() << "\n\n";
std::cout << "\n+==================================================+\n"
<< "| INVICTA MOTION COMMAND LINK |\n"
<< "+==================================================+\n"
<< " Command source: " << topic_ << " (published by invicta_remote)\n"
<< " Default gait: " << GaitName(uint32_t(default_gait_)) << "\n"
<< " Supported gaits: " << SupportedGaitList() << "\n\n";
}
void Stop() override {}
@@ -61,7 +61,7 @@ public:
private:
std::string SupportedGaitList() const {
if (!loaded_gaits_ || loaded_gaits_->empty()) return "(无)";
if (!loaded_gaits_ || loaded_gaits_->empty()) return "(none)";
std::string s;
for (int gait : *loaded_gaits_) {
char buf[16];
@@ -83,39 +83,39 @@ private:
switch (int(target)) {
case kMotionStanding:
name = "站立";
name = "stand up";
next = uint8_t(RobotMotionState::StandingUp);
accepted = (cur == RobotMotionState::WaitingForStand || cur == RobotMotionState::LieDown);
break;
case kMotionJointDamping:
name = "关节阻尼";
name = "joint damping";
next = uint8_t(RobotMotionState::JointDamping);
accepted = true;
break;
case kMotionBootDamping:
name = "开机阻尼(按关节阻尼处理)";
name = "boot damping (handled as joint damping)";
next = uint8_t(RobotMotionState::JointDamping);
accepted = true;
break;
case kMotionLieDown:
name = "趴下";
name = "lie down";
next = uint8_t(RobotMotionState::LieDown);
accepted = (cur == RobotMotionState::StandingUp || cur == RobotMotionState::RLControlMode);
break;
case kMotionRlControl:
name = "RL控制";
name = "RL control";
next = uint8_t(RobotMotionState::RLControlMode);
accepted = (cur == RobotMotionState::StandingUp);
break;
case kMotionIdle:
// 状态机没有「直接回空闲」的路径（RL / 站立只能经阻尼或趴下退出），不作为请求
if (last_request_ != kMotionIdle) {
std::cout << "[MODE] 空闲(0)：不作为切换请求（退出 RL 请先切关节阻尼或趴下）" << std::endl;
std::cout << "[MODE] idle (0): not treated as a switch request (to exit RL, switch to joint damping or lie down first)" << std::endl;
}
last_request_ = kMotionIdle;
return false;
default:
std::cout << "[MODE] 未知目标状态 " << int(target) << "（来自 " << src << "）" << std::endl;
std::cout << "[MODE] unknown target state " << int(target) << " (from " << src << ")" << std::endl;
return false;
}
@@ -127,9 +127,9 @@ private:
if (accepted) {
usr_cmd_->target_mode = next;
if (last_request_ != target) std::cout << "[MODE] " << name << "（来自 " << src << "）" << std::endl;
if (last_request_ != target) std::cout << "[MODE] " << name << " (from " << src << ")" << std::endl;
} else if (last_request_ != target) {
std::cout << "[MODE] 拒绝 " << name << "（来自 " << src << "，当前状态 " << int(cur) << "）" << std::endl;
std::cout << "[MODE] reject " << name << " (from " << src << ", current state " << int(cur) << ")" << std::endl;
}
last_request_ = target;
return accepted;
@@ -145,14 +145,14 @@ private:
if (gait == 0 || gait == last_gait_request_) return;
// 只有"启动时真的加载了模型"的步态才接受；否则状态会上报未切换，而不是假成功
if (!loaded_gaits_ || loaded_gaits_->count(int(gait)) == 0) {
std::cout << "[GAIT] 拒绝 " << GaitName(gait) << "（来自 " << src
<< "）：该步态没有对应的 RL 模型（启动时未加载，见 gait/policy_map）" << std::endl;
std::cout << "[GAIT] reject " << GaitName(gait) << " (from " << src
<< "): this gait has no corresponding RL model (not loaded at startup, see gait/policy_map)" << std::endl;
last_gait_request_ = gait;
return;
}
usr_cmd_->gait = gait;
if (msfb_) msfb_->UpdateCurrentGait(gait);
std::cout << "[GAIT] " << GaitName(gait) << "（来自 " << src << "），实际生效" << std::endl;
std::cout << "[GAIT] " << GaitName(gait) << " (from " << src << "), now effective" << std::endl;
last_gait_request_ = gait;
}
@@ -160,7 +160,7 @@ private:
const std::string src = "invicta_remote";
if (!has_command_) {
has_command_ = true;
std::cout << "[MODE] 收到第一条运动指令（" << topic_ << "）" << std::endl;
std::cout << "[MODE] received first motion command (" << topic_ << ")" << std::endl;
}
// 轴指令：协议里是比例 [-1,1]，策略直接用这三个分量
@@ -171,7 +171,7 @@ private:
if (msg->usage_mode != usr_cmd_->usage_mode) {
usr_cmd_->usage_mode = msg->usage_mode;
std::cout << "[MODE] 使用模式 -> " << int(msg->usage_mode) << "（0 常规 / 1 导航 / 2 辅助）" << std::endl;
std::cout << "[MODE] usage mode -> " << int(msg->usage_mode) << " (0 normal / 1 navigation / 2 assist)" << std::endl;
}
RequestMotionState(msg->target_state, src);
src/invicta_deploy/main.cpp
+1 -1
View File
@@ -29,7 +29,7 @@ int main(int argc, char** argv) {
std::string err;
auto& cfg = invicta::Config();
if (!cfg.Load(config_path, &err)) {
ROS_FATAL("[invicta_deploy] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
ROS_FATAL("[invicta_deploy] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
return 1;
}
std::cout << cfg.Summary() << std::flush;
src/invicta_deploy/run_policy/invicta_policy_runner.hpp
+1 -1
View File
@@ -37,7 +37,7 @@ public:
action_dim_ = cfg_.action_dim;
obs_dim_ = cfg_.obs_dim;
if (action_dim_ <= 0 || obs_dim_ <= 0) {
throw std::runtime_error("策略维度为 0：配置未加载或 [policy] 段不完整");
throw std::runtime_error("policy dimension is 0: config not loaded or [policy] section incomplete");
}
robot2policy_idx_ = Permutation(cfg_.joint_names, cfg_.policy_joint_order);
src/invicta_deploy/scripts/smoke_test.sh
+2 -2
View File
@@ -16,8 +16,8 @@ echo "=== 2. 启动 roscore ==="
nohup roscore >/tmp/roscore.log 2>&1 &
sleep 8
echo "=== 3. 启动 invicta_hardware（骨架：发零位状态）==="
nohup roslaunch invicta_hardware invicta_hardware.launch >/tmp/hw.log 2>&1 &
echo "=== 3. 启动 invicta_hardware（require_can:=false：无 CAN 时只发零位状态）==="
nohup roslaunch invicta_hardware invicta_hardware.launch require_can:=false >/tmp/hw.log 2>&1 &
sleep 4
echo "--- 话题 ---"
src/invicta_deploy/state_machine/quadruped_wheel/idle_state.hpp
+1 -1
View File
@@ -3,7 +3,7 @@
* @brief 待机：按 invicta.toml 的逐关节限位检查本体数据，等指令
*
* 相比 M20：不再有「每腿 4 关节 / 下标 %4」的假设，限位改成逐关节查表；
* 左右/前后的差异已经在配置的标定（dir/offset）里归一。
* 左右/前后的镜像已经在 invicta_hardware 的标定（dir/offset）里归一。
*/
#pragma once
src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp
+1 -1
View File
@@ -88,7 +88,7 @@ public:
current_state_name_ = kIdle;
next_state_name_ = kIdle;
std::cout << "control period: " << cfg.control_period_ms << " ms, 默认模型: " << policy_path << std::endl;
std::cout << "control period: " << cfg.control_period_ms << " ms, default model: " << policy_path << std::endl;
// ---- 状态回报（供 invicta_remote 判前提 + 状态上报）----
state_reporter_ = std::make_shared<interface::MotionStateReporter>(
src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp
+2 -2
View File
@@ -34,13 +34,13 @@ public:
virtual void OnEnter() {
start_time_ = std::chrono::steady_clock::now();
std::cout << "[DAMPING] 进入关节阻尼，持续 " << invicta::Config().damping_hold_s << " s 后转入空闲"
std::cout << "[DAMPING] entering joint damping, duration " << invicta::Config().damping_hold_s << " s then transition to idle"
<< std::endl;
msfb_.UpdateCurrentState(RobotMotionState::JointDamping);
}
virtual void OnExit() {
std::cout << "[DAMPING] 结束（实际 " << Elapsed() << " s），转入空闲" << std::endl;
std::cout << "[DAMPING] exit (actual " << Elapsed() << " s), transition to idle" << std::endl;
}
virtual void Run() { ri_ptr_->SetJointCommand(joint_cmd_); }
src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp
+1 -1
View File
@@ -44,7 +44,7 @@ public:
joint_cmd_(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel);
char buf[160];
snprintf(buf, sizeof(buf), "[GAIT] 趴下: %s 目标高度 %.3f m → 摆臂 %+.1f°\n", cfg.legs[l].name.c_str(),
snprintf(buf, sizeof(buf), "[GAIT] lie down: %s target height %.3f m -> swing %+.1f deg\n", cfg.legs[l].name.c_str(),
cfg.lie_height, target_swing_(int(l)) * 180. / M_PI);
std::cout << buf;
}
src/invicta_deploy/state_machine/quadruped_wheel/rl_control_state.hpp
+31 -11
View File
@@ -48,6 +48,7 @@ private:
std::thread run_policy_thread_;
bool start_flag_ = true;
std::atomic<bool> policy_output_bad_{false}; ///< 推理输出异常（NaN/Inf）
bool warned_imu_exit_ = false; ///< IMU 丢失退出 RL 的日志只打一次
Eigen::MatrixXf acc_rot = Eigen::MatrixXf::Zero(20, 3);
int acc_rot_count = 0;
@@ -88,8 +89,8 @@ private:
auto next = PolicyForGait(gait, &switched);
if (switched) {
next->OnEnter(); // 清 last_action / run_cnt，避免跨模型残留
std::cout << "[GAIT] 切换模型 -> " << invicta_msgs::GaitName(gait)
<< (gait_runners_.count(int(gait)) ? "（已注册模型）" : "（默认模型）") << std::endl;
std::cout << "[GAIT] switch model -> " << invicta_msgs::GaitName(gait)
<< (gait_runners_.count(int(gait)) ? " (registered model)" : " (default model)") << std::endl;
}
active_policy_ = next;
active_gait_ = int(gait);
@@ -101,7 +102,7 @@ private:
MatXf res = ra.ConvertToMat();
if (!res.allFinite()) {
policy_output_bad_ = true;
std::cout << "[RL] 推理输出含 NaN/Inf，停止下发并转入关节阻尼" << std::endl;
std::cout << "[RL] policy output contains NaN/Inf, stopping commands and switching to joint damping" << std::endl;
} else {
ri_ptr_->SetJointCommand(res);
}
@@ -123,13 +124,13 @@ public:
try {
gait_paths_[std::stoi(kv.first)] = kv.second;
} catch (const std::exception&) {
ROS_WARN("[RLControlState] 步态表键 '%s' 不是整数，已忽略", kv.first.c_str());
ROS_WARN("[RLControlState] gait map key '%s' is not an integer, ignored", kv.first.c_str());
}
}
default_policy_ = std::make_shared<InvictaPolicyRunner>("invicta_policy", policy_path);
active_policy_ = default_policy_;
std::cout << "[GAIT] 默认模型: " << policy_path << std::endl;
std::cout << "[GAIT] default model: " << policy_path << std::endl;
// 默认步态 = 默认模型
if (loaded_gaits_) loaded_gaits_->insert(default_gait_);
@@ -138,14 +139,14 @@ public:
for (const auto& kv : gait_paths_) {
if (kv.first == default_gait_) continue; // 默认步态已在上面处理
if (kv.second.empty()) {
std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first))
<< " 未提供模型路径，该步态不可用" << std::endl;
std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first))
<< " has no model path, gait unavailable" << std::endl;
continue;
}
if (kv.second == policy_path) {
gait_runners_[kv.first] = default_policy_;
if (loaded_gaits_) loaded_gaits_->insert(kv.first);
std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) << " 复用默认模型"
std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first)) << " reuses default model"
<< std::endl;
continue;
}
@@ -153,11 +154,11 @@ public:
gait_runners_[kv.first] = std::make_shared<InvictaPolicyRunner>("gait_" + std::to_string(kv.first),
kv.second);
if (loaded_gaits_) loaded_gaits_->insert(kv.first);
std::cout << "[GAIT] 预加载步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) << " 模型 "
std::cout << "[GAIT] preload gait " << invicta_msgs::GaitName(uint32_t(kv.first)) << " model "
<< kv.second << std::endl;
} catch (const std::exception& e) {
std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first))
<< " 模型加载失败（该步态将不可用）：" << e.what() << std::endl;
std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first))
<< " failed to load model (gait unavailable): " << e.what() << std::endl;
}
}
policy_output_bad_ = false;
@@ -168,6 +169,7 @@ public:
virtual void OnEnter() {
policy_output_bad_ = false;
warned_imu_exit_ = false;
active_gait_ = -1; // 强制按当前步态重选模型
state_run_cnt_ = -1;
start_flag_ = true;
@@ -198,6 +200,24 @@ public:
virtual StateName GetNextStateName() {
if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) return StateName::kJointDamping;
// IMU 断流 / 姿态不可信：走「正常切出」——交给趴下状态做受控下降。
// 刻意不用 safe_control_mode 表达，否则 kLieDown 自己会立刻转阻尼（见 liedown_state），
// 变成就地垮下来，正是这里要避免的。恢复之前 RL 也进不来（见 StandUpState::ReadyForRl）。
if (invicta::Config().rl_require_imu && !ri_ptr_->IsImuHealthy()) {
if (!warned_imu_exit_) {
warned_imu_exit_ = true;
const double age = ri_ptr_->GetImuAgeSec();
if (age > 1e6) {
std::cout << "[RL] no usable IMU frame yet: leaving RL via normal exit -> lie down" << std::endl;
} else {
std::cout << "[RL] IMU lost (age " << age << " s): leaving RL via normal exit -> lie down"
<< std::endl;
}
}
return StateName::kLieDown;
}
if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::LieDown))
return StateName::kLieDown;
return StateName::kRLControl;
src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp
+15 -10
View File
@@ -7,7 +7,7 @@
* 摆臂倒向由 splay_sign 决定：本题要求趴下/收腿时「前腿向前、后腿向后」（与 M20 相反）。
*
* 与上游 M20 的差别：不再有「每腿 4 关节、i%4==3 是轮、外摆钉在 0、后腿髋膝取反」这些假设，
* 左右/前后的镜像已经在配置的标定（dir/offset）里归一。
* 左右/前后的镜像已经在 invicta_hardware 的标定（dir/offset）里归一。
*/
#pragma once
@@ -58,7 +58,7 @@ public:
joint_cmd_(cfg.legs[l].wheel_index, 0) = 0.f; // 轮 kp 恒 0
char buf[160];
snprintf(buf, sizeof(buf), "[GAIT] 起立: %s 收腿(%.3f m) %+.1f° → 站立(%.3f m) %+.1f°\n",
snprintf(buf, sizeof(buf), "[GAIT] stand up: %s tuck leg(%.3f m) %+.1f deg -> stand(%.3f m) %+.1f deg\n",
cfg.legs[l].name.c_str(), cfg.pre_height, tuck_swing_(int(l)) * 180. / M_PI, cfg.stand_height,
stand_swing_(int(l)) * 180. / M_PI);
std::cout << buf;
@@ -134,12 +134,12 @@ public:
bool ReadyForRl() {
const auto& cfg = invicta::Config();
if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) {
LogNotReady("安全标志非 0");
LogNotReady("safe flag is not 0");
return false;
}
for (int i = 0; i < int(current_joint_pos_.size()); ++i) {
if (std::isnan(current_joint_pos_(i)) || std::isnan(current_joint_vel_(i))) {
LogNotReady("关节数据含 NaN");
LogNotReady("joint data contains NaN");
return false;
}
}
@@ -147,11 +147,11 @@ public:
for (size_t l = 0; l < cfg.legs.size(); ++l) {
const int si = cfg.legs[l].swing_index;
if (std::fabs(current_joint_pos_(si) - stand_pose_(si)) > cfg.rl_joint_tolerance) {
LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 未到站立位姿");
LogNotReady("joint " + cfg.joint_names[size_t(si)] + " not at stand pose");
return false;
}
if (std::fabs(current_joint_vel_(si)) > cfg.rl_velocity_tolerance) {
LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 速度过大");
LogNotReady("joint " + cfg.joint_names[size_t(si)] + " velocity too large");
return false;
}
}
@@ -161,12 +161,17 @@ public:
const Vec3f acc = ri_ptr_->GetImuAcc();
for (int i = 0; i < 3; ++i) {
if (std::isnan(rpy(i)) || std::isnan(acc(i))) {
LogNotReady("IMU 数据含 NaN");
LogNotReady("IMU data contains NaN");
return false;
}
}
if (acc.norm() < 0.1f * gravity || acc.norm() > 3.0f * gravity) {
LogNotReady("IMU 加速度异常");
LogNotReady("IMU acceleration abnormal");
return false;
}
// 连续性：断流或刚恢复还没稳够时间，都不许进 RL（恢复去抖见 InvictaInterface::RefreshImuHealth）
if (!ri_ptr_->IsImuHealthy()) {
LogNotReady("IMU unhealthy: no fresh usable frame, or still recovering");
return false;
}
}
@@ -176,7 +181,7 @@ public:
void LogNotReady(const std::string& why) {
if (run_time_ - last_ready_log_ < 1.0) return;
last_ready_log_ = run_time_;
std::cout << "[RL] 暂不进入 RL：" << why << std::endl;
std::cout << "[RL] not entering RL yet: " << why << std::endl;
}
virtual bool LoseControlJudge() {
@@ -200,7 +205,7 @@ public:
if (elapsed >= ready_time + cfg.rl_enter_delay && ReadyForRl()) {
if (!rl_enter_logged_) {
rl_enter_logged_ = true;
std::cout << "[RL] 站立完成且自检通过，进入 RL 控制" << std::endl;
std::cout << "[RL] stand up complete and self-check passed, entering RL control" << std::endl;
}
return StateName::kRLControl;
}
src/invicta_deploy/third_party/tomlplusplus/README.md
Vendored
+13
View File
@@ -0,0 +1,13 @@
# toml++ (vendored)
这三个节点各自用到的 TOML 解析库，以 **single-header / header-only** 方式编进节点
（无 `find_package`，目标机也不需要额外 `.so`）。
- 上游：https://github.com/marzer/tomlplusplus
- 版本：v3.4.0（单头文件 `toml.hpp`）
- 取用：`toml.hpp`（amalgamated single header）。单头版本本身就是 header-only——
「drop it in, there is no step two」——文件内部在未定义时自动 `#define TOML_HEADER_ONLY 1`，
**不需要任何编译宏**。放在 `toml++/toml.hpp` 是为了匹配上游 `#include <toml++/toml.hpp>`
的路径，源码无需改动。
升级：下载新的 `toml.hpp` 覆盖本目录下 `toml++/toml.hpp` 即可（三个包各一份，保持同版本）。
src/invicta_deploy/third_party/tomlplusplus/toml++/toml.hpp
Vendored
+17897
View File
File diff suppressed because it is too large Load Diff
src/invicta_hardware/CMakeLists.txt
+6 -6
View File
@@ -21,12 +21,9 @@ catkin_package(
CATKIN_DEPENDS roscpp roslib sensor_msgs geometry_msgs invicta_msgs
)
# TOML 解析库：系统安装（见 README「依赖」），CMake 里体现为 required
find_package(tomlplusplus REQUIRED)
get_target_property(TOMLPLUSPLUS_INCLUDE_DIR tomlplusplus::tomlplusplus INTERFACE_INCLUDE_DIRECTORIES)
include_directories(${TOMLPLUSPLUS_INCLUDE_DIR})
# 以 header-only 方式使用：实现编进本节点，避免引入运行时动态库依赖（目标机无需部署 .so）
add_definitions(-DTOML_HEADER_ONLY=1)
# TOML 解析库：single-header，vendored 在 third_party/tomlplusplus/toml++/toml.hpp
# 单头版本本身就是 header-only（自带 TOML_HEADER_ONLY），实现直接编进本节点：无 find_package、无运行时库依赖
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/tomlplusplus)
include_directories(
include
@@ -42,6 +39,9 @@ endif()
add_executable(invicta_hardware_node
src/invicta_hardware_node.cpp
include/invicta_hardware/can_bus.h
include/invicta_hardware/joint_module_protocol.h
include/invicta_hardware/hardware_bridge.h
${NODE_CONFIG_HEADER}
)
src/invicta_hardware/README.md
+75 -26
View File
@@ -1,58 +1,107 @@
# invicta_hardware
关节/IMU 数据源与关节指令执行端。**当前是骨架**：结构、话题、参数都齐了，只有 `ReadHardware()` / `WriteHardware()` 是待实现的 TODO。
关节数据源与关节指令执行端。已接 **SocketCAN + 智身关节模组 CANFD 协议**（协议见
[docs/zhishen_joint_module_canfd.md](../../docs/zhishen_joint_module_canfd.md)）。
## 它负责什么
以 `publish_rate_hz`（默认 200 Hz）为一个周期，循环做三件事：
1. `ReadHardware()` — 从电机/IMU 读回状态（TODO）
2. `PublishState()` — 发布 `sensor_msgs/JointState` 与 `sensor_msgs/Imu`（已实现）
3. `WriteHardware()` — 把最新指令下发到电机（TODO，含长度与有限性校验、控制字处理）
1. `ReadHardware()` — 非阻塞收各路总线上的 12 字节反馈帧，更新关节位置/速度/力矩、温度、状态码、电压
2. `PublishState()` — 发 `sensor_msgs/JointState`
3. `WriteHardware()` — 使能后每周期发 MIT 运行帧（p / v / kp / kd / tau）
上游指令断流时（`command_timeout_s`，默认 100 ms）自动转**阻尼帧**（kp=0、速度/力矩 0、只留 `timeout_*_kd`），
避免 deploy 挂掉/卡住后本节点一直重发最后一帧。指令恢复后自动退出阻尼。
另按 `diagnostics_rate_hz`（默认 10 Hz）发 `invicta_msgs/JointDiagnostics`（温度/状态/电压）。
**反馈不需要主动查询**：模组对每条指令都回一帧状态（手册 §3.2），所以"发了指令就有反馈"，读回和写入天然同频。
| 话题 | 方向 | 消息 |
|---|---|---|
| `/joint_states` | 发布 | `sensor_msgs/JointState` |
| `/imu/data` | 发布 | `sensor_msgs/Imu` |
| `/joint_diagnostics` | 发布 | `invicta_msgs/JointDiagnostics`（默认 10 Hz） |
| `/joint_commands` | 订阅 | `invicta_msgs/JointCommandArray` |
## 控制字 → CAN 帧
| `control_word` | 实际动作 |
|---|---|
| `kDisable`(1) | 每关节块 `0xFF×9 + 0xFD`，线圈断开，之后不再发运行帧 |
| `kEnable`(2) | `0xFF×9 + 0xFC`，按 `enable_frames`/`enable_interval_s` 连续刷几帧 |
| `kMotorControl`(3) | 发运行块；**未使能时拒发**（手册 §9.1 要求先使能再运行） |
| `kErrorReset`(4) | 本协议没有独立清错指令（手册 §6：故障后靠重新使能/重启），按"重发使能"处理 |
| `kGetStatus`(5) | 不需要发帧：状态随每条指令自动返回 |
| `kSetZero`(6) | `0xFF×9 + 0xEF`：把当前机械位置写成模组零位。**只作用于指令 `name` 选中的关节**，且只发摆臂（轮毂不支持，手册 §4.4）。之后该位姿反馈角≈0，请把对应 `offset_deg` 置 0 |
### 关节选择（部分接入 / 单腿调试）
`JointCommandArray` 的 **`name` 列表就是本次要控制的关节子集**：
- `name` 为空 → 全部已配置关节（deploy 一直填全部，行为不变）。
- `name` 是子集 → **只驱动/置零这些关节**；不在子集里的关节会被写入一个**零力矩「自由」块**（`kp=kd=0`、`tau=0`），既不驱动也不保持，也不参与置零。
- ⚠ 这里**不能**简单地把未选中的块留成全 0：按协议 `(0,0,0,0,0)` 会解码成 `P_MIN/V_MIN/T_MIN`，其中 `tau=T_MIN`（摆臂 −100 N·m）是危险的前馈力矩。所以显式写零力矩块。
- 数组长度（`position`/`velocity`/`effort`/`kp`/`kd`）**仍必须是全部关节数**，只有 `name` 决定选择范围。
- `kEnable`/`kDisable`/`kErrorReset` 仍作用于全部已配置关节（总线级动作）；只有运行帧与 `kSetZero` 按 `name` 选择。
- 控制字只在**变化**时落地：要对另一个子集重新置零，先把控制字切到别的值（如 `kGetStatus`）再切回 `kSetZero`。
这样台架上只接一条腿也能单独控制、单独置零，不必把电机全接上。
同一路总线上最多 4 个关节（CANID 1~4），合成一帧下发：Byte0-1 是 MSGID `0x0100`，每个 CANID 占
10 字节，块起点 `2 + 10×(CANID−1)`，长度按手册裁剪表（本机 4 个 ID → 48 字节）。
## 约定（最重要的部分）
**硬件约定（方向、零点、单位、CAN 帧格式）在本节点内消化，话题上流动的必须是 policy 约定下的量**：
- 关节角 `[rad]`、角速度 `[rad/s]`、力矩 `[Nm]`，按 `joint_names` 的固定顺序。
- IMU：姿态四元数（**弧度**）、角速度 `[rad/s]`（机体系）、线加速度 `[m/s²]`（机体系）。
- ⚠️ 上游 M20 的 `/IMU_DATA` 里 `rpy` 是**度**，不要照搬；本工程统一弧度。四元数能避免单位歧义，这也是选 `sensor_msgs/Imu` 的原因之一。
- 姿态确实拿不到时，必须把 `orientation_covariance[0]` 置 `-1`（而不是发单位四元数假装有效），deploy 侧会据此丢弃该帧。
- **边界和限幅是两件事**：`[motor.*]` 的 p/v/t 是**归一化边界**——只决定 CAN 帧里 `0~65535` 映射到多少物理量（编码/解码必须用同一套，手册 §5/§9.6），不是运行限幅；`[joint_limits]` 才是唯一实际生效的**限幅**。下发顺序是「先按限幅夹紧 → 再归一化编码」。加载时会校验限幅不超过边界，超了直接报错，不靠编码饱和兜着。
- **kp/kd 不是策略算的**，来自 deploy 的 `[policy].kp/kd` 与 `[leg.gains]`（用户配置）。越界时本节点会夹到 `[0, kp_max]` / `[0, kd_max]` 并打警告——那种日志说明配置写错了，不要靠饱和兜着。
- 前馈力矩按 `[joint_limits].torque_limit`（默认取厂家峰值扭矩）夹紧，超过也告警。
- **本节点是 raw↔model 的唯一边界**：话题上收发的都是**模型帧**（模型坐标、rad）。
`[[legs]]` 的 `dir`/`offset_deg` 由本节点施加（`q_model = q_motor·dir + offset`，反向 `q_motor = (q_model − offset)·dir`；速度/力矩只乘 `dir`，不含 offset）。
`invicta_deploy` 侧不再做任何标定，只处理模型量。这样 `[joint_limits]`（模型帧限幅）与被限幅的指令同帧，不会错位；任何看 `/joint_states` 的消费者拿到的也是有物理意义的角度。
- IMU **不由本节点发布**。之前这里发的是硬编码的单位四元数 + 零角速度/零加速度（假数据），还把 `orientation_covariance[0]` 标成 `0`（有效），会让下游把「永远水平静止」当真值——已删除。姿态/角速度由独立的 IMU 节点发布到 `imu/data`（话题名见 deploy/remote 的 `[topics].imu`），契约见 [invicta_deploy/README.md](../invicta_deploy/README.md)。
- 温度取自反馈帧 Byte8：`T = raw − 40`（0~255 ↔ −40~215 ℃）；电压 Byte10 直接是伏。
- 状态码 Byte9：bit0 使能，bit1~6 故障（过压/过流/过温/过速/编码器/欠压）。故障位出现时本节点会打 ERROR 日志，并原样放进 `/joint_diagnostics`。
方向/零点换算参考（M20 原车，角度制）：`joint_dir = [1,1,-1,1, 1,-1,1,-1, -1,1,-1,1, -1,-1,1,-1]`，`joint_offset_deg = [-25,-131,160,0, 25,-131,160,0, -25,131,-160,0, 25,131,-160,0]`。新机器人用自己的值。
## 总线前置条件
**同一套换算只能在一边施加**：本节点做了，`invicta_deploy` 的 `calibration/*` 就保持恒等（默认如此）；反之则要把参数填上并打开 `auto_calibrate`。
位率由系统配置，本节点只负责 socket 收发：
## 待实现清单
```bash
sudo ip link set can0 up type can bitrate 1000000 dbitrate 5000000 fd on
sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
```
- [ ] `ReadHardware()`：SocketCAN（或你的总线）读回 16 关节的 position/velocity/effort + IMU，换算到 policy 约定。读失败返回 `false`，保留上一帧值，**不要发 NaN**。
- [ ] `WriteHardware()`：按 `control_word` 落实使能/失能/清错，把目标角/角速度/前馈力矩/`kp`/`kd` 组装成帧下发；`tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff` 由驱动器算还是本节点算，取决于关节模组协议，选一种并写清楚。
- [ ] 上电顺序与安全：使能前的自检、通信超时（多久没指令就卸力）、过温/过流保护。注意 deploy 在 `idle` 状态也会以 200 Hz 发零增益指令，所以「按控制字判断是否使能」比「按有无指令判断」更稳。
- [ ] 关节顺序与 `invicta_deploy` 共用同一份配置里的 `joint_names`，不会不一致。
`[motor.can].bitrate/data_bitrate` 只用于出错时的提示，不能透过 SocketCAN 设置。
## 无硬件调试开关
| 位置 | 默认 | 作用 |
|---|---|---|
| `[hardware].require_can` | `true` | CAN 打不开就退出，避免"以为在控车其实没连上" |
| `roslaunch ... require_can:=false` | — | 无 CAN 也启动：照常发零位状态、缓存指令，但不下发电机指令 |
`src/invicta_deploy/scripts/smoke_test.sh` 用的就是 `require_can:=false`，因此容器/podman 里没有 CAN 也能跑通。
## 参数（本节点自己的 `config/invicta_hardware.toml`）
| TOML 段 | 内容 |
|---|---|
| `[robot].joint_names` | 关节名与顺序（8 个：`fl/fr/hl/hr × swing/wheel`） |
| `[joint_limits]` | 逐关节限位（下发前自检用） |
| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `dir`、`offset_deg`、`bus`、`canid` |
| `[motor.joint]` / `[motor.wheel]` | 电机量程：`p/v/t/kp/kd` 上下限（P65MAXS / W190S） |
| `[motor.can]` | 总线名、仲裁 ID、1M/5M 比特率、上电使能帧数与间隔 |
| `[motor.gains]` | 台架脚本当前增益（进 RL 前以 `invicta_deploy` 的 `[leg.gains]`/策略为准） |
| `[topics]` `[hardware]` | 本节点话题、发布频率、IMU frame |
| `[joint_limits]` | 人为限幅（模型帧）：位置 `[rad]`、速度 `[rad/s]`、前馈力矩 `[N·m]`（下发前夹紧） |
| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `dir`、`offset_deg`（硬件标定，本节点施加）、`bus`、`canid` |
| `[motor.joint]` / `[motor.wheel]` | 归一化边界（0~65535 ↔ 物理量），编码/解码必须一致（P65MAX-S / W190-S） |
| `[motor.can]` | 总线名、仲裁 ID、1M/5M、上电使能帧数与间隔 |
| `[motor.gains]` | `bench_*` 仅作参考（实际 kp/kd 由指令给）；`timeout_*_kd` 是看门狗阻尼 |
| `[topics]` `[hardware]` | 本节点话题、200 Hz 状态频率、10 Hz 诊断频率、`require_can`、`command_timeout_s` |
启动时打印关节表、每条腿的总线/CANID 与电机量程，便于接线核对。
## 运行
```bash
roslaunch invicta_hardware invicta_hardware.launch
roslaunch invicta_hardware invicta_hardware.launch # 真机
roslaunch invicta_hardware invicta_hardware.launch require_can:=false # 无 CAN 调试
```
骨架状态下会打印告警，并且只发布零位状态、只缓存指令，**不会驱动电机**。
自查：`rostopic echo /joint_diagnostics` 看温度/状态；`candump can0` 看反馈帧的仲裁 ID（当前按
"反馈使用关节自身 CANID" 实现，若模组回帧 ID 不同需改 `ReadHardware` 的匹配）。
src/invicta_hardware/config/invicta_hardware.toml
+43 -12
View File
@@ -1,5 +1,5 @@
# =============================================================================
# invicta_hardware（关节/IMU 数据源 + 指令执行端）自己的配置
# invicta_hardware（关节数据源 + 指令执行端）自己的配置
# 只放本节点需要的参数：关节表与硬件标定、话题、发布频率、电机与 CAN
# 运动学/策略/步态等属于 invicta_deploy，见 invicta_deploy/config/invicta_deploy.toml
# =============================================================================
@@ -13,18 +13,29 @@ joint_names = [
"hr_swing", "hr_wheel",
]
# ------------------------------------------------------------------ 关节限位
# 顺序与 joint_names 一致；轮关节的 lower/upper 填 ±100 表示不限（用 vel_limit 约束）
# 本节点实现 I/O 时用它做下发前的自检
# ------------------------------------------------------------------ 关节限位（唯一实际生效的限幅）
# 顺序与 joint_names 一致。这里才是「限幅」：机械/使用上的可用范围，下发前按它夹紧。
# 限幅与话题上的指令/反馈同为**模型帧**（本节点是 raw↔model 的唯一边界，见 [[legs]]），
# 所以夹紧窗口天然自洽（不会出现「拿模型帧限幅去夹电机帧指令」）。
# 与 [motor.*] 的关系：那里是「归一化边界」，只决定 0~65535 映射到多少物理量，不是限幅。
# 加载时会校验本节限幅不超过归一化边界；超了直接报错，不靠编码饱和悄悄压回去。
# 轮关节：轮毂不支持位置控制（手册 §4.4），lower/upper 是占位值，只用 vel_limit；
# 轮 vel_limit = 100 [rad/s] 是人为限幅：略高于额定速度（额定功率/扭矩推得 81.2 rad/s）。
[joint_limits]
lower = [-1.40, -100.0, -1.40, -100.0, -1.40, -100.0, -1.40, -100.0] # [rad]
upper = [ 1.40, 100.0, 1.40, 100.0, 1.40, 100.0, 1.40, 100.0] # [rad]
vel_limit = [ 22.40, 100.0, 22.40, 100.0, 22.40, 100.0, 22.40, 100.0] # [rad/s]
# 前馈力矩限幅 [N·m]：取厂家参数页的峰值扭矩（摆臂 P65MAX-S 75、轮 W190-S 28.68），
# 比驱动器 T_MAX（100 / 50）更保守。超了就夹紧并告警。
torque_limit = [75.0, 28.68, 75.0, 28.68, 75.0, 28.68, 75.0, 28.68]
# ------------------------------------------------------------------ 每腿的硬件标定与总线
# 指令换算：q_motor = (q_model − offset_deg) · dir；反馈反向。
# 本节点是 raw↔model 的唯一边界，收到/发出的都是模型帧；本节的 dir/offset_deg 由本节点施加：
# 指令换算：q_motor = (q_model − offset_deg) · dir；反馈反向 q_model = q_motor · dir + offset_deg。
# velocity/torque 只乘 dir（不含 offset）。
# dir ：电机正方向与模型正方向的关系（±1，左右镜像、前后镜像都在这里）
# offset_deg：机械零点偏置 [deg]（模型零位 = 摆臂竖直向下时读到的电机角）
# offset_deg：机械零点偏置 [deg]（模型零位 = 摆臂竖直向下时读到的电机角）；
# 用调试脚本的置零（0xEF，control_word=kSetZero）把当前位姿写成零位后可置 0
# bus/canid ：该关节挂在哪条 CANFD 总线、CANID 是多少
# 注意：TOML 里数组表之后的裸键会归到最后一个元素，本节必须放在 motor 段之前
[[legs]]
@@ -56,7 +67,10 @@ swing = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 2 }
wheel = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 4 }
# ------------------------------------------------------------------ 电机与总线
# 数值取自当前 CANFD 台架脚本（遥控 → can0/can1 双路），实现 ReadHardware/WriteHardware 时按此驱动
# p/v/t/kp/kd 是「归一化边界」（厂家手册 V1.2 §7，见 docs/zhishen_joint_module_canfd.md）：
# 它只决定 CAN 帧里 0~65535 映射到多少物理量，编码与解码必须用同一套（§5/§9.6），
# 不是运行限幅——限幅在 [joint_limits]。必须与模组实际配置一致。
# CAN 参数（总线名/仲裁 ID/波特率/使能帧）取自台架脚本。
[motor.joint] # 摆臂关节电机
model = "P65MAXS"
p_max = 12.5 # [rad]
@@ -72,8 +86,10 @@ kd_max = 5.0
model = "W190S"
p_max = 12.5
p_min = -12.5
v_max = 10.0 # [rad/s]
v_min = -10.0
v_max = 200.0 # [rad/s] 手册 §7 的 W190-S 阈值，编码/解码满量程
v_min = -200.0 # 注意：这不是工作上限（工作上限是 [joint_limits] 的 100）。
# 台架脚本曾把摇杆范围 ±10 当满量程，换算后轮速被放大 20×，
# 属用法错误，不要照搬
t_max = 50.0
t_min = -50.0
kp_max = 500.0
@@ -91,14 +107,29 @@ enable_interval_s = 0.02
bench_joint_kp = 150.0
bench_joint_kd = 2.0
bench_wheel_kd = 0.5
# 指令看门狗触发后的阻尼（kp=0、给定速度 0，只留 Kd 抵抗外力）
timeout_joint_kd = 4.0
timeout_wheel_kd = 1.0
# ------------------------------------------------------------------ 本节点的话题与节奏
[topics]
joint_states = "joint_states" # 发布 sensor_msgs/JointState
imu = "imu/data" # 发布 sensor_msgs/Imu
joint_commands = "joint_commands" # 订阅 invicta_msgs/JointCommandArray
joint_diagnostics = "joint_diagnostics" # 发布 invicta_msgs/JointDiagnostics（温度/状态/电压）
[hardware]
publish_rate_hz = 200.0 # 关节/IMU 状态发布频率（RL 侧控制环 200 Hz）
imu_frame_id = "base_link"
publish_rate_hz = 200.0 # 关节状态发布频率（RL 侧控制环 200 Hz）
diagnostics_rate_hz = 10.0 # 诊断量频率：CAN 反馈每条指令都有，但话题按低频发
command_queue = 1 # 指令只取最新一帧
# true：CAN 打不开就退出（真机默认，避免以为在控车其实没连上）
# false：没有 CAN 也启动，只发零位状态与指令缓存（调试 / 无硬件冒烟测试）
# launch 里可用 require_can:=false 覆盖，例如 smoke_test.sh
require_can = true
# 指令看门狗：超过这么久没收到 /joint_commands 就转阻尼（kp=0 + [motor.gains].timeout_*_kd），
# 避免上游进程挂掉后一直重发最后一帧。0 = 关闭。
command_timeout_s = 0.1
# ------------------------------------------------------------------ 总线前置条件（不在本节点里设置）
# 位率必须由系统配置好，本节点只负责 socket 收发：
# sudo ip link set can0 up type can bitrate 1000000 dbitrate 5000000 fd on
# sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
src/invicta_hardware/include/invicta_hardware/can_bus.h
+176
View File
@@ -0,0 +1,176 @@
/**
* @file can_bus.h
* @brief SocketCAN / CANFD 收发封装（Linux 原生接口，无第三方依赖）
*
* 只负责 socket 层的打开、非阻塞收发；帧内编码见 joint_module_protocol.h。
* 比特率由系统配置，本文件不动（需要 root）：
*
* sudo ip link set can0 up type can bitrate 1000000 dbitrate 5000000 fd on
* sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
*
* 配置里的 bitrate / data_bitrate 只用于日志提示，不能透过 SocketCAN 设置。
*/
#pragma once
#include <fcntl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cerrno>
#include <cstring>
#include <string>
#include <utility>
#include <vector>
namespace invicta_hardware {
/// 与 canfd_frame 同形的载体，让上层不必直接依赖 linux/can.h
struct CanFrame {
uint32_t id = 0; ///< 仲裁 ID（标准帧，不含标志位）
uint8_t len = 0; ///< 数据长度
bool fd = false; ///< 是否 CANFD 帧
uint8_t data[64] = {};
};
class CanBus {
public:
CanBus() = default;
explicit CanBus(std::string name) : name_(std::move(name)) {}
~CanBus() { Close(); }
CanBus(const CanBus&) = delete;
CanBus& operator=(const CanBus&) = delete;
CanBus(CanBus&& other) noexcept : fd_(other.fd_), name_(std::move(other.name_)) { other.fd_ = -1; }
CanBus& operator=(CanBus&& other) noexcept {
if (this != &other) {
Close();
fd_ = other.fd_;
name_ = std::move(other.name_);
other.fd_ = -1;
}
return *this;
}
/// 打开并绑定一路 CAN 接口，开启 FD 帧与非阻塞读
bool Open(const std::string& ifname, std::string* err = nullptr) {
Close();
const auto fail = [&](const std::string& what) {
if (err) *err = what + ": " + std::strerror(errno);
Close();
return false;
};
const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (fd < 0) return fail("socket(PF_CAN)");
struct ifreq ifr {};
std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
::close(fd);
errno = ENODEV;
return fail("ioctl(SIOCGIFINDEX) " + ifname);
}
const int enable_fd = 1;
if (::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_fd, sizeof(enable_fd)) < 0) {
::close(fd);
return fail("setsockopt(CAN_RAW_FD_FRAMES)");
}
struct sockaddr_can addr {};
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (::bind(fd, reinterpret_cast<struct sockaddr*>(&addr), sizeof(addr)) < 0) {
::close(fd);
return fail("bind(" + ifname + ")");
}
const int flags = ::fcntl(fd, F_GETFL, 0);
if (::fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
::close(fd);
return fail("fcntl(O_NONBLOCK)");
}
fd_ = fd;
name_ = ifname;
return true;
}
void Close() {
if (fd_ >= 0) {
::close(fd_);
fd_ = -1;
}
}
bool is_open() const { return fd_ >= 0; }
const std::string& name() const { return name_; }
/// 发送一帧。fd=true 时走 CANFD（并置 BRS，对应 5 Mbps 数据段）
bool Send(uint32_t id, const uint8_t* data, uint8_t len, bool fd, std::string* err = nullptr) {
if (fd_ < 0) {
if (err) *err = "bus not open";
return false;
}
if (len > 64) {
if (err) *err = "payload > 64 bytes";
return false;
}
if (fd) {
struct canfd_frame raw {};
raw.can_id = id & CAN_SFF_MASK;
raw.len = len;
raw.flags = CANFD_BRS;
std::memcpy(raw.data, data, len);
if (::write(fd_, &raw, sizeof(raw)) != static_cast<ssize_t>(sizeof(raw))) {
if (err) *err = std::string("write: ") + std::strerror(errno);
return false;
}
} else {
struct can_frame raw {};
raw.can_id = id & CAN_SFF_MASK;
raw.can_dlc = len;
std::memcpy(raw.data, data, len);
if (::write(fd_, &raw, sizeof(raw)) != static_cast<ssize_t>(sizeof(raw))) {
if (err) *err = std::string("write: ") + std::strerror(errno);
return false;
}
}
return true;
}
/// 非阻塞读，最多取 max_frames 帧；返回实际帧数（0 = 当前没数据）
size_t Recv(std::vector<CanFrame>* out, size_t max_frames = 128) {
if (fd_ < 0 || !out) return 0;
size_t count = 0;
while (count < max_frames) {
struct canfd_frame raw {};
const ssize_t got = ::recv(fd_, &raw, CANFD_MTU, MSG_DONTWAIT);
if (got < 0) {
if (errno == EINTR) continue;
break; // EAGAIN/EWOULDBLOCK：这一轮读空了
}
if (got != CAN_MTU && got != CANFD_MTU) continue;
CanFrame frame;
frame.fd = (got == CANFD_MTU);
frame.id = (raw.can_id & CAN_EFF_FLAG) ? (raw.can_id & CAN_EFF_MASK) : (raw.can_id & CAN_SFF_MASK);
frame.len = static_cast<uint8_t>(raw.len > 64 ? 64 : raw.len);
std::memcpy(frame.data, raw.data, frame.len);
out->push_back(frame);
++count;
}
return count;
}
private:
int fd_ = -1;
std::string name_;
};
} // namespace invicta_hardware
src/invicta_hardware/include/invicta_hardware/hardware_bridge.h
+77 -41
View File
@@ -1,24 +1,31 @@
/**
* @file hardware_bridge.h
* @brief 关节/IMU 数据源与关节指令执行端（骨架，硬件 I/O 待实现）
* @brief 关节数据源与关节指令执行端（SocketCAN + 智身模组 CANFD 协议）
*
* 这个节点是「关节状态发布 + 关节命令接收」合一的那一个：
* - 以 publish_rate_hz（默认 200 Hz）向 /joint_states 与 /imu/data 发布状态
* - 订阅 /joint_commands，缓存最新一帧，交给 writeMotors() 下发
* 循环（publish_rate_hz，默认 200 Hz）：
* 1. ReadHardware() —— 非阻塞收各路总线上的 12 字节反馈帧，更新关节状态/温度/状态码
* 2. PublishState() —— 发 /joint_states（IMU 不在本节点：由独立节点发布）
* 3. WriteHardware() —— 按控制字发使能/失能，使能后每周期发 MIT 运行帧
* 另按 diagnostics_rate_hz（默认 10 Hz）发 /joint_diagnostics。
*
* 约定（重要，见 README）：
* 硬件约定（关节方向 dir、零点 offset、单位、CAN 帧格式）全部由本节点负责，
* 话题上流动的必须是 policy 约定下的量：关节角 [rad]、角速度 [rad/s]、力矩 [Nm]、
* IMU 姿态为四元数且角度为弧度。这样 invicta_deploy 侧的 calibration 参数保持恒等。
* 反馈不需要主动查询：模组对每条指令都回一帧状态（手册 §3.2），所以本节点
* 「发了指令就有反馈」，读回与写入天然同频。
*
* 当前 ReadHardware()/WriteHardware() 是空的 TODO，节点只会：
* - 发布全零的关节状态与单位四元数姿态（说明数据源还没接上）
* - 缓存指令并打印控制字变化，不会真的驱动电机
* 标定边界（重要）：
* 本节点是 raw ↔ model 的**唯一**换算边界。话题上收发的都是**模型帧**（模型坐标、rad）：
* 反馈：q_model = q_motor·dir + offset
* 指令：q_motor = (q_model − offset)·dir
* dir/offset 取自本节点自己的 config/invicta_hardware.toml（[[legs]] 的 swing/wheel）。
* invicta_deploy 侧不再做任何标定，只处理模型量（其 toml 不再有 dir/offset_deg）。
*
* 关节选择（部分接入/单腿调试）：
* 指令 JointCommandArray 的 name 列表即本次要控制的关节子集（空 = 全部已配置关节）。
* 不在子集里的关节**不写指令块**（其块保持 0 → 电机自由），不会被驱动也不参与置零。
* kSetZero 同样只作用于子集里的摆臂。数组长度仍必须是全部关节数（position/… 各 8 个）。
*/
#pragma once
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <atomic>
@@ -29,7 +36,10 @@
#include <thread>
#include <vector>
#include "invicta_hardware/can_bus.h"
#include "invicta_hardware/joint_module_protocol.h"
#include "invicta_msgs/JointCommandArray.h"
#include "invicta_msgs/JointDiagnostics.h"
namespace invicta_hardware {
@@ -37,73 +47,99 @@ class HardwareBridge {
public:
explicit HardwareBridge(const ros::NodeHandle& pnh);
/// 主循环：以 publish_rate_hz 周期跑「读硬件 → 发状态 → 下发指令」，阻塞
/// CAN 总线是否全部打开成功（false 时 main 直接退出）
bool Ready() const { return ready_; }
/// 主循环，阻塞
void Run();
void Stop();
private:
/// 一路总线：通道名 + 帧长 + CANID → 关节下标
struct BusRuntime {
std::string name;
CanBus can;
int max_canid = 0;
int frame_len = 0;
std::vector<int> canid_to_joint; ///< 下标即 CANID，-1 表示该 ID 未使用
};
void LoadParams(const ros::NodeHandle& pnh);
bool OpenBuses();
void CommandCallback(const invicta_msgs::JointCommandArray::ConstPtr& msg);
/// 发布一帧关节状态与 IMU
void PublishState();
void PublishDiagnostics();
/// 控制字变化时的处理（使能/失能/清错），后续在 WriteHardware 中落实
void HandleControlWord(uint16_t word);
/// 控制字变化：使能/失能按语义落成 CAN 帧；include 是本次指令选中的关节（见 WriteHardware）
void HandleControlWord(uint16_t word, const std::vector<uint8_t>* include);
/**
* @brief 从电机读回状态
*
* TODO(硬件)：接 SocketCAN 读回各关节（关节表见 invicta.toml）的
* position[rad] / velocity[rad/s] / effort[Nm]
* 并做 raw → policy 约定的换算（方向、零点、单位）。
* 同时读回 IMU：姿态四元数（弧度）、角速度[rad/s]（机体系）、线加速度[m/s²]（机体系）。
* 注意 M20 原车 /IMU_DATA 的 rpy 是「度」，这里约定统一用弧度，不要照搬。
* 读失败时返回 false，本帧沿用上一次的值（不要发 NaN）。
* 向所有总线各发若干帧「9×0xFF + code」指令块。
* include 为空指针 = 所有已配置关节；否则只有 include[joint] != 0 的关节才写入指令块。
*/
void SendCodeAll(uint8_t code, int frames = 1, double interval_s = 0.0,
const std::vector<uint8_t>* include = nullptr);
/// 由指令里的 name 列表算出关节选择掩码（长度 = 关节数；空列表 = 全选）
std::vector<uint8_t> SelectionMask(const std::vector<std::string>& names, bool* had_unknown) const;
/// 指令超时：kp=0、速度 0、只留阻尼，防止上游挂掉后一直重发最后一帧
bool SendTimeoutFrame();
bool ReadHardware();
/**
* @brief 把最新指令下发给电机
*
* TODO(硬件)：把 policy 约定的量换算成 raw（零点、方向、单位），
* 按 invicta_msgs/control_word.h 的语义设置使能状态，组装 CAN 帧写下去。
* torque = kp*(q_des - q) + kd*(dq_des - dq) + tau_ff 由驱动器或本节点计算，
* 取决于关节模组协议，二选一并写清楚。
*/
bool WriteHardware();
// ---- 参数 ----
std::string joint_state_topic_ = "joint_states";
std::string imu_topic_ = "imu/data";
std::string command_topic_ = "joint_commands";
std::string imu_frame_id_ = "base_link";
std::string diagnostics_topic_ = "joint_diagnostics";
std::vector<std::string> joint_names_;
double publish_rate_hz_ = 200.0;
double diagnostics_rate_hz_ = 10.0;
int command_queue_ = 1;
/// true：CAN 打不开就退出（真机默认）；false：无 CAN 也启动，只发状态（调试/冒烟测试）
bool require_can_ = true;
MotorRange joint_range_, wheel_range_;
// ---- 指令看门狗 ----
double command_timeout_s_ = 0.1;
double timeout_joint_kd_ = 4.0;
double timeout_wheel_kd_ = 1.0;
/// 最近一次收到指令的时刻（steady_clock 纳秒）；回调线程写、循环线程读，用原子量避免数据竞争
std::atomic<int64_t> last_cmd_ns_{0};
bool timeout_active_ = false;
// ---- ROS ----
ros::NodeHandle nh_;
ros::Publisher joint_state_pub_;
ros::Publisher imu_pub_;
ros::Publisher diagnostics_pub_;
ros::Subscriber command_sub_;
// ---- 指令缓存 ----
std::mutex cmd_mutex_;
invicta_msgs::JointCommandArray latest_cmd_;
bool has_cmd_ = false;
uint16_t active_control_word_ = 0;
uint16_t applied_control_word_ = 0; ///< 已在循环线程里落地的控制字
std::atomic<bool> enabled_{false}; ///< 是否已使能（未使能不发运行帧）
// ---- 状态缓存（policy 约定）----
// ---- CAN ----
std::mutex can_mutex_; ///< 使能序列（回调线程）与运行帧（循环线程）互斥
std::vector<BusRuntime> buses_;
bool ready_ = false;
// ---- 状态缓存（模型帧：已施加 dir/offset）----
std::vector<double> joint_position_, joint_velocity_, joint_effort_;
double imu_orientation_[4] = {1.0, 0.0, 0.0, 0.0}; ///< w,x,y,z
double imu_angular_velocity_[3] = {0.0, 0.0, 0.0};
double imu_linear_acceleration_[3] = {0.0, 0.0, 0.0};
std::vector<double> temperature_, voltage_;
std::vector<uint8_t> driver_status_, last_status_;
std::vector<uint8_t> has_feedback_; ///< 该关节是否收到过反馈（诊断只报有数据的）
std::atomic<bool> running_{false};
bool warned_no_command_ = false;
bool warned_unknown_joint_ = false; ///< 指令 name 里有不认识的关节名，只警告一次
};
} // namespace invicta_hardware
src/invicta_hardware/include/invicta_hardware/hardware_config.h
+66 -21
View File
@@ -10,6 +10,7 @@
#include <toml++/toml.hpp>
#include <algorithm>
#include <cmath>
#include <string>
#include <vector>
@@ -20,10 +21,18 @@ struct JointSpec {
std::string name;
bool is_wheel = false;
int leg = -1;
double lower = 0., upper = 0.; ///< 位置限位 [rad]（下发前自检用）
double vel_limit = 0.; ///< 速度限位 [rad/s]
double dir = 1.; ///< 硬件方向 ±1
double offset_deg = 0.; ///< 硬件零点偏置 [deg]
double lower = 0., upper = 0.; ///< 位置限位 [rad]（模型帧；下发前自检用）
double vel_limit = 0.; ///< 速度限位 [rad/s]（模型帧，幅值）
double torque_limit = 0.; ///< 前馈力矩限幅 [N·m]（模型帧，幅值；0 = 不额外限制）
double dir = 1.; ///< 硬件方向 ±1（电机正方向 vs 模型正方向）
double offset_deg = 0.; ///< 硬件零点偏置 [deg]（模型零位对应的电机角）
/// 模型帧 → 电机帧：q_motor = (q_model − offset)·dir
double ModelToMotorPos(double q_model) const {
return (q_model - offset_deg * M_PI / 180.0) * dir;
}
/// 电机帧 → 模型帧：q_model = q_motor·dir + offset
double MotorToModelPos(double q_motor) const { return q_motor * dir + offset_deg * M_PI / 180.0; }
};
/// 一条腿在总线上的位置
@@ -66,11 +75,18 @@ public:
double bench_joint_kp = 150., bench_joint_kd = 2., bench_wheel_kd = 0.5;
// ---- 本节点话题与频率 ----
std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
std::string topic_joint_states = "joint_states";
std::string topic_joint_commands = "joint_commands";
std::string topic_joint_diagnostics = "joint_diagnostics";
double publish_rate_hz = 200.0;
std::string imu_frame_id = "base_link";
double diagnostics_rate_hz = 10.0; ///< 温度/状态/电压发布频率（低频诊断，不进控制路径）
int command_queue = 1;
bool require_can = true; ///< false = 无 CAN 也启动（调试/冒烟测试）
// ---- 指令看门狗 ----
double command_timeout_s = 0.1; ///< 超过这么久没收到 /joint_commands 就转阻尼；0 = 关闭
double timeout_joint_kd = 4.0; ///< 超时后的摆臂阻尼
double timeout_wheel_kd = 1.0; ///< 超时后的轮阻尼
bool Load(const std::string& path, std::string* error = nullptr);
@@ -131,22 +147,24 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
try {
tbl = toml::parse_file(path);
} catch (const toml::parse_error& e) {
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
return fail(std::string("failed to parse TOML: ") + std::string(e.description()) + " @ " + path);
}
const toml::table* robot = detail::Section(tbl, "robot");
if (!robot) return fail("缺少 [robot] 段");
if (!robot) return fail("missing [robot] section");
detail::GetStrArr(robot, "joint_names", joint_names);
if (joint_names.empty()) return fail("[robot].joint_names 为空");
if (joint_names.empty()) return fail("[robot].joint_names is empty");
dof = int(joint_names.size());
std::vector<double> lower, upper, vel_limit;
std::vector<double> lower, upper, vel_limit, torque_limit;
const toml::table* limits = detail::Section(tbl, "joint_limits");
detail::GetDoubleArr(limits, "lower", lower);
detail::GetDoubleArr(limits, "upper", upper);
detail::GetDoubleArr(limits, "vel_limit", vel_limit);
if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
detail::GetDoubleArr(limits, "torque_limit", torque_limit);
if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof ||
int(torque_limit.size()) != dof) {
return fail("[joint_limits] lower/upper/vel_limit/torque_limit length must equal joint count");
}
joints.assign(size_t(dof), JointSpec{});
for (int i = 0; i < dof; ++i) {
@@ -154,6 +172,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
joints[size_t(i)].lower = lower[size_t(i)];
joints[size_t(i)].upper = upper[size_t(i)];
joints[size_t(i)].vel_limit = vel_limit[size_t(i)];
joints[size_t(i)].torque_limit = torque_limit[size_t(i)];
}
legs.clear();
@@ -168,7 +187,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
leg.swing_index = IndexOf(leg.swing_joint);
leg.wheel_index = IndexOf(leg.wheel_joint);
if (leg.swing_index < 0 || leg.wheel_index < 0) {
return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
return fail("[[legs]] " + leg.name + " swing_joint/wheel_joint not in joint_names");
}
const toml::table* sw = detail::Section(*lt, "swing");
const toml::table* wh = detail::Section(*lt, "wheel");
@@ -186,7 +205,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
legs.push_back(leg);
}
}
if (legs.empty()) return fail("缺少 [[legs]] 段");
if (legs.empty()) return fail("missing [[legs]] section");
if (const toml::table* mj = detail::Section(tbl, "motor")) {
const auto load_motor = [](const toml::table* m, MotorSpec& out) {
@@ -215,18 +234,44 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
detail::Get(mg, "bench_joint_kp", bench_joint_kp);
detail::Get(mg, "bench_joint_kd", bench_joint_kd);
detail::Get(mg, "bench_wheel_kd", bench_wheel_kd);
detail::Get(mg, "timeout_joint_kd", timeout_joint_kd);
detail::Get(mg, "timeout_wheel_kd", timeout_wheel_kd);
}
// 校验人为限幅落在「归一化边界」内。
// [motor.*] 的 p/v/t 只决定 0~65535 的映射关系，不是限幅；用户限幅超出它说明配置写错
// （或模组阈值被改过），在加载期直接失败，不靠编码饱和兜着。
// 轮关节没有位置控制，lower/upper 是占位值，跳过位置校验。
for (int i = 0; i < dof; ++i) {
const JointSpec& j = joints[size_t(i)];
const MotorSpec& m = j.is_wheel ? motor_wheel : motor_joint;
if (!j.is_wheel && (j.lower < m.p_min || j.upper > m.p_max)) {
return fail("[joint_limits] " + j.name + " position limit outside normalized bounds [p_min, p_max]");
}
if (j.vel_limit > std::min(std::fabs(m.v_max), std::fabs(m.v_min))) {
return fail("[joint_limits] " + j.name + " velocity limit outside normalized bounds [v_min, v_max]");
}
if (j.torque_limit > std::min(std::fabs(m.t_max), std::fabs(m.t_min))) {
return fail("[joint_limits] " + j.name + " torque limit outside normalized bounds [t_min, t_max]");
}
}
const toml::table* topics = detail::Section(tbl, "topics");
detail::GetStr(topics, "joint_states", topic_joint_states);
detail::GetStr(topics, "imu", topic_imu);
detail::GetStr(topics, "joint_commands", topic_joint_commands);
detail::GetStr(topics, "joint_diagnostics", topic_joint_diagnostics);
const toml::table* hw = detail::Section(tbl, "hardware");
detail::Get(hw, "publish_rate_hz", publish_rate_hz);
detail::GetStr(hw, "imu_frame_id", imu_frame_id);
detail::Get(hw, "diagnostics_rate_hz", diagnostics_rate_hz);
detail::Get(hw, "command_queue", command_queue);
if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz 必须 > 0");
detail::Get(hw, "command_timeout_s", command_timeout_s);
if (hw) {
if (auto v = (*hw)["require_can"].value<bool>()) require_can = *v;
}
if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz must be > 0");
if (diagnostics_rate_hz <= 0.) return fail("[hardware].diagnostics_rate_hz must be > 0");
if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s must not be negative");
if (error) error->clear();
return true;
@@ -235,8 +280,8 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
inline std::string HardwareConfig::Summary() const {
char buf[512];
std::string s;
snprintf(buf, sizeof(buf), "[config] %d 关节，%zu 条腿，状态发布 %.0f Hz，IMU frame=%s\n", dof, legs.size(),
publish_rate_hz, imu_frame_id.c_str());
snprintf(buf, sizeof(buf), "[config] %d joints, %zu legs, state publish %.0f Hz\n", dof, legs.size(),
publish_rate_hz);
s += buf;
for (const auto& leg : legs) {
snprintf(buf, sizeof(buf), "[config] %s: swing %s(%s#%d) wheel %s(%s#%d)\n", leg.name.c_str(),
@@ -244,11 +289,11 @@ inline std::string HardwareConfig::Summary() const {
leg.wheel_bus.c_str(), leg.wheel_canid);
s += buf;
}
snprintf(buf, sizeof(buf), "[config] 电机: 关节 %s(p±%.1f v±%.1f t±%.0f) 轮 %s(p±%.1f v±%.1f t±%.0f)\n",
snprintf(buf, sizeof(buf), "[config] motor: joint %s(p+/-%.1f v+/-%.1f t+/-%.0f) wheel %s(p+/-%.1f v+/-%.1f t+/-%.0f)\n",
motor_joint.model.c_str(), motor_joint.p_max, motor_joint.v_max, motor_joint.t_max,
motor_wheel.model.c_str(), motor_wheel.p_max, motor_wheel.v_max, motor_wheel.t_max);
s += buf;
snprintf(buf, sizeof(buf), "[config] CAN: %zu 路 %s.. @%.1fM/%.1fM, 使能 %d 帧 x %.3f s\n",
snprintf(buf, sizeof(buf), "[config] CAN: %zu channels %s.. @%.1fM/%.1fM, enable %d frames x %.3f s\n",
can.channels.size(), can.channels.empty() ? "-" : can.channels.front().c_str(), can.bitrate / 1e6,
can.data_bitrate / 1e6, can.enable_frames, can.enable_interval_s);
s += buf;
src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h
+127
View File
@@ -0,0 +1,127 @@
/**
* @file joint_module_protocol.h
* @brief 智身关节模组 CANFD 协议的编解码（docs/zhishen_joint_module_canfd.md）
*
* 下发帧：64 字节以内，Byte0-1 是 MSGID，之后每个 CANID 占 10 字节，
* 块起点 = 2 + 10 × (CANID − 1)，多字节小端。
* 反馈帧：每个关节独立返回 12 字节（位置/速度/扭矩/温度/状态/电压）。
*
* ⚠ 编码与解码都用「归一化边界」（手册 §7 的 P/V/T/KP/KD）；它只决定 0~65535 映射到多少物理量，
* 不是运行限幅。限幅在 [joint_limits] 里先夹紧，再送进 Encode()（见 invicta_hardware/README.md）。
*/
#pragma once
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstring>
namespace invicta_hardware {
/// 归一化边界：决定 0~65535 与物理量的映射，不是限幅
struct MotorRange {
double p_min = 0., p_max = 0.;
double v_min = 0., v_max = 0.;
double t_min = 0., t_max = 0.;
double kp_max = 0., kd_max = 0.;
};
constexpr int kBlockBytes = 10; ///< 每个关节在下发帧里占 10 字节
constexpr int kMaxMotorsPerFrame = 6; ///< 一帧最多 6 个关节
constexpr uint16_t kMsgId = 0x0100; ///< 主机 MSGID，从机原样回传
constexpr uint8_t kCodeEnable = 0xFC; ///< 使能（9 字节 0xFF + 本码）
constexpr uint8_t kCodeDisable = 0xFD; ///< 失能，MOS 管全关
constexpr uint8_t kCodeZero = 0xEF; ///< 把当前位置设为机械零位
/// 物理量 → uint16（0~65535 线性映射到 MIN~MAX，手册 §5）
inline uint16_t Encode(double x, double lo, double hi) {
if (hi <= lo) return 0;
if (!std::isfinite(x)) return uint16_t(Encode(0.0, lo, hi));
const double r = std::round((x - lo) / (hi - lo) * 65535.0);
return static_cast<uint16_t>(std::min(65535.0, std::max(0.0, r)));
}
/// uint16 → 物理量（必须与 Encode 用同一套量程）
inline double Decode(uint16_t raw, double lo, double hi) { return lo + double(raw) / 65535.0 * (hi - lo); }
inline uint16_t ReadU16(const uint8_t* p) { return uint16_t(p[0] | (uint16_t(p[1]) << 8)); }
inline void WriteU16(uint8_t* p, uint16_t v) {
p[0] = uint8_t(v & 0xFF);
p[1] = uint8_t(v >> 8);
}
/// 运行块：位置 / 速度 / Kp / Kd / 前馈力矩，各 2 字节小端
inline void PackRunBlock(uint8_t* p, double pos, double vel, double kp, double kd, double tau,
const MotorRange& r) {
WriteU16(p + 0, Encode(pos, r.p_min, r.p_max));
WriteU16(p + 2, Encode(vel, r.v_min, r.v_max));
WriteU16(p + 4, Encode(kp, 0.0, r.kp_max));
WriteU16(p + 6, Encode(kd, 0.0, r.kd_max));
WriteU16(p + 8, Encode(tau, r.t_min, r.t_max));
}
/// 使能/失能/置零块：前 9 字节固定 0xFF，最后 1 字节是指令码
inline void PackCodeBlock(uint8_t* p, uint8_t code) {
std::memset(p, 0xFF, 9);
p[9] = code;
}
/// 12 字节状态帧（手册 §3.2）
struct MotorFeedback {
double position = 0.; ///< [rad]
double velocity = 0.; ///< [rad/s]
double torque = 0.; ///< [N·m]
double temperature = 0.; ///< [℃]（raw − 40）
double voltage = 0.; ///< [V]（raw 直接是伏）
uint8_t status = 0; ///< bit0 使能，bit1..6 故障
bool enabled() const { return (status & 0x01) != 0; }
bool fault() const { return ((status >> 1) & 0x3F) != 0; }
};
/// 状态码非零故障位的中文名，用于日志
inline const char* FaultName(uint8_t status) {
switch ((status >> 1) & 0x3F) {
case 0x00: return "none";
case 0x01: return "overvoltage";
case 0x02: return "overcurrent";
case 0x04: return "overtemperature";
case 0x08: return "overspeed";
case 0x10: return "encoder";
case 0x20: return "undervoltage/pre-driver";
default: return "combined fault";
}
}
/**
* @brief 解一帧 12 字节反馈
* @param msgid 可选输出：帧内 MSGID（用于和请求配对）
*/
inline bool DecodeFeedback(const uint8_t* d, size_t len, const MotorRange& r, MotorFeedback* out,
uint16_t* msgid = nullptr) {
if (!d || !out || len < 12) return false;
if (msgid) *msgid = ReadU16(d);
out->position = Decode(ReadU16(d + 2), r.p_min, r.p_max);
out->velocity = Decode(ReadU16(d + 4), r.v_min, r.v_max);
out->torque = Decode(ReadU16(d + 6), r.t_min, r.t_max);
out->temperature = double(d[8]) - 40.0;
out->status = d[9];
out->voltage = double(d[10]);
return true;
}
/// 下发长度（手册 §3.1 的裁剪表：按本路最大 CANID 取最小字节数）
inline int FrameLenForMaxCanId(int max_canid) {
switch (max_canid) {
case 0: return 0;
case 1: return 12;
case 2: return 24;
case 3: return 32;
case 4: return 48;
default: return 64;
}
}
} // namespace invicta_hardware
src/invicta_hardware/launch/invicta_hardware.launch
+4 -1
View File
@@ -1,6 +1,9 @@
<launch>
<!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数来自本节点自己的 config/invicta_hardware.toml -->
<!-- 关节数据源 + 关节指令执行端（SocketCAN）。参数来自本节点自己的 config/invicta_hardware.toml -->
<!-- require_can:=false 用于无 CAN 的调试/冒烟测试：节点照常发状态，但不下发电机指令 -->
<arg name="require_can" default="true" doc="true = CAN 打不开就退出；false = 无 CAN 也启动"/>
<node pkg="invicta_hardware" type="invicta_hardware_node" name="invicta_hardware" output="screen">
<param name="config_file" value="$(find invicta_hardware)/config/invicta_hardware.toml"/>
<param name="require_can" value="$(arg require_can)"/>
</node>
</launch>
src/invicta_hardware/scripts/hw_debug_test.py
Executable
+503
View File
@@ -0,0 +1,503 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
invicta_hardware 单节点调试脚本
只启动 hardware 节点（可 --start-node 代劳），直接往 /joint_commands 发
JointCommandArray，用来在台架上单独验证：
damping 阻尼模式：kp=0、kd 取阻尼值、速度给 0，只抵抗外力不保持位置
angle 摆臂角度：把 4 条摆臂打到指定角度（按硬编码限幅夹紧），轮子给 0 速
wheel 轮组速度：给 4 个轮子指定转速（按硬编码上限夹紧），摆臂阻尼
offset 标定核对：把摆臂打到模型 0 位（默认，可用 --swing 指定对照角），
看回显模型角是否等于指令——相等即说明 hardware 的 dir/offset 自洽
zero 置零：把当前机械位置写成模组零位（0xEF），仅摆臂（轮毂不支持）
monitor 只监听，不发任何指令
角度回显：发指令时默认每 ~0.2 s 打印一次各关节模型角（rad/deg）。阻尼模式下也照常打印，
可以边看角度边手动掰腿。
⚠ 话题上收发都是**模型帧**：raw↔model 的 dir/offset 换算由 hardware 节点独家承担
（见 invicta_hardware/README.md「标定边界」），本脚本不再自己做换算。
限幅等参数全部硬编码在本文件，不读任何 toml。
用法（需先 roscore）：
# 另开终端先起节点：
rosrun invicta_hardware invicta_hardware_node # 有 CAN
rosrun invicta_hardware invicta_hardware_node _require_can:=false # 无 CAN 干跑
# 或让脚本代起（--require-can / --no-can）：
python3 hw_debug_test.py --start-node --no-can --mode damping
python3 hw_debug_test.py --start-node --mode damping
python3 hw_debug_test.py --mode angle --swing 0.5 --swing-vel 0.5
python3 hw_debug_test.py --mode wheel --wheel 3.0
python3 hw_debug_test.py --mode offset --swing 0.5
python3 hw_debug_test.py --mode zero
# 只接了一条腿时：只控制/置零这条腿的关节
python3 hw_debug_test.py --mode damping --joints fl_swing,fl_wheel
python3 hw_debug_test.py --mode zero --joints fl_swing
python3 hw_debug_test.py --mode monitor
python3 hw_debug_test.py --mode damping --duration 10
"""
from __future__ import print_function
import argparse
import atexit
import math
import signal
import subprocess
import sys
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
try:
from invicta_msgs.msg import JointCommandArray, JointDiagnostics
except ImportError as exc: # pragma: no cover - 环境没 source devel 时给出人话提示
sys.stderr.write(
"无法 import invicta_msgs：%s\n"
"请先 source /opt/ros/noetic/setup.bash 和本工作空间的 devel/setup.bash，并确保已 catkin_make。\n" % exc
)
sys.exit(1)
# 控制字取值，见 invicta_msgs/control_word.h
K_DISABLE = 1
K_ENABLE = 2
K_MOTOR_CONTROL = 3
K_ERROR_RESET = 4
K_GET_STATUS = 5
K_SET_ZERO = 6
# =============================================================================
# 硬编码参数（不读 toml）
# =============================================================================
JOINT_NAMES = [
"fl_swing", "fl_wheel",
"fr_swing", "fr_wheel",
"hl_swing", "hl_wheel",
"hr_swing", "hr_wheel",
]
SWING_SLOTS = [0, 2, 4, 6]
WHEEL_SLOTS = [1, 3, 5, 7]
# 标定边界：raw↔model 的 dir/offset 换算**由 hardware 节点独家承担**，
# 所以本脚本在 /joint_commands 与 /joint_states 上看到的都是**模型帧**，不再自己换算。
# 核对标定 = 发一个模型角 → 看回显模型角是否等于它（angle/offset 模式）。
# 限幅（hardware 节点里 [joint_limits] 的值，这里同步一份）
SWING_ANGLE_MIN = -1.40 # [rad]
SWING_ANGLE_MAX = 1.40 # [rad]
SWING_VEL_LIMIT = 22.4 # [rad/s]
SWING_TAU_LIMIT = 75.0 # [N·m]
WHEEL_VEL_LIMIT = 100.0 # [rad/s] 轮组工作上限（额定 81.2 的余量）
WHEEL_TAU_LIMIT = 28.68 # [N·m]
KP_MAX = 500.0
KD_MAX = 5.0
# 默认增益
DEFAULT_KP = 150.0 # 摆臂位置环（台架值）
DEFAULT_KD = 2.0 # 摆臂速度环
DEFAULT_DAMP_JOINT_KD = 4.0 # 阻尼模式摆臂 kd
DEFAULT_DAMP_WHEEL_KD = 1.0 # 阻尼模式轮组 kd
TOPIC_JOINT_STATES = "/joint_states"
TOPIC_JOINT_COMMANDS = "/joint_commands"
TOPIC_DIAGNOSTICS = "/joint_diagnostics"
DEFAULT_PUB_HZ = 200.0
DEFAULT_SHOW_HZ = 5.0
# =============================================================================
# 工具
# =============================================================================
def clamp(v, lo, hi, what, warns):
if v > hi:
warns.append("%s=%.3f 超上限 %.3f，已夹紧" % (what, v, hi))
return hi
if v < lo:
warns.append("%s=%.3f 超下限 %.3f，已夹紧" % (what, v, lo))
return lo
return v
def parse_list(text, n, default):
"""把 '0.5' 或 '0.5,0.6,0.7,0.8' 解析成 n 个浮点。"""
if text is None:
return [float(default)] * n
parts = [p.strip() for p in str(text).split(",") if p.strip() != ""]
if len(parts) == 1:
return [float(parts[0])] * n
if len(parts) != n:
raise ValueError("需要 1 个或 %d 个值，收到 %d 个：%s" % (n, len(parts), text))
return [float(p) for p in parts]
# =============================================================================
# 主体
# =============================================================================
class HardwareDebug(object):
def __init__(self, args):
self.args = args
self.n = len(JOINT_NAMES)
self.name2slot = {nm: i for i, nm in enumerate(JOINT_NAMES)}
self.state_pos = [None] * self.n # 模型角（hardware 已换算）
self.state_vel = [None] * self.n
self.state_stamp = 0.0
self.state_ok = False
self.diag_text = {}
# 本次要控制的关节子集（指令 name 列表）。空 = 全部 8 个。
self.selected_names = list(args.joints) if args.joints else list(JOINT_NAMES)
self.selected_mask = [nm in self.selected_names for nm in JOINT_NAMES]
self.enabled = False
self.stop_requested = False
self.pub = rospy.Publisher(TOPIC_JOINT_COMMANDS, JointCommandArray, queue_size=1)
rospy.Subscriber(TOPIC_JOINT_STATES, JointState, self._on_state, queue_size=1)
rospy.Subscriber(TOPIC_DIAGNOSTICS, JointDiagnostics, self._on_diag, queue_size=1)
# ------------------------------------------------------------ 回调
def _on_state(self, msg):
get = {}
for k, nm in enumerate(msg.name):
if nm in self.name2slot:
get[self.name2slot[nm]] = k
for i, nm in enumerate(JOINT_NAMES):
if i in get:
k = get[i]
if k < len(msg.position):
self.state_pos[i] = msg.position[k]
if k < len(msg.velocity):
self.state_vel[i] = msg.velocity[k]
elif i < len(msg.position):
# 没给 name 就按数组下标
self.state_pos[i] = msg.position[i]
if i < len(msg.velocity):
self.state_vel[i] = msg.velocity[i]
self.state_stamp = msg.header.stamp.to_sec()
self.state_ok = True
def _on_diag(self, msg):
for k, nm in enumerate(msg.name):
if k < len(msg.temperature):
t = msg.temperature[k]
v = msg.voltage[k] if k < len(msg.voltage) else float("nan")
s = msg.status[k] if k < len(msg.status) else 0
self.diag_text[nm] = "%.0f℃ %.1fV st=0x%02X" % (t, v, s)
# ------------------------------------------------------------ 发指令
def publish(self, control_word, kp, kd, pos, vel, tau):
msg = JointCommandArray()
msg.header = Header(stamp=rospy.Time.now())
# name = 本次要控制的关节子集；数组长度仍是全部 8 个（hardware 要求），
# 未选中的位置清零/增益置 0，做到既不驱动也不保持。
msg.name = list(self.selected_names)
msg.position = [pos[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
msg.velocity = [vel[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
msg.effort = [tau[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
msg.kp = [kp[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
msg.kd = [kd[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
msg.control_word = control_word
self.pub.publish(msg)
def startup_sequence(self):
"""disable → error_reset → enable，之后再走 motor control。"""
print("[dbg] 使能序列 disable → error_reset → enable ...")
zero = [0.0] * self.n
for word in (K_DISABLE, K_ERROR_RESET, K_ENABLE):
for _ in range(3):
self.publish(word, zero, zero, zero, zero, zero)
rospy.sleep(0.02)
# 等 hardware 的 Run 循环把控制字处理掉
rospy.sleep(0.15)
self.enabled = True
# ------------------------------------------------------------ 打印
def show_header(self):
cols = "%-10s %10s %10s %10s" % ("joint", "model_rad", "model_deg", "model_vel")
print("\n" + cols)
print("-" * len(cols))
def show_once(self):
if not self.state_ok:
print("[dbg] 还没有 /joint_states 反馈（hardware 节点起了吗？CAN 通吗？）")
return
self.show_header()
for i, nm in enumerate(JOINT_NAMES):
m = self.state_pos[i]
if m is None:
continue
v = self.state_vel[i]
v = 0.0 if v is None else v
extra = self.diag_text.get(nm, "")
print("%-10s %10.4f %10.2f %10.3f %s" % (nm, m, math.degrees(m), v, extra))
age = rospy.get_time() - self.state_stamp
print(" (反馈 %s, 龄期 %.3fs)" % ("OK" if self.state_ok else "无", age))
# ------------------------------------------------------------ 模式
def build_cmd(self, t, phase):
"""返回 (kp, kd, pos, vel, tau) 或 None（monitor）。"""
a = self.args
n = self.n
kp = [0.0] * n
kd = [0.0] * n
pos = [0.0] * n
vel = [0.0] * n
tau = [0.0] * n
warns = []
if a.mode == "damping":
for i in SWING_SLOTS:
kd[i] = a.damp_joint_kd
for i in WHEEL_SLOTS:
kd[i] = a.damp_wheel_kd
return kp, kd, pos, vel, tau, warns
if a.mode in ("angle", "offset"):
# 摆臂：位置环；轮子：阻尼 + 0 速。指令即模型帧，hardware 负责换算到电机帧。
kp_s = clamp(a.kp, 0.0, KP_MAX, "kp", warns)
kd_s = clamp(a.kd, 0.0, KD_MAX, "kd", warns)
vel_s = clamp(a.swing_vel, -SWING_VEL_LIMIT, SWING_VEL_LIMIT, "swing_vel", warns)
for leg_i, i in enumerate(SWING_SLOTS):
target = a.swing[leg_i] # 模型角 [rad]
target = clamp(target, SWING_ANGLE_MIN, SWING_ANGLE_MAX, "swing%s" % JOINT_NAMES[i], warns)
if a.ramp > 0.0:
frac = min(1.0, t / a.ramp)
target = target * frac
kp[i] = kp_s
kd[i] = kd_s
vel[i] = vel_s
pos[i] = target
for i in WHEEL_SLOTS:
kd[i] = a.damp_wheel_kd
return kp, kd, pos, vel, tau, warns
if a.mode == "wheel":
kd_w = clamp(a.kd, 0.0, KD_MAX, "kd", warns)
for leg_i, i in enumerate(WHEEL_SLOTS):
target = clamp(a.wheel[leg_i], -WHEEL_VEL_LIMIT, WHEEL_VEL_LIMIT,
"wheel%s" % JOINT_NAMES[i], warns)
if a.ramp > 0.0:
target = target * min(1.0, t / a.ramp)
kd[i] = kd_w
vel[i] = target
for i in SWING_SLOTS:
kd[i] = a.damp_joint_kd
return kp, kd, pos, vel, tau, warns
return None
# ------------------------------------------------------------ 置零
def do_set_zero(self):
"""把当前机械位置写成模组零位（0xEF）。只作用于 --joints 选中的关节；轮毂不支持。"""
swings = [nm for nm in self.selected_names if nm in [JOINT_NAMES[i] for i in SWING_SLOTS]]
print("\n" + "=" * 60)
print(" 置零（control_word=kSetZero -> 0xEF）")
print(" 目标关节: %s" % (", ".join(swings) if swings else "（无摆臂，将被忽略）"))
print(" ⚠ 请确认这些摆臂已处在**机械零位**姿态（参考：竖直向下）。")
print(" ⚠ 只对摆臂生效，轮毂不支持置零（hardware 节点会自动跳过）。")
print(" ⚠ 置零后该位姿的反馈角≈0；请把 hardware toml 的 offset_deg 置 0（或据此重标）。")
print("=" * 60)
if not swings:
print("[dbg] 选中的关节里没有摆臂，置零不会有任何效果；用 --joints 指定摆臂名。")
return
zero = [0.0] * self.n
# hardware 只在控制字「变化」时落地一次。先发 kGetStatus（无副作用）再发 kSetZero，
# 保证即便节点进程被复用也能触发 5→6 的变化。
for _ in range(3):
self.publish(K_GET_STATUS, zero, zero, zero, zero, zero)
rospy.sleep(0.02)
for _ in range(3):
self.publish(K_SET_ZERO, zero, zero, zero, zero, zero)
rospy.sleep(0.02)
rospy.sleep(0.4) # 等模组把零位写进去、反馈归零
print("\n[dbg] 已发置零。下面看反馈模型角（offset 设为 0 时，此处应≈0）：")
for _ in range(8):
self.show_once()
rospy.sleep(0.35)
# ------------------------------------------------------------ 主循环
def run(self):
a = self.args
print("[dbg] 模式 = %s，发布 %s @ %.0f Hz，回显 %.0f Hz" %
(a.mode, TOPIC_JOINT_COMMANDS, a.pub_hz, a.show_hz))
print("[dbg] 话题上收发都是模型帧；dir/offset 由 hardware 节点施加（本脚本不换算）")
# 等订阅者出现（ROS1 发布无 latch，太早发时序帧会丢）
for _ in range(200):
if self.pub.get_num_connections() > 0 or rospy.is_shutdown():
break
rospy.sleep(0.05)
if self.pub.get_num_connections() == 0:
print("[dbg] 警告：%s 还没有订阅者，确认 hardware 节点在跑" % TOPIC_JOINT_COMMANDS)
if a.mode == "zero":
self.do_set_zero()
return
if a.mode in ("angle", "offset") and not any(self.selected_mask[i] for i in SWING_SLOTS):
print("[dbg] 警告：--joints 里没有摆臂，angle/offset 模式不会驱动任何关节")
if a.mode == "wheel" and not any(self.selected_mask[i] for i in WHEEL_SLOTS):
print("[dbg] 警告：--joints 里没有轮组，wheel 模式不会驱动任何关节")
if a.mode != "monitor":
self.startup_sequence()
pub_period = 1.0 / a.pub_hz
show_period = 1.0 / a.show_hz
t0 = rospy.get_time()
last_show = 0.0
last_warn_t = 0.0
r = rospy.Rate(a.pub_hz)
while not rospy.is_shutdown() and not self.stop_requested:
t = rospy.get_time() - t0
if a.duration > 0.0 and t >= a.duration:
print("[dbg] 到达 --duration %.1fs，停止指令" % a.duration)
break
if a.mode != "monitor":
built = self.build_cmd(t, None)
if built is not None:
kp, kd, pos, vel, tau, warns = built
# 限幅提示节流
if warns and (rospy.get_time() - last_warn_t) > 2.0:
last_warn_t = rospy.get_time()
for w in warns:
print("[dbg][限幅] " + w)
self.publish(K_MOTOR_CONTROL, kp, kd, pos, vel, tau)
if (rospy.get_time() - last_show) >= show_period:
last_show = rospy.get_time()
self.show_once()
r.sleep()
self.safe_stop()
def safe_stop(self):
if self.args.mode in ("monitor", "zero"):
return
print("\n[dbg] 收尾：先转阻尼 0.3s，再失能 ...")
zero = [0.0] * self.n
kd = [self.args.damp_joint_kd if i in SWING_SLOTS else self.args.damp_wheel_kd for i in range(self.n)]
kp = zero
end = time.time() + 0.3
while time.time() < end and not rospy.is_shutdown():
self.publish(K_MOTOR_CONTROL, kp, kd, zero, zero, zero)
rospy.sleep(0.005)
for _ in range(3):
self.publish(K_DISABLE, zero, zero, zero, zero, zero)
rospy.sleep(0.02)
print("[dbg] 已失能，退出")
# =============================================================================
# 启动 hardware 节点（可选）
# =============================================================================
def start_node(args):
require_can = "true" if args.require_can else "false"
cmd = ["roslaunch", "invicta_hardware", "invicta_hardware.launch", "require_can:=" + require_can]
print("[dbg] 代起节点：%s" % " ".join(cmd))
proc = subprocess.Popen(cmd)
atexit.register(lambda: proc.poll() is None and proc.terminate())
rospy.sleep(3.0) # 等节点把话题挂上
return proc
# =============================================================================
# CLI
# =============================================================================
def parse_args(argv):
p = argparse.ArgumentParser(
description="invicta_hardware 单节点调试脚本（限幅参数硬编码）",
formatter_class=argparse.RawDescriptionHelpFormatter,
)
p.add_argument("--mode", choices=["monitor", "damping", "angle", "wheel", "offset", "zero"], default="monitor",
help="monitor=只监听；damping=阻尼；angle=摆臂角度；wheel=轮速；offset=标定核对；zero=置零（默认 monitor）")
p.add_argument("--swing", default=None,
help="摆臂目标模型角 [rad]，单个或逗号分隔 4 个（fl,fr,hl,hr）。angle/offset 模式用")
p.add_argument("--swing-vel", dest="swing_vel", type=float, default=0.0,
help="摆臂目标角速度 [rad/s]，kd 会跟踪它（默认 0；>0 时不适合位置保持）")
p.add_argument("--wheel", default=None,
help="轮组目标转速 [rad/s]，单个或逗号分隔 4 个（fl,fr,hl,hr）。wheel 模式用")
p.add_argument("--joints", default=None,
help="只控制/置零这些关节（逗号分隔，如 fl_swing,fl_wheel）；默认全部 8 个。"
"对应指令 name 列表；不在列表里的关节不驱动、不置零")
p.add_argument("--kp", type=float, default=DEFAULT_KP, help="摆臂位置增益（默认 %.0f）" % DEFAULT_KP)
p.add_argument("--kd", type=float, default=DEFAULT_KD, help="速度增益（摆臂角度模式/轮速模式，默认 %.1f）" % DEFAULT_KD)
p.add_argument("--damp-joint-kd", dest="damp_joint_kd", type=float, default=DEFAULT_DAMP_JOINT_KD,
help="阻尼模式摆臂 kd（默认 %.1f）" % DEFAULT_DAMP_JOINT_KD)
p.add_argument("--damp-wheel-kd", dest="damp_wheel_kd", type=float, default=DEFAULT_DAMP_WHEEL_KD,
help="阻尼模式轮组 kd（默认 %.1f）" % DEFAULT_DAMP_WHEEL_KD)
p.add_argument("--ramp", type=float, default=0.5, help="从 0 斜坡到目标的时长 [s]，0=直接给（默认 0.5）")
p.add_argument("--rate", dest="pub_hz", type=float, default=DEFAULT_PUB_HZ, help="指令发布频率 [Hz]（默认 200）")
p.add_argument("--show-hz", dest="show_hz", type=float, default=DEFAULT_SHOW_HZ, help="角度回显频率 [Hz]（默认 5）")
p.add_argument("--duration", type=float, default=0.0, help="运行时长 [s]，0=直到 Ctrl-C（默认 0）")
p.add_argument("--start-node", dest="start_node", action="store_true", help="用 roslaunch 代起 hardware 节点")
p.add_argument("--require-can", dest="require_can", action="store_true", default=True,
help="代起节点时 require_can:=true（默认）")
p.add_argument("--no-can", dest="require_can", action="store_false",
help="代起节点时 require_can:=false（无 CAN 干跑）")
args = p.parse_args(argv)
# 预解析列表参数
args.swing = parse_list(args.swing, 4, 0.0)
args.wheel = parse_list(args.wheel, 4, 0.0)
# --joints：空格/逗号分隔的关节名，校验必须是已知关节
if args.joints:
names = [s.strip() for s in str(args.joints).replace(" ", ",").split(",") if s.strip()]
bad = [s for s in names if s not in JOINT_NAMES]
if bad:
p.error("--joints 里有未知关节名：%s（可选：%s）" % (",".join(bad), ",".join(JOINT_NAMES)))
# 去重、按关节表顺序排好
args.joints = [nm for nm in JOINT_NAMES if nm in names]
else:
args.joints = None
if args.mode in ("angle", "offset") and args.swing == [0.0] * 4:
print("[dbg] 提示：angle/offset 模式未给 --swing，目标角为 0（= 模型零位）")
if args.mode == "wheel" and args.wheel == [0.0] * 4:
print("[dbg] 提示：wheel 模式未给 --wheel，目标转速为 0")
return args
def main(argv):
args = parse_args(argv)
rospy.init_node("invicta_hw_debug", anonymous=True)
node_proc = start_node(args) if args.start_node else None
dbg = HardwareDebug(args)
def _sigint(_sig, _frm):
dbg.stop_requested = True
signal.signal(signal.SIGINT, _sigint)
print("=" * 70)
print(" invicta_hardware 调试脚本")
print(" 关节顺序: " + ", ".join(JOINT_NAMES))
print(" 控制关节: " + ", ".join(args.joints) if args.joints else " 控制关节: 全部 8 个")
print(" 话题为模型帧（dir/offset 由 hardware 节点施加）")
print(" 硬编码限幅: 摆臂角 [%.2f, %.2f] rad, 摆臂 vel %.1f, tau %.1f; 轮 vel %.1f, tau %.2f" %
(SWING_ANGLE_MIN, SWING_ANGLE_MAX, SWING_VEL_LIMIT, SWING_TAU_LIMIT, WHEEL_VEL_LIMIT, WHEEL_TAU_LIMIT))
print("=" * 70)
try:
dbg.run()
except rospy.ROSInterruptException:
dbg.safe_stop()
finally:
if node_proc is not None and node_proc.poll() is None:
node_proc.terminate()
return 0
if __name__ == "__main__":
sys.exit(main(sys.argv[1:]))
src/invicta_hardware/src/invicta_hardware_node.cpp
+461 -82
View File
@@ -1,86 +1,492 @@
/**
* @file invicta_hardware_node.cpp
* @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口）
* @brief 关节数据源 + 关节指令执行端（SocketCAN，智身模组 CANFD 协议）
*/
#include <ros/package.h>
#include <ros/ros.h>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstring>
#include <iostream>
#include <thread>
#include "invicta_hardware/hardware_config.h"
#include "invicta_hardware/hardware_bridge.h"
#include "invicta_hardware/hardware_config.h"
#include "invicta_msgs/control_word.h"
namespace invicta_hardware {
namespace {
MotorRange ToRange(const MotorSpec& m) {
MotorRange r;
r.p_min = m.p_min;
r.p_max = m.p_max;
r.v_min = m.v_min;
r.v_max = m.v_max;
r.t_min = m.t_min;
r.t_max = m.t_max;
r.kp_max = m.kp_max;
r.kd_max = m.kd_max;
return r;
}
} // namespace
HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
LoadParams(pnh);
joint_position_.assign(joint_names_.size(), 0.0);
joint_velocity_.assign(joint_names_.size(), 0.0);
joint_effort_.assign(joint_names_.size(), 0.0);
const auto& cfg = Config();
const size_t n = joint_names_.size();
joint_position_.assign(n, 0.0);
joint_velocity_.assign(n, 0.0);
joint_effort_.assign(n, 0.0);
temperature_.assign(n, 0.0);
voltage_.assign(n, 0.0);
driver_status_.assign(n, 0);
last_status_.assign(n, 0);
has_feedback_.assign(n, 0);
joint_range_ = ToRange(cfg.motor_joint);
wheel_range_ = ToRange(cfg.motor_wheel);
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>(joint_state_topic_, 1);
imu_pub_ = nh_.advertise<sensor_msgs::Imu>(imu_topic_, 1);
diagnostics_pub_ = nh_.advertise<invicta_msgs::JointDiagnostics>(diagnostics_topic_, 1);
command_sub_ = nh_.subscribe(command_topic_, command_queue_, &HardwareBridge::CommandCallback, this);
ROS_WARN("[invicta_hardware] 这是骨架节点：ReadHardware/WriteHardware 尚未实现，");
ROS_WARN("[invicta_hardware] 只会发布零位状态并缓存指令，不会驱动电机。实现见 hardware_bridge.h 的 TODO。");
ROS_INFO("[invicta_hardware] %s -> %s/%s，订阅 %s @ %.0f Hz", joint_state_topic_.c_str(), imu_topic_.c_str(),
imu_frame_id_.c_str(), command_topic_.c_str(), publish_rate_hz_);
ready_ = OpenBuses();
ROS_INFO("[invicta_hardware] %s -> %s @ %.0f Hz; diagnostics %s @ %.0f Hz", joint_state_topic_.c_str(),
command_topic_.c_str(), publish_rate_hz_, diagnostics_topic_.c_str(), diagnostics_rate_hz_);
}
bool HardwareBridge::OpenBuses() {
const auto& cfg = Config();
buses_.clear();
if (cfg.can.channels.empty()) {
ROS_ERROR("[invicta_hardware] no CAN channels in config ([motor.can].channels)");
return !require_can_;
}
for (const auto& name : cfg.can.channels) {
BusRuntime bus;
bus.name = name;
std::string err;
if (!bus.can.Open(name, &err)) {
ROS_ERROR("[invicta_hardware] failed to open %s: %s", name.c_str(), err.c_str());
ROS_ERROR("[invicta_hardware] configure the bus first: sudo ip link set %s up type can bitrate %.0f dbitrate %.0f fd on",
name.c_str(), cfg.can.bitrate, cfg.can.data_bitrate);
if (require_can_) return false;
ROS_WARN("[invicta_hardware] require_can=false, skipping %s, state published only, no motor commands sent", name.c_str());
continue;
}
// CANID → 关节下标（同一 CANID 在 can0/can1 上各有一份，互不冲突）
for (const auto& leg : cfg.legs) {
if (leg.swing_bus == name) bus.max_canid = std::max(bus.max_canid, leg.swing_canid);
if (leg.wheel_bus == name) bus.max_canid = std::max(bus.max_canid, leg.wheel_canid);
}
if (bus.max_canid <= 0 || bus.max_canid > kMaxMotorsPerFrame) {
ROS_ERROR("[invicta_hardware] invalid CANID on %s (max %d, protocol requires 1~%d)", name.c_str(), bus.max_canid,
kMaxMotorsPerFrame);
if (require_can_) return false;
continue;
}
bus.canid_to_joint.assign(size_t(bus.max_canid) + 1, -1);
for (const auto& leg : cfg.legs) {
if (leg.swing_bus == name && leg.swing_canid > 0) bus.canid_to_joint[size_t(leg.swing_canid)] = leg.swing_index;
if (leg.wheel_bus == name && leg.wheel_canid > 0) bus.canid_to_joint[size_t(leg.wheel_canid)] = leg.wheel_index;
}
bus.frame_len = FrameLenForMaxCanId(bus.max_canid);
ROS_INFO("[invicta_hardware] %s opened (CANID 1~%d, sending %d bytes)", name.c_str(), bus.max_canid, bus.frame_len);
buses_.push_back(std::move(bus));
}
if (buses_.empty() && require_can_) return false;
return true;
}
void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
// 参数来自本节点自己的 config/invicta_hardware.toml
const auto& cfg = invicta_hardware::Config();
const auto& cfg = Config();
joint_state_topic_ = cfg.topic_joint_states;
imu_topic_ = cfg.topic_imu;
command_topic_ = cfg.topic_joint_commands;
imu_frame_id_ = cfg.imu_frame_id;
diagnostics_topic_ = cfg.topic_joint_diagnostics;
publish_rate_hz_ = cfg.publish_rate_hz;
diagnostics_rate_hz_ = cfg.diagnostics_rate_hz;
command_queue_ = cfg.command_queue;
require_can_ = cfg.require_can;
command_timeout_s_ = cfg.command_timeout_s;
timeout_joint_kd_ = cfg.timeout_joint_kd;
timeout_wheel_kd_ = cfg.timeout_wheel_kd;
// launch 参数可覆盖配置（冒烟测试/无硬件调试用）
pnh.param<bool>("require_can", require_can_, require_can_);
joint_names_ = cfg.joint_names; // 顺序必须与 invicta_deploy 一致（同一份配置，天然一致）
if (publish_rate_hz_ <= 0.) {
ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0，回退到 200 Hz");
ROS_ERROR("[invicta_hardware] publish_rate_hz must be > 0, falling back to 200 Hz");
publish_rate_hz_ = 200.0;
}
if (diagnostics_rate_hz_ <= 0.) {
ROS_ERROR("[invicta_hardware] diagnostics_rate_hz must be > 0, falling back to 10 Hz");
diagnostics_rate_hz_ = 10.0;
}
}
void HardwareBridge::CommandCallback(const invicta_msgs::JointCommandArray::ConstPtr& msg) {
// 只缓存，不做任何阻塞 I/O：ROS1 的 spinner 是单线程的，回调里发使能序列（约 80 ms）
// 会把紧随其后的 kEnable/kMotorControl 挤掉。控制字在循环线程的 WriteHardware 里处理。
std::lock_guard<std::mutex> lock(cmd_mutex_);
if (msg->control_word != active_control_word_) {
HandleControlWord(msg->control_word);
active_control_word_ = msg->control_word;
}
latest_cmd_ = *msg;
has_cmd_ = true;
last_cmd_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::steady_clock::now().time_since_epoch())
.count();
}
void HardwareBridge::HandleControlWord(uint16_t word) {
// ---------------------------------------------------------------- 关节选择
std::vector<uint8_t> HardwareBridge::SelectionMask(const std::vector<std::string>& names, bool* had_unknown) const {
const size_t n = joint_names_.size();
std::vector<uint8_t> mask(n, 1); // 空列表 = 全选
if (had_unknown) *had_unknown = false;
if (names.empty()) return mask;
std::fill(mask.begin(), mask.end(), uint8_t(0));
int unknown = 0;
for (const auto& nm : names) {
const int i = Config().IndexOf(nm);
if (i < 0) {
++unknown;
continue;
}
mask[size_t(i)] = 1;
}
if (unknown > 0 && had_unknown) *had_unknown = true;
return mask;
}
// ---------------------------------------------------------------- 控制字 → CAN
void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>* include) {
switch (word) {
case invicta_msgs::kDisable:
ROS_INFO("[invicta_hardware] control_word -> kDisable(1)：清使能，电机自由");
SendCodeAll(kCodeDisable, 1);
enabled_ = false;
ROS_INFO("[invicta_hardware] control_word -> kDisable: disable sent (0xFD), coils open");
break;
case invicta_msgs::kEnable:
ROS_INFO("[invicta_hardware] control_word -> kEnable(2)：使能并保持当前角");
break;
case invicta_msgs::kMotorControl:
ROS_INFO("[invicta_hardware] control_word -> kMotorControl(3)：进入 PD+前馈控制");
SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
enabled_ = true;
ROS_INFO("[invicta_hardware] control_word -> kEnable: enable sent (0xFC x %d)", Config().can.enable_frames);
break;
case invicta_msgs::kErrorReset:
ROS_INFO("[invicta_hardware] control_word -> kErrorReset(4)：清驱动错误");
// 协议没有独立的清错指令（手册 §6：故障后靠重新使能或重启恢复）
SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
enabled_ = true;
ROS_INFO("[invicta_hardware] control_word -> kErrorReset: no fault-reset command in this protocol, handled as re-enable");
break;
case invicta_msgs::kMotorControl:
if (!enabled_) {
// 上游的启动时序可能丢（ROS1 发布无 latch，或本节点比上游后启动）。
// 协议要求先使能再运行，这里补一次使能，而不是永远不发帧。
SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
enabled_ = true;
ROS_WARN("[invicta_hardware] kMotorControl received but not enabled: enable sequence re-sent (upstream startup sequence may have been lost)");
}
ROS_INFO("[invicta_hardware] control_word -> kMotorControl: entering kp/kd + feedforward control");
break;
case invicta_msgs::kGetStatus:
ROS_INFO("[invicta_hardware] control_word -> kGetStatus(5)：上报驱动状态");
// 每条指令都会回 12 字节状态帧，无需专门请求
ROS_INFO("[invicta_hardware] control_word -> kGetStatus: status returns automatically with every command, no request needed");
break;
case invicta_msgs::kSetZero: {
// 把当前机械位置写成模组零位（0xEF）。只作用于指令 name 选中的关节子集，
// 且跳过轮毂（不支持置零，手册 §4.4）。
const size_t n = joint_names_.size();
std::vector<uint8_t> mask(n, 1);
if (include) mask = *include;
std::string sel;
for (size_t i = 0; i < n; ++i) {
if (Config().IsWheel(int(i))) mask[i] = 0;
if (mask[i]) sel += (sel.empty() ? "" : ",") + joint_names_[i];
}
if (sel.empty()) {
ROS_WARN("[invicta_hardware] kSetZero: no swing joints to set zero among the selected joints, ignoring");
break;
}
SendCodeAll(kCodeZero, 1, 0.0, &mask);
ROS_WARN("[invicta_hardware] control_word -> kSetZero: set zero sent (0xEF) to [%s]; "
"from now on the feedback angle of these swing joints at this pose is about 0, set the corresponding offset_deg to 0 (or recalibrate accordingly)",
sel.c_str());
break;
}
default:
ROS_WARN("[invicta_hardware] 未知 control_word=%u，忽略", word);
ROS_WARN("[invicta_hardware] unknown control_word=%u, ignore", word);
break;
}
// TODO(硬件)：把上面这些语义落到电机使能序列上（上电顺序、清错指令、超时保护）。
}
void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s, const std::vector<uint8_t>* include) {
std::lock_guard<std::mutex> lock(can_mutex_);
for (int f = 0; f < std::max(1, frames); ++f) {
for (auto& bus : buses_) {
if (!bus.can.is_open()) continue;
uint8_t payload[64] = {};
WriteU16(payload, kMsgId);
bool any = false;
for (int canid = 1; canid <= bus.max_canid; ++canid) {
const int joint = bus.canid_to_joint[size_t(canid)];
if (joint < 0) continue;
uint8_t* blk = payload + 2 + kBlockBytes * (canid - 1);
if (include && !(*include)[size_t(joint)]) {
// 未选中的关节：写「自由」运行块（kp=kd=0、tau=0，即零力矩）。
// ⚠ 绝不能留全 0：(0,0,0,0,0) 按协议会解码成 P_MIN/V_MIN/T_MIN，
// 其中 tau=T_MIN（摆臂 −100 N·m）会是危险的前馈力矩。
PackRunBlock(blk, 0.0, 0.0, 0.0, 0.0, 0.0,
Config().IsWheel(joint) ? wheel_range_ : joint_range_);
any = true;
continue;
}
PackCodeBlock(blk, code);
any = true;
}
if (!any) continue;
std::string err;
if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send control code 0x%02X: %s", bus.name.c_str(), code,
err.c_str());
}
}
if (f + 1 < std::max(1, frames) && interval_s > 0.) {
std::this_thread::sleep_for(std::chrono::duration<double>(interval_s));
}
}
}
// ---------------------------------------------------------------- 看门狗
bool HardwareBridge::SendTimeoutFrame() {
std::lock_guard<std::mutex> lock(can_mutex_);
for (auto& bus : buses_) {
if (!bus.can.is_open()) continue;
uint8_t payload[64] = {};
WriteU16(payload, kMsgId);
bool any = false;
for (int canid = 1; canid <= bus.max_canid; ++canid) {
const int joint = bus.canid_to_joint[size_t(canid)];
if (joint < 0) continue;
const auto& spec = Config().Joint(joint);
const bool is_wheel = spec.is_wheel;
const MotorRange& range = is_wheel ? wheel_range_ : joint_range_;
const double kd = is_wheel ? timeout_wheel_kd_ : timeout_joint_kd_;
// kp=0、速度与力矩都给 0：只留阻尼抵抗外力，不保持位置。
// kp=0 时位置指令不产生力矩，但仍按模型帧 0 换算成电机帧下发，避免读者误会。
const double pos_motor = spec.ModelToMotorPos(0.0);
PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), pos_motor, 0.0, 0.0, kd, 0.0, range);
any = true;
}
if (!any) continue;
std::string err;
if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send damping frame: %s", bus.name.c_str(), err.c_str());
}
}
return true;
}
// ---------------------------------------------------------------- 读回
bool HardwareBridge::ReadHardware() {
bool updated = false;
std::vector<CanFrame> frames;
for (auto& bus : buses_) {
if (!bus.can.is_open()) continue;
frames.clear();
bus.can.Recv(&frames, 256);
for (const auto& frame : frames) {
const int canid = int(frame.id);
const int joint = (canid > 0 && canid <= bus.max_canid) ? bus.canid_to_joint[size_t(canid)] : -1;
if (joint < 0) {
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s received unknown frame id=0x%X len=%u, ignore", bus.name.c_str(),
unsigned(frame.id), unsigned(frame.len));
continue;
}
if (frame.len < 12) {
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s CANID=%d feedback only %u bytes (protocol requires 12)", bus.name.c_str(),
canid, unsigned(frame.len));
continue;
}
MotorFeedback fb;
if (!DecodeFeedback(frame.data, frame.len, Config().IsWheel(joint) ? wheel_range_ : joint_range_, &fb)) {
continue;
}
// 电机帧 → 模型帧：位置含 offset+dir，速度/力矩只翻符号（M20 参考同此约定）
const auto& spec = Config().Joint(joint);
joint_position_[size_t(joint)] = spec.MotorToModelPos(fb.position);
joint_velocity_[size_t(joint)] = fb.velocity * spec.dir;
joint_effort_[size_t(joint)] = fb.torque * spec.dir;
temperature_[size_t(joint)] = fb.temperature;
voltage_[size_t(joint)] = fb.voltage;
driver_status_[size_t(joint)] = fb.status;
has_feedback_[size_t(joint)] = 1;
if (fb.fault() && last_status_[size_t(joint)] != fb.status) {
ROS_ERROR("[invicta_hardware] %s fault: %s (status=0x%02X, temperature %.0f C)",
joint_names_[size_t(joint)].c_str(), FaultName(fb.status), unsigned(fb.status), fb.temperature);
}
last_status_[size_t(joint)] = fb.status;
updated = true;
}
}
return updated;
}
// ---------------------------------------------------------------- 写
bool HardwareBridge::WriteHardware() {
invicta_msgs::JointCommandArray cmd;
{
std::lock_guard<std::mutex> lock(cmd_mutex_);
if (!has_cmd_) {
if (!warned_no_command_) {
warned_no_command_ = true;
ROS_WARN("[invicta_hardware] %s not received yet, not sending any command", command_topic_.c_str());
}
return false;
}
cmd = latest_cmd_;
}
const size_t n = joint_names_.size();
// 指令 name 列表 = 本次要控制的关节子集（空 = 全部已配置关节）。数组长度仍必须是 n。
bool had_unknown = false;
const std::vector<uint8_t> selected = SelectionMask(cmd.name, &had_unknown);
if (had_unknown && !warned_unknown_joint_) {
warned_unknown_joint_ = true;
ROS_WARN("[invicta_hardware] unrecognized joint name in %s name list, ignored (only recognized joints are controlled)",
command_topic_.c_str());
}
// 控制字变化在这里处理（循环线程），不阻塞订阅回调
if (cmd.control_word != applied_control_word_) {
HandleControlWord(cmd.control_word, &selected);
applied_control_word_ = cmd.control_word;
}
if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n ||
cmd.kd.size() != n) {
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] command array length does not match %zu joints, dropping frame", n);
return false;
}
for (size_t i = 0; i < n; ++i) {
if (!std::isfinite(cmd.position[i]) || !std::isfinite(cmd.velocity[i]) || !std::isfinite(cmd.effort[i]) ||
!std::isfinite(cmd.kp[i]) || !std::isfinite(cmd.kd[i])) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] command contains non-finite value, dropping frame");
return false;
}
}
// 协议要求先使能再发运行帧（手册 §9.1）
if (!enabled_) return false;
// 指令看门狗：上游（deploy）挂掉/卡住时不能一直重发最后一帧
if (command_timeout_s_ > 0.) {
const int64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::steady_clock::now().time_since_epoch())
.count();
const double age = double(now_ns - last_cmd_ns_.load()) / 1e9;
if (age > command_timeout_s_) {
if (!timeout_active_) {
timeout_active_ = true;
ROS_ERROR("[invicta_hardware] %.0f ms without %s, switching to damping (kp=0, kd=%.1f/%.1f)",
age * 1000.0, command_topic_.c_str(), timeout_joint_kd_, timeout_wheel_kd_);
}
return SendTimeoutFrame();
}
if (timeout_active_) {
timeout_active_ = false;
ROS_WARN("[invicta_hardware] command recovered, exiting damping");
}
}
std::lock_guard<std::mutex> lock(can_mutex_);
for (auto& bus : buses_) {
if (!bus.can.is_open()) continue;
uint8_t payload[64] = {};
WriteU16(payload, kMsgId);
bool any = false;
for (int canid = 1; canid <= bus.max_canid; ++canid) {
const int joint = bus.canid_to_joint[size_t(canid)];
if (joint < 0) continue;
uint8_t* blk = payload + 2 + kBlockBytes * (canid - 1);
const MotorRange& range = Config().IsWheel(joint) ? wheel_range_ : joint_range_;
// 未被 name 选中的关节：写「自由」运行块（零力矩），即不驱动也不保持。
// ⚠ 不能留全 0（会解码成 T_MIN 力矩，见 SendCodeAll 注释）。
if (!selected[size_t(joint)]) {
PackRunBlock(blk, 0.0, 0.0, 0.0, 0.0, 0.0, range);
any = true;
continue;
}
const auto& spec = Config().Joint(joint);
const std::string& name = joint_names_[size_t(joint)];
const size_t j = size_t(joint);
// 先按人为限幅夹紧（限幅与指令同为模型帧，见 [joint_limits]），再换算到电机帧编码。
// [motor.*] 只是归一化边界，不参与取最小。
const double pos = std::min(std::max(cmd.position[j], spec.lower), spec.upper);
const double vlim = spec.vel_limit > 0. ? spec.vel_limit : std::abs(range.v_max);
const double vel = std::min(std::max(cmd.velocity[j], -vlim), vlim);
const double tlim = spec.torque_limit > 0. ? spec.torque_limit : std::abs(range.t_max);
double tau = cmd.effort[j];
if (std::abs(tau) > tlim) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s feedforward torque %.1f N*m exceeds clamp %.1f, clamped", name.c_str(),
tau, tlim);
tau = std::min(std::max(tau, -tlim), tlim);
}
// 模型帧 → 电机帧：位置 q_motor=(q_model−offset)·dir，速度/力矩只乘 dir
const double pos_motor = spec.ModelToMotorPos(pos);
const double vel_motor = vel * spec.dir;
const double tau_motor = tau * spec.dir;
// kp/kd 来自配置（[policy].kp/kd、[leg.gains]），不是策略算出来的：
// 越界会被编码器静默饱和，这里显式夹紧并把配置错误报出来
double kp = cmd.kp[j];
if (kp < 0. || kp > range.kp_max) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s kp=%.1f out of module range [0, %.1f], clamped", name.c_str(),
kp, range.kp_max);
kp = std::min(std::max(kp, 0.0), range.kp_max);
}
double kd = cmd.kd[j];
if (kd < 0. || kd > range.kd_max) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s kd=%.2f out of module range [0, %.1f], clamped", name.c_str(),
kd, range.kd_max);
kd = std::min(std::max(kd, 0.0), range.kd_max);
}
PackRunBlock(blk, pos_motor, vel_motor, kp, kd, tau_motor, range);
any = true;
}
if (!any) continue;
std::string err;
if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send run frame: %s", bus.name.c_str(), err.c_str());
}
}
return true;
}
// ---------------------------------------------------------------- 发布
void HardwareBridge::PublishState() {
sensor_msgs::JointState js;
js.header.stamp = ros::Time::now();
@@ -89,71 +495,42 @@ void HardwareBridge::PublishState() {
js.velocity = joint_velocity_;
js.effort = joint_effort_;
joint_state_pub_.publish(js);
sensor_msgs::Imu imu;
imu.header.stamp = js.header.stamp;
imu.header.frame_id = imu_frame_id_;
imu.orientation.w = imu_orientation_[0];
imu.orientation.x = imu_orientation_[1];
imu.orientation.y = imu_orientation_[2];
imu.orientation.z = imu_orientation_[3];
imu.angular_velocity.x = imu_angular_velocity_[0];
imu.angular_velocity.y = imu_angular_velocity_[1];
imu.angular_velocity.z = imu_angular_velocity_[2];
imu.linear_acceleration.x = imu_linear_acceleration_[0];
imu.linear_acceleration.y = imu_linear_acceleration_[1];
imu.linear_acceleration.z = imu_linear_acceleration_[2];
// 首元素置 0 表示「该字段可用」；若某字段确实拿不到，必须置 -1，让使用方知道不能信
imu.orientation_covariance[0] = 0.;
imu.angular_velocity_covariance[0] = 0.;
imu.linear_acceleration_covariance[0] = 0.;
imu_pub_.publish(imu);
}
bool HardwareBridge::ReadHardware() {
// TODO(硬件)：SocketCAN 读回关节与 IMU，换算到 policy 约定后写入
// joint_position_ / joint_velocity_ / joint_effort_ / imu_*
// 读不到就 return false，保留上一帧值。
return false;
}
bool HardwareBridge::WriteHardware() {
invicta_msgs::JointCommandArray cmd;
{
std::lock_guard<std::mutex> lock(cmd_mutex_);
if (!has_cmd_) {
if (!warned_no_command_) {
warned_no_command_ = true;
ROS_WARN("[invicta_hardware] 还没收到 %s，不下发任何指令", command_topic_.c_str());
void HardwareBridge::PublishDiagnostics() {
invicta_msgs::JointDiagnostics msg;
msg.header.stamp = ros::Time::now();
for (size_t i = 0; i < joint_names_.size(); ++i) {
if (!has_feedback_[i]) continue; // 没反馈的关节不报，避免下游把 0 ℃ 当有效值
msg.name.push_back(joint_names_[i]);
msg.temperature.push_back(temperature_[i]);
msg.status.push_back(driver_status_[i]);
msg.voltage.push_back(voltage_[i]);
}
return false;
}
cmd = latest_cmd_;
}
// 上游 M20 的启停只看控制字，这里也先做长度与有限性检查，避免把坏指令写进电机
const size_t n = joint_names_.size();
if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n ||
cmd.kd.size() != n) {
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] 指令数组长度与 %zu 个关节不匹配，丢弃该帧", n);
return false;
}
// TODO(硬件)：raw = 反换算(零点, 方向, 单位) 后按控制字下发。
return false;
if (msg.name.empty()) return;
diagnostics_pub_.publish(msg);
}
void HardwareBridge::Run() {
running_ = true;
const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / publish_rate_hz_));
const auto diag_period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / diagnostics_rate_hz_));
auto next = std::chrono::steady_clock::now();
auto next_diag = next;
while (ros::ok() && running_) {
next += period;
ReadHardware();
PublishState();
WriteHardware();
const auto now = std::chrono::steady_clock::now();
if (now >= next_diag) {
next_diag = now + diag_period;
PublishDiagnostics();
}
std::this_thread::sleep_until(next);
}
}
@@ -172,19 +549,21 @@ int main(int argc, char** argv) {
if (config_path.empty()) config_path = ros::package::getPath("invicta_hardware") + "/config/invicta_hardware.toml";
std::string err;
if (!invicta_hardware::Config().Load(config_path, &err)) {
ROS_FATAL("[invicta_hardware] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
ROS_FATAL("[invicta_hardware] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
return 1;
}
std::cout << invicta_hardware::Config().Summary() << std::flush;
invicta_hardware::HardwareBridge bridge(pnh);
std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);
if (!bridge.Ready()) {
ROS_FATAL("[invicta_hardware] CAN init failed, exiting (for hardware-free debug set require_can:=false)");
return 1;
}
std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);
ros::spin();
bridge.Stop();
if (loop_thread.joinable()) {
loop_thread.join();
}
if (loop_thread.joinable()) loop_thread.join();
return 0;
}
src/invicta_hardware/third_party/tomlplusplus/README.md
Vendored
+13
View File
@@ -0,0 +1,13 @@
# toml++ (vendored)
这三个节点各自用到的 TOML 解析库，以 **single-header / header-only** 方式编进节点
（无 `find_package`，目标机也不需要额外 `.so`）。
- 上游：https://github.com/marzer/tomlplusplus
- 版本：v3.4.0（单头文件 `toml.hpp`）
- 取用：`toml.hpp`（amalgamated single header）。单头版本本身就是 header-only——
「drop it in, there is no step two」——文件内部在未定义时自动 `#define TOML_HEADER_ONLY 1`，
**不需要任何编译宏**。放在 `toml++/toml.hpp` 是为了匹配上游 `#include <toml++/toml.hpp>`
的路径，源码无需改动。
升级：下载新的 `toml.hpp` 覆盖本目录下 `toml++/toml.hpp` 即可（三个包各一份，保持同版本）。
src/invicta_hardware/third_party/tomlplusplus/toml++/toml.hpp
Vendored
+17897
View File
File diff suppressed because it is too large Load Diff
src/invicta_msgs/CMakeLists.txt
+1
View File
@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
JointCommandArray.msg
JointDiagnostics.msg
MotionCommand.msg
MotionState.msg
)
src/invicta_msgs/README.md
+12 -8
View File
@@ -9,12 +9,14 @@
| `/joint_states` | `sensor_msgs/JointState` | **标准** | 关节状态：`name` / `position[rad]` / `velocity[rad/s]` / `effort[Nm]` |
| `/imu/data` | `sensor_msgs/Imu` | **标准** | IMU：`orientation`（四元数，rad）、`angular_velocity[rad/s]`、`linear_acceleration[m/s²]` |
| `/joint_commands` | `invicta_msgs/JointCommandArray` | **自定义** | 关节指令：目标角/角速度/前馈力矩/`kp`/`kd`/`control_word` |
| `/joint_diagnostics` | `invicta_msgs/JointDiagnostics` | **自定义** | 低频诊断：电机温度/驱动状态码/母线电压（默认 10 Hz） |
| `/motion_command` | `invicta_msgs/MotionCommand` | **自定义** | 协议节点 → RL 节点：运动状态/步态/使用模式/轴指令 |
| `/motion_state` | `invicta_msgs/MotionState` | **自定义** | RL 节点 → 协议节点：实际运动状态/步态等 |
自定义消息只有 3 条，原因：
自定义消息只有 4 条，原因：
- `JointCommandArray`：标准消息无法表达 `kp`/`kd`/前馈力矩/控制字（`sensor_msgs/JointState` 只有 position/velocity/effort，`trajectory_msgs/JointTrajectory` 也没有增益）。
- `JointDiagnostics`：`JointState` 没有温度和状态字字段；诊断量也不该塞进 200 Hz 控制路径，所以单独一条低频话题。
- `MotionCommand` / `MotionState`：是节点间的指令/状态接口，字段与厂家协议一一对应，不适合塞进 `std_msgs`。
## 共享值域：`include/invicta_msgs/motion_defs.h`
@@ -36,20 +38,22 @@
| M20 | invicta | 变化 |
|---|---|---|
| `/JOINTS_DATA` `drdds/JointsData` | `/joint_states` `sensor_msgs/JointState` | 换标准消息；丢掉 `motion_temp`/`driver_temp`/`status_word`（见下） |
| `/JOINTS_DATA` `drdds/JointsData` | `/joint_states` `sensor_msgs/JointState` | 换标准消息；`motion_temp`/`driver_temp`/`status_word` 移到 `/joint_diagnostics`（见下） |
| `/IMU_DATA` `drdds/ImuData` | `/imu/data` `sensor_msgs/Imu` | 换标准消息；`rpy` 由**度**改为四元数 + 弧度 |
| `/JOINTS_CMD` `drdds/JointsDataCmd` | `/joint_commands` `JointCommandArray` | 控制字重新定义（平台无关） |
| `/BATTERY_DATA` `drdds/BatteryData` | 无 | RL 层不需要；若做协议状态上报，由 `invicta_hardware` 用标准消息提供 |
| `drdds/GamepadData` | 无 | 不需要：手柄走 UDP，在 `invicta_remote` 里 |
| — | `/motion_command` / `/motion_state` | 新增：协议节点与 RL 节点之间的接口 |
## 被删掉的字段与影响
## 温度与驱动状态字：`/joint_diagnostics`
`sensor_msgs/JointState` 放不下关节温度与驱动器状态字，相对 M20 少了两条保护：
`sensor_msgs/JointState` 放不下关节温度与驱动器状态字，相对 M20 会少两条保护：
| M20 字段 | 用途 | 现在的行为 |
| M20 字段 | 用途 | 现在的实现 |
|---|---|---|
| `motion_temp` | 电机过温 >100 ℃ 告警 | 温度接口返回常量，检查恒不触发 |
| `status_word` | 驱动器报错判断（`!=1` 即报错） | 状态字接口返回全 1，检查恒不触发 |
| `motion_temp` | 电机过温 >100 ℃ 告警 | `JointDiagnostics.temperature`（默认 10 Hz），协议来源是 CAN 反馈帧 Byte8 |
| `status_word` | 驱动器报错判断（`!=1` 即报错） | `JointDiagnostics.status`（Byte9 原样），invicta_deploy 侧尚未接 |
| — | 母线电压 | `JointDiagnostics.voltage`（Byte10） |
要恢复就加一条**独立低频诊断话题**（例如 1 Hz 的 `JointDiagnostics`），不要塞回 200 Hz 控制路径。协议里的设备状态上报（`1002/5`）需要的温度/电池数据，也应走 `invicta_hardware` → 协议节点这条线。
设计上一律走**独立低频诊断话题**，不塞回 200 Hz 控制路径。协议里的设备状态上报（`1002/5`）需要的
电机温度，也走 `invicta_hardware` → `/joint_diagnostics` → `invicta_remote` 这条线。
src/invicta_msgs/include/invicta_msgs/control_word.h
+1
View File
@@ -18,6 +18,7 @@ enum JointControlWord : uint16_t {
kMotorControl = 3, ///< 正常控制：tau = kp*(q_des - q) + kd*(dq_des - dq) + tau_ff
kErrorReset = 4, ///< 清除驱动错误
kGetStatus = 5, ///< 请求驱动器状态（诊断用，当前 RL 层不主动发）
kSetZero = 6, ///< 把关节当前位置设为机械零位（标定用；轮毂电机不支持，节点会跳过）
};
} // namespace invicta_msgs
src/invicta_msgs/include/invicta_msgs/error_code.h
+94
View File
@@ -0,0 +1,94 @@
/**
* @file error_code.h
* @brief 机器人错误位与协议错误码的共用定义，供 invicta_deploy（产生）与 invicta_remote（上报）共用
*
* 上游对应关系：invicta_deploy 的 `RobotErrorState`（safe_controller.hpp）是一个 32 位位域
* union，位序**必须**与本文件的 RobotErrorBit 一致（那里是位域声明顺序，这里是显式掩码）。
* deploy 通过 invicta_msgs/MotionState.msg 的 `error_code` 字段把掩码发出来，
* remote 用 RobotErrorList() 展开成协议 1002/3 的 ErrorList（proto.md §3.4）。
*/
#pragma once
#include <cstdint>
#include <vector>
namespace invicta_msgs {
/// 机器人内部错误位。位序与 invicta_deploy `RobotErrorState` 的位域声明顺序一一对应。
enum RobotErrorBit : uint32_t {
kRobotErrImuNum = 1u << 0, ///< IMU 数据含 NaN
kRobotErrImuOvertime = 1u << 1, ///< IMU 断流 / 不可信（超过 rl_imu_timeout_s 无新鲜有效帧）
kRobotErrCommandHeartbeatOvertime = 1u << 2, ///< 与上位机心跳超时
kRobotErrDriver = 1u << 3, ///< 关节驱动器状态字异常
kRobotErrMotorHeat = 1u << 4, ///< 电机过温预警
kRobotErrJointNum = 1u << 5, ///< 关节数据含 NaN / 数量不符
kRobotErrJointUpdate = 1u << 6, ///< 关节数据不更新
kRobotErrCpuHeat = 1u << 7, ///< CPU 过温
kRobotErrCpuFreq = 1u << 8, ///< CPU 降频
};
/// 协议错误码（proto.md §3.4，仅列本工程会用到的部分）
enum ProtocolErrorCode : int {
kProtoErrNone = 0,
kProtoErrMotorTempWarn = 0x8001, ///< 电机温度预警
kProtoErrDriverCommTimeout = 0x8012, ///< 与关节驱动器通讯超时
kProtoErrEncoderNoValue = 0x8016, ///< 编码器无值
kProtoErrBodyPosture = 0x8027, ///< 机身姿态错误
kProtoErrMotionPosture = 0x8029, ///< 运动姿态错误
kProtoErrCpuTempWarn = 0x8202, ///< CPU 温度过高预警
kProtoErrCpuFreqProtect = 0x8211, ///< CPU 占用率过高保护
};
/// 协议 1002/3 ErrorList 的一项
struct RobotErrorItem {
int error_code; ///< 协议 errorCode
int component; ///< bit 位标识部件（关节/电池）；本工程暂无按关节定位的信息，填 0 = 整机
const char* bit_name; ///< 触发的内部错误位名（日志用）
};
inline const char* RobotErrorBitName(uint32_t bit) {
switch (bit) {
case kRobotErrImuNum: return "IMU data contains NaN";
case kRobotErrImuOvertime: return "IMU stale or unusable";
case kRobotErrCommandHeartbeatOvertime: return "command heartbeat overtime";
case kRobotErrDriver: return "driver fault";
case kRobotErrMotorHeat: return "motor overheat warning";
case kRobotErrJointNum: return "joint data invalid";
case kRobotErrJointUpdate: return "joint data not updating";
case kRobotErrCpuHeat: return "CPU overheat warning";
case kRobotErrCpuFreq: return "CPU frequency warning";
default: return "unknown";
}
}
/// 内部错误位 → 协议 errorCode。协议表里没有对应项时返回 kProtoErrNone（只记日志不上报）。
inline int ProtocolCodeForRobotError(uint32_t bit) {
switch (bit) {
// IMU 不新鲜/姿态不可信，对上位机表现为机身姿态错误；解算结果含 NaN 则算运动姿态错误
case kRobotErrImuOvertime: return kProtoErrBodyPosture;
case kRobotErrImuNum: return kProtoErrMotionPosture;
case kRobotErrMotorHeat: return kProtoErrMotorTempWarn;
case kRobotErrDriver: return kProtoErrDriverCommTimeout;
case kRobotErrJointNum:
case kRobotErrJointUpdate: return kProtoErrEncoderNoValue;
case kRobotErrCpuHeat: return kProtoErrCpuTempWarn;
case kRobotErrCpuFreq: return kProtoErrCpuFreqProtect;
// 心跳超时在 proto.md §3.4 的错误码表里没有对应项：只记日志，不编造错误码
case kRobotErrCommandHeartbeatOvertime: return kProtoErrNone;
default: return kProtoErrNone;
}
}
/// 把内部错误位掩码展开成协议 ErrorList，跳过没有对应协议码的位。
inline std::vector<RobotErrorItem> RobotErrorList(uint32_t mask) {
std::vector<RobotErrorItem> items;
for (uint32_t bit = 1u; bit != 0u; bit <<= 1) {
if ((mask & bit) == 0u) continue;
const int code = ProtocolCodeForRobotError(bit);
if (code == kProtoErrNone) continue;
items.push_back(RobotErrorItem{code, 0, RobotErrorBitName(bit)});
}
return items;
}
} // namespace invicta_msgs
src/invicta_msgs/include/invicta_msgs/motion_defs.h
+12 -12
View File
@@ -37,23 +37,23 @@ enum UsageMode : uint8_t {
inline const char* MotionParamName(uint8_t v) {
switch (v) {
case kMotionIdle: return "空闲";
case kMotionStanding: return "站立";
case kMotionJointDamping: return "关节阻尼";
case kMotionBootDamping: return "开机阻尼";
case kMotionLieDown: return "趴下";
case kMotionRlControl: return "RL控制";
default: return "未知";
case kMotionIdle: return "Idle";
case kMotionStanding: return "Standing";
case kMotionJointDamping: return "Joint damping";
case kMotionBootDamping: return "Boot damping";
case kMotionLieDown: return "Lie down";
case kMotionRlControl: return "RL control";
default: return "Unknown";
}
}
inline const char* GaitName(uint32_t v) {
switch (v) {
case kGaitBasic: return "基础(平地)";
case kGaitStairs: return "楼梯";
case kGaitFlatAgile: return "平地(敏捷)";
case kGaitStairsAgile: return "楼梯(敏捷)";
default: return "未知";
case kGaitBasic: return "Basic (flat)";
case kGaitStairs: return "Stairs";
case kGaitFlatAgile: return "Flat (agile)";
case kGaitStairsAgile: return "Stairs (agile)";
default: return "Unknown";
}
}
src/invicta_msgs/msg/JointDiagnostics.msg
+19
View File
@@ -0,0 +1,19 @@
# 关节低频诊断量：温度 / 驱动状态码 / 母线电压。
#
# 为什么不放进 /joint_states：sensor_msgs/JointState 没有温度和状态字字段，
# 而 200 Hz 的控制路径也不该承载诊断量。本话题由 invicta_hardware 以
# [hardware].diagnostics_rate_hz（默认 10 Hz）发布。
#
# 字段来源：智身关节模组 CANFD 12 字节反馈帧（docs/zhishen_joint_module_canfd.md §3.2）
# Byte8 电机温度：raw 0~255 对应 −40~215 ℃
# Byte9 状态码：bit0 使能，bit1~6 故障（过压/过流/过温/过速/编码器/欠压）
# Byte10 母线电压：raw 0~255 直接对应 0~255 V
#
# 数组顺序与 /joint_states 的 name 一致；没有反馈的关节不出现在本消息里。
std_msgs/Header header
string[] name # 关节名，与 /joint_states 顺序一致
float64[] temperature # 电机温度 [℃]
uint8[] status # 驱动状态码（按协议原样上报，位定义见文档 §6）
float64[] voltage # 母线电压 [V]
src/invicta_msgs/msg/MotionState.msg
+1
View File
@@ -12,4 +12,5 @@ uint8 usage_mode # 使用模式：0 常规 · 1 导航 · 2 辅助
uint8 charge # 0 空闲 · 1 前往充电桩 · 2 充电中 · 3 退出充电桩 · 4 异常 · 5 在桩未充电
uint8 hes # 硬急停：0 未触发 · 1 已触发
uint8 sleep # 0 未休眠 · 1 已休眠 · 2 进入休眠中
uint32 error_code # 机器人内部错误位掩码，0 = 正常；位定义见 invicta_msgs/error_code.h
string version # 机型标识
src/invicta_remote/CMakeLists.txt
+3 -6
View File
@@ -20,12 +20,9 @@ catkin_package(
CATKIN_DEPENDS roscpp sensor_msgs invicta_msgs
)
# TOML 解析库：系统安装（见 README「依赖」），CMake 里体现为 required
find_package(tomlplusplus REQUIRED)
get_target_property(TOMLPLUSPLUS_INCLUDE_DIR tomlplusplus::tomlplusplus INTERFACE_INCLUDE_DIRECTORIES)
include_directories(${TOMLPLUSPLUS_INCLUDE_DIR})
# 以 header-only 方式使用：实现编进本节点，避免引入运行时动态库依赖（目标机无需部署 .so）
add_definitions(-DTOML_HEADER_ONLY=1)
# TOML 解析库：single-header，vendored 在 third_party/tomlplusplus/toml++/toml.hpp
# 单头版本本身就是 header-only（自带 TOML_HEADER_ONLY），实现直接编进本节点：无 find_package、无运行时库依赖
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/tomlplusplus)
include_directories(
include
src/invicta_remote/README.md
+16 -3
View File
@@ -36,11 +36,24 @@
| 1101 | 2 | 照明灯 | ❌ `0xE00A`（本平台无灯硬件） |
| 1101 | 6/7 | 休眠设置/查询 | ❌ `0xE00A` |
| 1002 | 6 | 基础状态上报 | ✅ 2 Hz（MotionState/Gait/Charge/HES/ControlUsageMode/Sleep/Version） |
| 1002 | 4 | 运控状态上报 | ✅ 10 Hz（姿态/OmegaZ + 16 关节角/轮速） |
| 1002 | 5 | 设备状态上报 | ⚠️ 2 Hz，结构完整但电池/温度/CPU/GPS 为占位 0（等硬件数据） |
| 1002 | 4 | 运控状态上报 | ✅ 10 Hz（姿态/OmegaZ + 16 槽关节，8 个实关节折叠进 M20 布局，见下） |
| 1002 | 5 | 设备状态上报 | ⚠️ 2 Hz，电机温度来自 `/joint_diagnostics`；电池/CPU/GPS 仍为占位 0 |
| 1002 | 3 | 异常上报 | ✅ 2 Hz，当前恒为空列表（暂无错误源） |
| 1003/1004/1007/2101 | — | 导航任务（Pro） | ❌ 未实现 |
### 1002/4 的关节：8 → 16 槽
协议（`proto.md` §3.2）固定 16 个关节，每腿 `HipX / HipY / Knee / Wheel`，腿序 前左、前右、后左、后右。本机每腿只有 swing + wheel 两个自由度，因此：
| M20 槽 | 本机来源 |
|---|---|
| `*HipX`（侧摆） | 不存在，恒 **0**（机构无侧摆） |
| `*HipY`（髋俯仰） | `*_swing` 关节角 [rad] |
| `*Knee`（膝） | 不存在，恒 **0**（单连杆，腿笔直） |
| `*Wheel` | `*_wheel` 轮转速 [rad/s] |
`MotorStatus.Joint` 数组与 16 个具名字段用同一套值（非轮为角度、轮为转速）。保留 16 槽而不裁成 8 槽，是为了让同一套 App 不必区分机型；两个恒 0 槽不是缺数据，而是当前机构的正确取值，客户端姿态显示不会异常。映射优先按 `/joint_states` 的关节名解析（`fl_swing`/`fl_wheel` …），无 name 时退回「每腿两个、先 swing 后 wheel」的下标顺序。
未实现项默认回 `0xE00A "Unsupported function"`；若某个 App 把非 0 当致命错误，把参数 `unsupported_as_success` 设为 `true`（只影响这些未实现项，校验类错误码照旧）。
错误码采用厂家定义（`0xE001`～`0xE00B`），见 `include/invicta_remote/protocol_defs.h`。
@@ -50,7 +63,7 @@
| TOML 段 | 内容 |
|---|---|
| `[protocol]` | `udp_port`、`heartbeat_timeout_s`、`unsupported_as_success` |
| `[topics]` | `motion_command` / `motion_state` / `joint_states` / `imu` 话题名 |
| `[topics]` | `motion_command` / `motion_state` / `joint_states` / `imu` / `joint_diagnostics` 话题名 |
| `[telemetry]` | 四路上报的开关与频率（`enabled`、`require_subscription`、`basic_hz`/`motion_hz`/`device_hz`/`error_hz`） |
launch 只传 `config_file`（默认 `$(find invicta_remote)/config/invicta_remote.toml`）。
src/invicta_remote/config/invicta_remote.toml
+1
View File
@@ -17,6 +17,7 @@ motion_command = "motion_command" # 发布 invicta_msgs/MotionCommand（invic
motion_state = "motion_state" # 订阅 invicta_msgs/MotionState（判前提 + 状态上报）
joint_states = "joint_states" # 订阅（1002/4 运控状态上报用）
imu = "imu/data" # 订阅（状态上报用）
joint_diagnostics = "joint_diagnostics" # 订阅（1002/5 设备状态的电机温度用）
# ------------------------------------------------------------------ 状态上报（协议 1002/*）
[telemetry]
src/invicta_remote/include/invicta_remote/heartbeat.h
+3 -3
View File
@@ -52,7 +52,7 @@ public:
c.port = port;
c.last_seen = std::chrono::steady_clock::now();
clients_.push_back(c);
std::cout << "[GAMEPAD] 新客户端 " << ip << ":" << port << std::endl;
std::cout << "[GAMEPAD] new client " << ip << ":" << port << std::endl;
}
/// 记录某客户端订阅了哪一路状态上报
@@ -91,8 +91,8 @@ private:
const double idle =
std::chrono::duration_cast<std::chrono::duration<double>>(now - it->last_seen).count();
if (idle > timeout_sec_) {
std::cout << "[GAMEPAD] 客户端 " << it->ip << ":" << it->port << " 超过 " << timeout_sec_
<< " s 无报文，标记离线" << std::endl;
std::cout << "[GAMEPAD] client " << it->ip << ":" << it->port << " exceeded " << timeout_sec_
<< " s without packets, marked offline" << std::endl;
it = clients_.erase(it);
offline_logged = true;
} else {
src/invicta_remote/include/invicta_remote/remote_config.h
+9 -7
View File
@@ -24,6 +24,7 @@ public:
std::string topic_motion_state = "motion_state";
std::string topic_joint_states = "joint_states";
std::string topic_imu = "imu/data";
std::string topic_joint_diagnostics = "joint_diagnostics";
// ---- 状态上报（1002/*）----
bool telemetry_enabled = true;
@@ -71,21 +72,22 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
try {
tbl = toml::parse_file(path);
} catch (const toml::parse_error& e) {
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
return fail(std::string("TOML parse failed: ") + std::string(e.description()) + " @ " + path);
}
const toml::table* proto = detail::Section(tbl, "protocol");
if (!proto) return fail("缺少 [protocol] 段");
if (!proto) return fail("missing [protocol] section");
detail::Get(proto, "udp_port", udp_port);
detail::Get(proto, "heartbeat_timeout_s", heartbeat_timeout_s);
detail::GetBool(proto, "unsupported_as_success", unsupported_as_success);
if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port 非法");
if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port out of range");
const toml::table* topics = detail::Section(tbl, "topics");
detail::GetStr(topics, "motion_command", topic_motion_command);
detail::GetStr(topics, "motion_state", topic_motion_state);
detail::GetStr(topics, "joint_states", topic_joint_states);
detail::GetStr(topics, "imu", topic_imu);
detail::GetStr(topics, "joint_diagnostics", topic_joint_diagnostics);
const toml::table* tlm = detail::Section(tbl, "telemetry");
detail::GetBool(tlm, "enabled", telemetry_enabled);
@@ -101,11 +103,11 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
inline std::string RemoteConfig::Summary() const {
char buf[256];
snprintf(buf, sizeof(buf), "[config] UDP %d, 心跳超时 %.1f s, 未实现项回 0xE00A=%s\n", udp_port,
heartbeat_timeout_s, unsupported_as_success ? "否(回 0)" : "是");
snprintf(buf, sizeof(buf), "[config] UDP %d, heartbeat timeout %.1f s, unsupported item returns 0xE00A=%s\n", udp_port,
heartbeat_timeout_s, unsupported_as_success ? "no (returns 0)" : "yes");
std::string s = buf;
snprintf(buf, sizeof(buf), "[config] 状态上报: %s 基础 %.0f Hz / 运控 %.0f Hz / 设备 %.0f Hz / 异常 %.0f Hz\n",
telemetry_enabled ? "开" : "关", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz,
snprintf(buf, sizeof(buf), "[config] telemetry: %s basic %.0f Hz / motion %.0f Hz / device %.0f Hz / error %.0f Hz\n",
telemetry_enabled ? "on" : "off", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz,
telemetry_error_hz);
s += buf;
return s;
src/invicta_remote/include/invicta_remote/remote_node.h
+161 -39
View File
@@ -35,8 +35,11 @@
#include <vector>
#include "heartbeat.h"
#include "invicta_msgs/JointCommandArray.h"
#include "invicta_msgs/JointDiagnostics.h"
#include "invicta_msgs/MotionCommand.h"
#include "invicta_msgs/MotionState.h"
#include "invicta_msgs/error_code.h"
#include "remote_config.h"
#include "json.hpp"
#include "protocol_defs.h"
@@ -56,6 +59,7 @@ public:
motion_state_topic_ = cfg_.topic_motion_state;
joint_states_topic_ = cfg_.topic_joint_states;
imu_topic_ = cfg_.topic_imu;
joint_diagnostics_topic_ = cfg_.topic_joint_diagnostics;
heartbeat_timeout_s_ = cfg_.heartbeat_timeout_s;
unsupported_as_success_ = cfg_.unsupported_as_success;
@@ -71,6 +75,8 @@ public:
motion_state_sub_ = nh_.subscribe(motion_state_topic_, 10, &RemoteNode::MotionStateCallback, this);
joint_state_sub_ = nh_.subscribe(joint_states_topic_, 10, &RemoteNode::JointStateCallback, this);
imu_sub_ = nh_.subscribe(imu_topic_, 10, &RemoteNode::ImuCallback, this);
diagnostics_sub_ =
nh_.subscribe(joint_diagnostics_topic_, 10, &RemoteNode::JointDiagnosticsCallback, this);
RegisterHandlers();
}
@@ -81,13 +87,13 @@ public:
udp_server_ = std::make_unique<UdpServer>([this](const Frame& f) { return OnFrame(f); }, udp_port_);
StartTelemetry();
ROS_INFO_STREAM("[invicta_remote] 协议节点就绪：" << motion_command_topic_ << " <- " << motion_state_topic_);
ROS_INFO("[invicta_remote] 已实现：心跳 / 轴指令(2,21) / 运动状态(2,22) / 步态(2,23) / "
"使用模式(1101,5 仅常规) / 按键(1101,12)");
ROS_INFO("[invicta_remote] 状态上报：1002/6 %.0f Hz、1002/4 %.0f Hz、1002/5 %.0f Hz、1002/3 %.0f Hz"
"（%s）", basic_hz_, motion_hz_, device_hz_, error_hz_,
require_subscription_ ? "需先发查询订阅" : "主动推送");
ROS_INFO("[invicta_remote] 未实现：照明灯 休眠 充电 导航 速度指令（回 0xE00A）");
ROS_INFO_STREAM("[invicta_remote] protocol node ready: " << motion_command_topic_ << " <- " << motion_state_topic_);
ROS_INFO("[invicta_remote] implemented: heartbeat / axis command(2,21) / motion state(2,22) / gait(2,23) / "
"usage mode(1101,5 normal only) / key(1101,12)");
ROS_INFO("[invicta_remote] telemetry: 1002/6 %.0f Hz, 1002/4 %.0f Hz, 1002/5 %.0f Hz, 1002/3 %.0f Hz"
"(%s)", basic_hz_, motion_hz_, device_hz_, error_hz_,
require_subscription_ ? "query subscription required" : "push autonomously");
ROS_INFO("[invicta_remote] not implemented: light, sleep, charge, navigation, velocity command (reply 0xE00A)");
}
private:
@@ -117,7 +123,7 @@ private:
try {
doc = json::parse(frame.body);
} catch (const std::exception& e) {
std::cout << "[GAMEPAD] JSON 解析失败：" << e.what() << std::endl;
std::cout << "[GAMEPAD] JSON parse failed: " << e.what() << std::endl;
return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
}
@@ -205,15 +211,25 @@ private:
actual_charge_ = msg->charge;
actual_hes_ = msg->hes;
actual_sleep_ = msg->sleep;
actual_error_code_ = msg->error_code;
actual_version_ = msg->version;
have_state_ = true;
}
void JointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) {
std::lock_guard<std::mutex> lock(state_mutex_);
joint_names_ = msg->name;
joint_pos_.assign(msg->position.begin(), msg->position.end());
joint_vel_.assign(msg->velocity.begin(), msg->velocity.end());
have_joints_ = joint_pos_.size() == 16;
// 本机 8 关节（每腿 swing + wheel），不是 M20 的 16；上报时由 FillM20Joints 折叠到 16 槽
have_joints_ = !joint_pos_.empty();
}
void JointDiagnosticsCallback(const invicta_msgs::JointDiagnostics::ConstPtr& msg) {
std::lock_guard<std::mutex> lock(state_mutex_);
diag_names_ = msg->name;
diag_temperature_.assign(msg->temperature.begin(), msg->temperature.end());
diag_status_.assign(msg->status.begin(), msg->status.end());
}
void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
@@ -230,6 +246,77 @@ private:
have_imu_ = true;
}
// ---------------------------------------------------------------- M20 16 关节布局
// 协议 §3.2 固定 16 槽：每腿 [HipX, HipY, Knee, Wheel]，腿序 前左/前右/后左/后右。
// 本机每腿只有 swing + wheel：HipY ← 摆臂角，Wheel ← 轮转速。
// HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，恒为 0——这不是缺数据，而是当前机构
// 「无侧摆、腿笔直」的正确取值，客户端按 16 槽解析时姿态不会异常。
// 保留 16 槽而不是裁成 8 槽，是为了让同一套 App 不必区分机型。
static constexpr int kM20Joints = 16;
static const char* M20JointName(int slot) {
static const char* kNames[kM20Joints] = {
"LeftFrontHipX", "LeftFrontHipY", "LeftFrontKnee", "LeftFrontWheel",
"RightFrontHipX", "RightFrontHipY", "RightFrontKnee", "RightFrontWheel",
"LeftBackHipX", "LeftBackHipY", "LeftBackKnee", "LeftBackWheel",
"RightBackHipX", "RightBackHipY", "RightBackKnee", "RightBackWheel"};
return kNames[slot];
}
/// 关节名 → 腿序号（0 前左 fl、1 前右 fr、2 后左 hl、3 后右 hr）；不认识返回 -1
static int LegIndexFromJointName(const std::string& n) {
if (n.compare(0, 2, "fl") == 0) return 0;
if (n.compare(0, 2, "fr") == 0) return 1;
if (n.compare(0, 2, "hl") == 0) return 2;
if (n.compare(0, 2, "hr") == 0) return 3;
return -1;
}
/**
* @brief 一组关节名 → M20 的 16 槽
*
* @param names 关节名（/joint_states 或 /joint_diagnostics 的 name）
* @param value_count 该消息实际带的数据条数（无 name 时按它做下标回退）
* @param slot_index 输出：槽 → names 下标，-1 表示本机没有这个自由度
*
* HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，永远 -1；swing → HipY，wheel → Wheel。
* 优先按 name 解析（fl_swing / fl_wheel …），无 name 时退回「每腿两个、先 swing 后 wheel」。
*/
static void BuildSlotToIndex(const std::vector<std::string>& names, size_t value_count,
int slot_index[kM20Joints]) {
for (int i = 0; i < kM20Joints; ++i) slot_index[i] = -1;
const bool by_name = !names.empty() && names.size() == value_count;
const size_t n = by_name ? names.size() : std::min(value_count, size_t(kM20Joints / 2));
for (size_t k = 0; k < n; ++k) {
int leg = -1;
bool is_wheel = false;
if (by_name) {
leg = LegIndexFromJointName(names[k]);
if (leg < 0) continue;
const bool wheel = names[k].find("wheel") != std::string::npos;
const bool swing = names[k].find("swing") != std::string::npos;
if (!wheel && !swing) continue;
is_wheel = wheel;
} else {
leg = int(k / 2);
is_wheel = (k % 2 == 1);
}
slot_index[leg * 4 + (is_wheel ? 3 : 1)] = int(k);
}
}
/// 16 槽取值：协议里 Joint 数组与具名字段的混合量（角度，轮为 rad/s）
void FillM20Joints(double value[kM20Joints]) const {
for (int i = 0; i < kM20Joints; ++i) value[i] = 0.0;
int slot_index[kM20Joints];
BuildSlotToIndex(joint_names_, std::min(joint_pos_.size(), joint_vel_.size()), slot_index);
for (int slot = 0; slot < kM20Joints; ++slot) {
const int k = slot_index[slot];
if (k < 0) continue;
value[slot] = (slot % 4 == 3) ? joint_vel_[size_t(k)] : joint_pos_[size_t(k)];
}
}
// ---------------------------------------------------------------- 状态上报
json BasicDoc(int type, int command) {
std::lock_guard<std::mutex> lock(state_mutex_);
@@ -268,19 +355,14 @@ private:
ms["RemainMile"] = 0.0; // 待电池/里程
auto& motor = items["MotorStatus"];
std::vector<double> joint = joint_pos_;
std::vector<double> vel = joint_vel_;
joint.resize(16, 0.0);
vel.resize(16, 0.0);
motor["Joint"] = joint;
static const char* kNames[16] = {
"LeftFrontHipX", "LeftFrontHipY", "LeftFrontKnee", "LeftFrontWheel",
"RightFrontHipX", "RightFrontHipY", "RightFrontKnee", "RightFrontWheel",
"LeftBackHipX", "LeftBackHipY", "LeftBackKnee", "LeftBackWheel",
"RightBackHipX", "RightBackHipY", "RightBackKnee", "RightBackWheel"};
for (int i = 0; i < 16; ++i) {
motor[kNames[i]] = (i % 4 == 3) ? vel[i] : joint[i]; // 轮关节报转速，其余报角度
double value[kM20Joints];
FillM20Joints(value);
if (!have_joints_ && std::chrono::steady_clock::now() - start_time_ > std::chrono::seconds(2)) {
ROS_WARN_THROTTLE(30.0, "[invicta_remote] %s not received yet, 1002/4 joints reported as 0",
joint_states_topic_.c_str());
}
motor["Joint"] = std::vector<double>(value, value + kM20Joints);
for (int i = 0; i < kM20Joints; ++i) motor[M20JointName(i)] = value[i];
doc["PatrolDevice"]["Type"] = type;
doc["PatrolDevice"]["Command"] = command;
doc["PatrolDevice"]["Time"] = CurrentDateTime();
@@ -288,9 +370,10 @@ private:
}
json DeviceDoc(int type, int command) {
std::lock_guard<std::mutex> lock(state_mutex_);
json doc;
json& items = doc["PatrolDevice"]["Items"];
// 电池 / 温度 / CPU / GPS 需要硬件桥接提供数据，当前用 0 占位（Version 除外）
// 电池 / CPU / GPS 还没有数据源，用 0 占位；电机温度来自 /joint_diagnostics
json battery = {{"Voltage", 0.0}, {"BatteryLevel", 0.0}, {"battery_temperature", 0.0},
{"charge", false}, {"serial", ""}};
items["BatteryList"] = json::array({battery, battery});
@@ -298,9 +381,24 @@ private:
{"BatteryLevelLeft", 0.0}, {"BatteryLevelRight", 0.0},
{"battery_temperatureLeft", 0.0}, {"battery_temperatureRight", 0.0},
{"chargeLeft", false}, {"chargeRight", false}};
json zeros16 = json::array();
for (int i = 0; i < 16; ++i) zeros16.push_back(0.0);
items["DeviceTemperature"] = {{"Motor", zeros16}, {"Driver", zeros16}};
// 温度按 M20 布局展开：本机 8 个电机落在对应槽（swing → HipY、wheel → Wheel），
// 其余槽没有对应电机填 0；驱动器温度本协议不返回，恒 0。
int slot_index[kM20Joints];
BuildSlotToIndex(diag_names_, diag_temperature_.size(), slot_index);
json motor_temp = json::array();
json driver_temp = json::array();
for (int slot = 0; slot < kM20Joints; ++slot) {
const int k = slot_index[slot];
motor_temp.push_back((k >= 0 && size_t(k) < diag_temperature_.size()) ? diag_temperature_[size_t(k)] : 0.0);
driver_temp.push_back(0.0);
}
items["DeviceTemperature"] = {{"Motor", motor_temp}, {"Driver", driver_temp}};
auto& temps = items["DeviceTemperature"];
for (int slot = 0; slot < kM20Joints; ++slot) {
const std::string base = M20JointName(slot);
temps[base + "Motor"] = motor_temp[slot];
temps[base + "Driver"] = 0.0;
}
items["Led"] = {{"Fill", {{"Front", 0}, {"Back", 0}}}};
items["GPS"] = {{"Latitude", 0.0}, {"Longitude", 0.0}, {"Speed", 0.0}, {"Course", 0.0},
{"FixQuality", 0}, {"NumSatellites", 0}, {"Altitude", 0.0},
@@ -319,7 +417,21 @@ private:
json ErrorDoc(int type, int command) {
json doc;
doc["PatrolDevice"]["Items"]["ErrorList"] = json::array(); // 暂无错误源，空列表
json list = json::array();
uint32_t mask = 0;
{
std::lock_guard<std::mutex> lock(state_mutex_);
mask = actual_error_code_;
}
// 内部错误位 → 协议 errorCode（proto.md §3.4）；没有对应协议码的位（如心跳超时）只在上报里省略
for (const auto& item : RobotErrorList(mask)) {
json e;
e["errorCode"] = item.error_code;
e["component"] = item.component;
list.push_back(e);
}
if (list.empty()) list = json::array();
doc["PatrolDevice"]["Items"]["ErrorList"] = list;
doc["PatrolDevice"]["Type"] = type;
doc["PatrolDevice"]["Command"] = command;
doc["PatrolDevice"]["Time"] = CurrentDateTime();
@@ -331,7 +443,7 @@ private:
heartbeat_->SetSubscribed(frame.ip, frame.port, command);
if (!subscribed_logged_.count({frame.ip, command})) {
subscribed_logged_.insert({frame.ip, command});
std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " 查询状态上报 1002/" << command
std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " query telemetry 1002/" << command
<< std::endl;
}
switch (command) {
@@ -405,7 +517,7 @@ private:
const auto now = std::chrono::steady_clock::now();
if (now - last_heartbeat_log_ > std::chrono::seconds(2)) {
last_heartbeat_log_ = now;
std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << "（状态上报已开始推送）"
std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << " (telemetry push started)"
<< std::endl;
}
return Result::Ok();
@@ -466,8 +578,8 @@ private:
std::lock_guard<std::mutex> lock(state_mutex_);
if (have_state_ && actual_motion_state_ != kMotionRlControl) {
return Result::Err(kErrNotAllowedOperation,
std::string("Not allowed operation: 步态切换仅 RL 控制状态下有效（当前 ") +
MotionParamName(actual_motion_state_) + "）");
std::string("Not allowed operation: gait switch only valid in RL control state (current ") +
MotionParamName(actual_motion_state_) + ")");
}
}
latest_.gait = uint32_t(gait);
@@ -487,7 +599,7 @@ private:
PublishMotionCommand();
return Result::Ok();
}
return Unsupported("使用模式 " + std::to_string(mode) + "（仅支持常规模式）");
return Unsupported("usage mode " + std::to_string(mode) + " (only normal mode supported)");
}
/// 1101/12 按键事件：新版 App（≥ V1.5.11）用按键，这里映射到同一套运动状态
@@ -505,7 +617,7 @@ private:
latest_.target_state = it->second;
PublishMotionCommand();
} else {
std::cout << "[GAMEPAD] 未知按键 " << name << std::endl;
std::cout << "[GAMEPAD] unknown key " << name << std::endl;
}
}
}
@@ -515,10 +627,10 @@ private:
/// 未实现的功能：统一返回 0xE00A（可用参数改成回 0）
Result Unsupported(const std::string& what) {
if (unsupported_as_success_) {
std::cout << "[GAMEPAD] " << what << " 未实现，按 unsupported_as_success=true 回 0" << std::endl;
std::cout << "[GAMEPAD] " << what << " not implemented, reply 0 per unsupported_as_success=true" << std::endl;
return Result::Ok();
}
std::cout << "[GAMEPAD] " << what << " 未实现，回 0xE00A" << std::endl;
std::cout << "[GAMEPAD] " << what << " not implemented, reply 0xE00A" << std::endl;
return Result::Err(kErrUnsupportedFunction, "Unsupported function: " + what);
}
@@ -529,18 +641,18 @@ private:
handlers_[{kTypeMotion, kCmdAxisControl}] = [this](json& i, const Frame& f) { return HandleAxis(i, f); };
handlers_[{kTypeMotion, kCmdMotionState}] = [this](json& i, const Frame& f) { return HandleMotionState(i, f); };
handlers_[{kTypeMotion, kCmdGait}] = [this](json& i, const Frame& f) { return HandleGait(i, f); };
handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("自主充电 2/24"); };
handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("autonomous charging 2/24"); };
handlers_[{kTypeMotion, kCmdVelocity}] = [this](json&, const Frame&) {
return Unsupported("速度指令 2/25（导航模式）");
return Unsupported("velocity command 2/25 (navigation mode)");
};
handlers_[{kTypeDevice, kCmdLight}] = [this](json&, const Frame&) {
return Unsupported("照明灯 1101/2（本平台无灯硬件）");
return Unsupported("light 1101/2 (no light hardware on this platform)");
};
handlers_[{kTypeDevice, kCmdUsageMode}] = [this](json& i, const Frame& f) { return HandleUsageMode(i, f); };
handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("休眠设置 1101/6"); };
handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("sleep set 1101/6"); };
handlers_[{kTypeDevice, kCmdSleepQuery}] = [this](json&, const Frame&) {
return Unsupported("休眠查询 1101/7");
return Unsupported("sleep query 1101/7");
};
handlers_[{kTypeDevice, kCmdKey}] = [this](json& i, const Frame& f) { return HandleKey(i, f); };
@@ -569,6 +681,7 @@ private:
ros::Subscriber motion_state_sub_;
ros::Subscriber joint_state_sub_;
ros::Subscriber imu_sub_;
ros::Subscriber diagnostics_sub_;
std::unique_ptr<Heartbeat> heartbeat_;
std::unique_ptr<UdpServer> udp_server_;
@@ -582,6 +695,7 @@ private:
std::string motion_state_topic_ = "motion_state";
std::string joint_states_topic_ = "joint_states";
std::string imu_topic_ = "imu/data";
std::string joint_diagnostics_topic_ = "joint_diagnostics";
double heartbeat_timeout_s_ = 3.0;
bool unsupported_as_success_ = false;
@@ -610,10 +724,18 @@ private:
uint32_t actual_gait_ = kGaitBasic;
uint8_t actual_usage_mode_ = kUsageNormal;
uint8_t actual_charge_ = 0, actual_hes_ = 0, actual_sleep_ = 0;
/// 机器人内部错误位掩码（来自 /motion_state）；展开成协议 ErrorList 见 invicta_msgs/error_code.h
uint32_t actual_error_code_ = 0;
std::string actual_version_ = "INVICTA";
bool have_state_ = false;
std::vector<std::string> joint_names_;
std::vector<double> joint_pos_, joint_vel_;
bool have_joints_ = false;
/// 启动后 2 s 内不报「还没收到关节状态」：节点刚起来时上游本来就还没发
const std::chrono::steady_clock::time_point start_time_ = std::chrono::steady_clock::now();
std::vector<std::string> diag_names_;
std::vector<double> diag_temperature_;
std::vector<uint8_t> diag_status_;
double rpy_[3] = {0., 0., 0.};
double omega_z_ = 0.;
bool have_imu_ = false;
src/invicta_remote/include/invicta_remote/udp_server.h
+1 -1
View File
@@ -154,7 +154,7 @@ private:
uint8_t from_type = kFromTypeUser;
bool format_ok = true;
if (ParseHeader(buffer.data(), int(recvLen), body_len, data_id, from_type, format_ok) < 0) {
std::cout << "[UdpServer] 非法帧头，丢弃（长度 " << recvLen << "）" << std::endl;
std::cout << "[UdpServer] invalid frame header, drop (length " << recvLen << ")" << std::endl;
continue;
}
src/invicta_remote/src/invicta_remote_node.cpp
+1 -1
View File
@@ -19,7 +19,7 @@ int main(int argc, char** argv) {
if (config_path.empty()) config_path = ros::package::getPath("invicta_remote") + "/config/invicta_remote.toml";
std::string err;
if (!invicta_remote::Config().Load(config_path, &err)) {
ROS_FATAL("[invicta_remote] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
ROS_FATAL("[invicta_remote] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
return 1;
}
std::cout << invicta_remote::Config().Summary() << std::flush;
src/invicta_remote/third_party/tomlplusplus/README.md
Vendored
+13
View File
@@ -0,0 +1,13 @@
# toml++ (vendored)
这三个节点各自用到的 TOML 解析库，以 **single-header / header-only** 方式编进节点
（无 `find_package`，目标机也不需要额外 `.so`）。
- 上游：https://github.com/marzer/tomlplusplus
- 版本：v3.4.0（单头文件 `toml.hpp`）
- 取用：`toml.hpp`（amalgamated single header）。单头版本本身就是 header-only——
「drop it in, there is no step two」——文件内部在未定义时自动 `#define TOML_HEADER_ONLY 1`，
**不需要任何编译宏**。放在 `toml++/toml.hpp` 是为了匹配上游 `#include <toml++/toml.hpp>`
的路径，源码无需改动。
升级：下载新的 `toml.hpp` 覆盖本目录下 `toml++/toml.hpp` 即可（三个包各一份，保持同版本）。
src/invicta_remote/third_party/tomlplusplus/toml++/toml.hpp
Vendored
+17897
View File
File diff suppressed because it is too large Load Diff