feat(hardware): 接入 SocketCAN 与智身模组协议，补温度诊断和指令看门狗
- hardware: 新增 can_bus.h / joint_module_protocol.h，实现使能/失能/运行帧与 12 字节反馈帧编解码；ReadHardware/WriteHardware 落地，控制字按协议语义映射 （kErrorReset 按重新使能处理，kGetStatus 无需请求） - hardware: 温度/驱动状态/电压经 /joint_diagnostics 发布（默认 10 Hz，可配置） - hardware: 指令看门狗，100 ms 无指令转阻尼，避免上游挂掉后重发最后一帧 - hardware: 限幅与归一化边界分离——[joint_limits] 是唯一实际限幅，加载期校验 其不超过 [motor.*] 的归一化边界；力矩按峰值扭矩夹紧，kp/kd 越界夹紧并告警 - hardware: require_can 开关，无 CAN 时可只发状态（冒烟测试用） - remote: 1002/4 关节按 M20 的 16 槽上报（swing→HipY、wheel→Wheel，其余占位 0）， 1002/5 温度改由 /joint_diagnostics 提供 - deploy: 启动时序等订阅者出现再发（ROS1 无 latch，否则整段丢失） - msgs: 新增 JointDiagnostics - docs: 智身模组 CANFD 协议整理、待办清单
This commit is contained in:
@@ -42,6 +42,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}
|
||||
)
|
||||
|
||||
|
||||
@@ -1,58 +1,92 @@
|
||||
# invicta_hardware
|
||||
|
||||
关节/IMU 数据源与关节指令执行端。**当前是骨架**：结构、话题、参数都齐了，只有 `ReadHardware()` / `WriteHardware()` 是待实现的 TODO。
|
||||
关节/IMU 数据源与关节指令执行端。已接 **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` 与 `sensor_msgs/Imu`
|
||||
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) | 不需要发帧：状态随每条指令自动返回 |
|
||||
|
||||
同一路总线上最多 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`（默认取厂家峰值扭矩）夹紧，超过也告警。
|
||||
- **本节点收发的都是原始电机量**（电机坐标系/电机单位），`dir`/`offset_deg` 由 `invicta_deploy` 的 `InvictaInterface` 施加。如果改成由本节点施加，必须同时把 deploy 的 `[legs].dir/offset` 置为恒等——否则 `dir=±1` 会二次相乘抵消、`offset_deg` 会翻倍。
|
||||
- IMU：姿态四元数（**弧度**）、角速度 `[rad/s]`（机体系）、线加速度 `[m/s²]`（机体系）。⚠️ 目前**还没有 IMU 数据源**，本节点发的是单位四元数，等硬件接上再改。
|
||||
- 温度取自反馈帧 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` 的 `bus`、`canid`（`dir`/`offset_deg` 目前由 deploy 施加） |
|
||||
| `[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` 的匹配）。
|
||||
|
||||
@@ -13,13 +13,19 @@ joint_names = [
|
||||
"hr_swing", "hr_wheel",
|
||||
]
|
||||
|
||||
# ------------------------------------------------------------------ 关节限位
|
||||
# 顺序与 joint_names 一致；轮关节的 lower/upper 填 ±100 表示不限（用 vel_limit 约束）
|
||||
# 本节点实现 I/O 时用它做下发前的自检
|
||||
# ------------------------------------------------------------------ 关节限位（唯一实际生效的限幅）
|
||||
# 顺序与 joint_names 一致。这里才是「限幅」：机械/使用上的可用范围，下发前按它夹紧。
|
||||
# 与 [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；反馈反向。
|
||||
@@ -56,7 +62,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 +81,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 +102,31 @@ 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_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"
|
||||
command_queue = 1 # 指令只取最新一帧
|
||||
publish_rate_hz = 200.0 # 关节/IMU 状态发布频率（RL 侧控制环 200 Hz）
|
||||
diagnostics_rate_hz = 10.0 # 诊断量频率：CAN 反馈每条指令都有，但话题按低频发
|
||||
imu_frame_id = "base_link"
|
||||
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
|
||||
|
||||
@@ -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
|
||||
@@ -1,19 +1,21 @@
|
||||
/**
|
||||
* @file hardware_bridge.h
|
||||
* @brief 关节/IMU 数据源与关节指令执行端（骨架，硬件 I/O 待实现）
|
||||
* @brief 关节/IMU 数据源与关节指令执行端（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/data
|
||||
* 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，节点只会：
|
||||
* - 发布全零的关节状态与单位四元数姿态（说明数据源还没接上）
|
||||
* - 缓存指令并打印控制字变化，不会真的驱动电机
|
||||
* 与 invicta_deploy 的分工（重要）：
|
||||
* 本节点在话题上收发的都是**原始电机量**（电机坐标系、电机单位），
|
||||
* dir / offset 由 invicta_deploy 的 InvictaInterface 施加（它已经这么做了）。
|
||||
* 如果改成由本节点施加，必须同时把 deploy 的 [legs].dir/offset 置为恒等，
|
||||
* 否则 dir=±1 会二次相乘抵消、offset_deg 会翻倍。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
@@ -29,7 +31,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,67 +42,91 @@ 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 中落实
|
||||
/// 控制字变化：使能/失能按语义落成 CAN 帧
|
||||
void HandleControlWord(uint16_t word);
|
||||
|
||||
/**
|
||||
* @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）。
|
||||
*/
|
||||
bool ReadHardware();
|
||||
/// 向所有总线各发若干帧「9×0xFF + code」指令块
|
||||
void SendCodeAll(uint8_t code, int frames = 1, double interval_s = 0.0);
|
||||
|
||||
/**
|
||||
* @brief 把最新指令下发给电机
|
||||
*
|
||||
* TODO(硬件)：把 policy 约定的量换算成 raw（零点、方向、单位），
|
||||
* 按 invicta_msgs/control_word.h 的语义设置使能状态，组装 CAN 帧写下去。
|
||||
* torque = kp*(q_des - q) + kd*(dq_des - dq) + tau_ff 由驱动器或本节点计算，
|
||||
* 取决于关节模组协议，二选一并写清楚。
|
||||
*/
|
||||
/// 指令超时：kp=0、速度 0、只留阻尼，防止上游挂掉后一直重发最后一帧
|
||||
bool SendTimeoutFrame();
|
||||
|
||||
bool ReadHardware();
|
||||
bool WriteHardware();
|
||||
|
||||
// ---- 参数 ----
|
||||
std::string joint_state_topic_ = "joint_states";
|
||||
std::string imu_topic_ = "imu/data";
|
||||
std::string command_topic_ = "joint_commands";
|
||||
std::string diagnostics_topic_ = "joint_diagnostics";
|
||||
std::string imu_frame_id_ = "base_link";
|
||||
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;
|
||||
|
||||
// ---- 状态缓存（原始电机量）----
|
||||
std::vector<double> joint_position_, joint_velocity_, joint_effort_;
|
||||
std::vector<double> temperature_, voltage_;
|
||||
std::vector<uint8_t> driver_status_, last_status_;
|
||||
std::vector<uint8_t> has_feedback_; ///< 该关节是否收到过反馈（诊断只报有数据的）
|
||||
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};
|
||||
|
||||
@@ -10,6 +10,7 @@
|
||||
#include <toml++/toml.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -22,6 +23,7 @@ struct JointSpec {
|
||||
int leg = -1;
|
||||
double lower = 0., upper = 0.; ///< 位置限位 [rad]（下发前自检用）
|
||||
double vel_limit = 0.; ///< 速度限位 [rad/s]
|
||||
double torque_limit = 0.; ///< 前馈力矩限幅 [N·m]（0 = 不额外限制）
|
||||
double dir = 1.; ///< 硬件方向 ±1
|
||||
double offset_deg = 0.; ///< 硬件零点偏置 [deg]
|
||||
};
|
||||
@@ -68,9 +70,17 @@ public:
|
||||
// ---- 本节点话题与频率 ----
|
||||
std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
|
||||
std::string topic_joint_commands = "joint_commands";
|
||||
std::string topic_joint_diagnostics = "joint_diagnostics";
|
||||
double publish_rate_hz = 200.0;
|
||||
double diagnostics_rate_hz = 10.0; ///< 温度/状态/电压发布频率（低频诊断，不进控制路径）
|
||||
std::string imu_frame_id = "base_link";
|
||||
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);
|
||||
|
||||
@@ -140,13 +150,15 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
|
||||
if (joint_names.empty()) return fail("[robot].joint_names 为空");
|
||||
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 长度必须等于关节数量");
|
||||
}
|
||||
joints.assign(size_t(dof), JointSpec{});
|
||||
for (int i = 0; i < dof; ++i) {
|
||||
@@ -154,6 +166,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();
|
||||
@@ -215,18 +228,46 @@ 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 + " 的位置限幅超出归一化边界 [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 + " 的速度限幅超出归一化边界 [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 + " 的力矩限幅超出归一化边界 [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::Get(hw, "diagnostics_rate_hz", diagnostics_rate_hz);
|
||||
detail::GetStr(hw, "imu_frame_id", imu_frame_id);
|
||||
detail::Get(hw, "command_queue", command_queue);
|
||||
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 必须 > 0");
|
||||
if (diagnostics_rate_hz <= 0.) return fail("[hardware].diagnostics_rate_hz 必须 > 0");
|
||||
if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s 不能为负");
|
||||
|
||||
if (error) error->clear();
|
||||
return true;
|
||||
|
||||
@@ -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 "无";
|
||||
case 0x01: return "过压";
|
||||
case 0x02: return "过流";
|
||||
case 0x04: return "过温";
|
||||
case 0x08: return "过速";
|
||||
case 0x10: return "编码器";
|
||||
case 0x20: return "欠压/预驱";
|
||||
default: return "组合故障";
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @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
|
||||
@@ -1,6 +1,9 @@
|
||||
<launch>
|
||||
<!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数来自本节点自己的 config/invicta_hardware.toml -->
|
||||
<!-- 关节/IMU 数据源 + 关节指令执行端（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>
|
||||
|
||||
@@ -1,43 +1,136 @@
|
||||
/**
|
||||
* @file invicta_hardware_node.cpp
|
||||
* @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口）
|
||||
* @brief 关节/IMU 数据源 + 关节指令执行端（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/%s，订阅 %s @ %.0f Hz；诊断 %s @ %.0f Hz", joint_state_topic_.c_str(),
|
||||
imu_topic_.c_str(), imu_frame_id_.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] 配置里没有 CAN 通道（[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] 打开 %s 失败：%s", name.c_str(), err.c_str());
|
||||
ROS_ERROR("[invicta_hardware] 请先配置总线：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，跳过 %s，只发布状态、不下发电机指令", 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] %s 上的 CANID 非法（最大 %d，协议要求 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 已打开（CANID 1~%d，下发 %d 字节）", 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;
|
||||
diagnostics_topic_ = cfg.topic_joint_diagnostics;
|
||||
imu_frame_id_ = cfg.imu_frame_id;
|
||||
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 一致（同一份配置，天然一致）
|
||||
|
||||
@@ -45,42 +138,279 @@ void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
|
||||
ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0，回退到 200 Hz");
|
||||
publish_rate_hz_ = 200.0;
|
||||
}
|
||||
if (diagnostics_rate_hz_ <= 0.) {
|
||||
ROS_ERROR("[invicta_hardware] diagnostics_rate_hz 必须 > 0，回退到 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();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 控制字 → CAN
|
||||
void HardwareBridge::HandleControlWord(uint16_t word) {
|
||||
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：已发失能（0xFD），线圈断开");
|
||||
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：已发使能（0xFC × %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：本协议无清错指令，按重新使能处理");
|
||||
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 但未使能：已补发使能序列（上游启动时序可能丢失）");
|
||||
}
|
||||
ROS_INFO("[invicta_hardware] control_word -> kMotorControl：进入 kp/kd + 前馈控制");
|
||||
break;
|
||||
case invicta_msgs::kGetStatus:
|
||||
ROS_INFO("[invicta_hardware] control_word -> kGetStatus(5)：上报驱动状态");
|
||||
// 每条指令都会回 12 字节状态帧，无需专门请求
|
||||
ROS_INFO("[invicta_hardware] control_word -> kGetStatus：状态随每条指令自动返回，无需请求");
|
||||
break;
|
||||
default:
|
||||
ROS_WARN("[invicta_hardware] 未知 control_word=%u，忽略", word);
|
||||
break;
|
||||
}
|
||||
// TODO(硬件)：把上面这些语义落到电机使能序列上（上电顺序、清错指令、超时保护）。
|
||||
}
|
||||
|
||||
void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s) {
|
||||
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) {
|
||||
if (bus.canid_to_joint[size_t(canid)] < 0) continue;
|
||||
PackCodeBlock(payload + 2 + kBlockBytes * (canid - 1), 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 发送控制码 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 bool is_wheel = Config().IsWheel(joint);
|
||||
const MotorRange& range = is_wheel ? wheel_range_ : joint_range_;
|
||||
const double kd = is_wheel ? timeout_wheel_kd_ : timeout_joint_kd_;
|
||||
// kp=0、速度与力矩都给 0：只留阻尼抵抗外力，不保持位置
|
||||
PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), 0.0, 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 发送阻尼帧失败: %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 收到未知帧 id=0x%X len=%u，忽略", 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 反馈只有 %u 字节（协议要求 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;
|
||||
}
|
||||
|
||||
joint_position_[size_t(joint)] = fb.position;
|
||||
joint_velocity_[size_t(joint)] = fb.velocity;
|
||||
joint_effort_[size_t(joint)] = fb.torque;
|
||||
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 故障：%s（status=0x%02X，温度 %.0f ℃）",
|
||||
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，不下发任何指令", command_topic_.c_str());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
cmd = latest_cmd_;
|
||||
}
|
||||
|
||||
// 控制字变化在这里处理（循环线程），不阻塞订阅回调
|
||||
if (cmd.control_word != applied_control_word_) {
|
||||
HandleControlWord(cmd.control_word);
|
||||
applied_control_word_ = cmd.control_word;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
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] 指令含非有限值，丢弃该帧");
|
||||
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 未收到 %s，转阻尼（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] 指令恢复，退出阻尼");
|
||||
}
|
||||
}
|
||||
|
||||
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 MotorRange& range = Config().IsWheel(joint) ? wheel_range_ : joint_range_;
|
||||
const std::string& name = joint_names_[size_t(joint)];
|
||||
const size_t j = size_t(joint);
|
||||
|
||||
// 先按人为限幅夹紧，再交给编码；[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 前馈力矩 %.1f N·m 超过限幅 %.1f，已夹紧", name.c_str(),
|
||||
tau, tlim);
|
||||
tau = std::min(std::max(tau, -tlim), tlim);
|
||||
}
|
||||
|
||||
// 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 超出模组范围 [0, %.1f]，已夹紧", 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 超出模组范围 [0, %.1f]，已夹紧", name.c_str(),
|
||||
kd, range.kd_max);
|
||||
kd = std::min(std::max(kd, 0.0), range.kd_max);
|
||||
}
|
||||
|
||||
PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), pos, vel, kp, kd, tau, 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 发送运行帧失败: %s", bus.name.c_str(), err.c_str());
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 发布
|
||||
void HardwareBridge::PublishState() {
|
||||
sensor_msgs::JointState js;
|
||||
js.header.stamp = ros::Time::now();
|
||||
@@ -110,50 +440,40 @@ void HardwareBridge::PublishState() {
|
||||
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());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
cmd = latest_cmd_;
|
||||
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]);
|
||||
}
|
||||
|
||||
// 上游 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);
|
||||
}
|
||||
}
|
||||
@@ -178,13 +498,15 @@ int main(int argc, char** argv) {
|
||||
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 初始化失败，退出（无硬件调试可设 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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user