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] 重复维护入待办
This commit is contained in:
+32
-39
@@ -1,15 +1,16 @@
|
||||
# TODO
|
||||
|
||||
本文件记录**当前会话中确认但本期不做**的事项。按优先级分组，每项尽量写清「为什么」和「验收方式」，
|
||||
避免下次重新推导。已完成的部分见各包 README。
|
||||
避免下次重新推导。已完成的部分见各包 README 与文末清单。
|
||||
|
||||
## P0：安全链路（真机跑 RL 前必须处理）
|
||||
|
||||
- [ ] **异常数据接入 deploy 的保护链路**
|
||||
硬件已在 `/joint_diagnostics` 提供温度/状态字/电压（10 Hz），但 deploy 侧仍是常量：
|
||||
`robot_interface.h` 的 `GetMotorTemperture()` 返回 30 ℃、`GetDriverStatusWord()` 返回全 1，
|
||||
所以 `SafeController` 的过温检查和驱动报错检查恒不触发。
|
||||
做法：`InvictaInterface` 订阅 `/joint_diagnostics` 并覆盖这两个虚函数。
|
||||
硬件已在 `/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`。
|
||||
@@ -18,32 +19,13 @@
|
||||
诊断话题 10 Hz，`SafeController` 读缓存值。若 hardware 进程死掉，温度会停在最后一帧的「正常」。
|
||||
需要在接口层记录诊断时间戳，超时（如 1 s）按异常处理。
|
||||
验收：杀掉 hardware 节点后 deploy 进入安全状态。
|
||||
|
||||
- [ ] **异常上报 1002/3 的数据源**
|
||||
remote 的 `ErrorDoc` 恒为空列表，因为 `MotionState.msg` 没有错误字段。
|
||||
建议给 `MotionState` 加 `uint32 error_code` + `uint16 error_component`（位域与
|
||||
`SafeController::robot_error_state_` 对齐，component 用 M20 槽号），remote 再映射成
|
||||
`0x8001/0x8002/0x8020/0x8012` 等协议错误码。
|
||||
验收：模拟驱动器故障，App 收到非空 `ErrorList` 且 component 指向正确关节。
|
||||
|
||||
- [ ] **IMU 数据源缺失**
|
||||
`/imu/data` 目前发的是单位四元数、`orientation_covariance[0] = 0`（声称有效），
|
||||
但协议里没有任何 IMU 字段，硬件桥也没有读 IMU。
|
||||
按本项目自己的约定，拿不到姿态时应把 `orientation_covariance[0]` 置 `-1`；
|
||||
改之前要确认 deploy 的 `[rl].require_imu` 会不会因此拒绝进 RL。
|
||||
（对照：IMU 的同类问题已解，走 `IsImuHealthy()` 的断流判定——本条可照抄那套「时间戳 + 超时 + 恢复去抖」。）
|
||||
|
||||
## P1：标定与配置一致性（上真机前）
|
||||
|
||||
- [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`。
|
||||
这样 `[joint_limits]`（模型帧）与被限幅的指令同帧，`/joint_states` 也是模型角。
|
||||
遗留：`[joint_limits]` 仍在两份 TOML 里重复维护（deploy 用一份做状态机自检、hardware 用一份做下发夹紧），
|
||||
后续可考虑合并或生成。
|
||||
|
||||
- [ ] **反馈帧仲裁 ID 需真机确认**
|
||||
`ReadHardware` 按「反馈使用关节自身 CANID」实现（手册只写了从站 CANID 1~6）。
|
||||
上电后 `candump can0` 核对；若不是 1~4，需要改匹配逻辑。
|
||||
- [ ] **`[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`，
|
||||
@@ -58,7 +40,8 @@
|
||||
如果当初是估的，建议按实测重新定。
|
||||
|
||||
- [ ] **位置指令越界目前静默夹紧**
|
||||
力矩和 kp/kd 越界会告警，位置不会。策略动作顶到限位时可能值得提示一次（注意别刷屏）。
|
||||
`invicta_hardware_node.cpp` 的 `PackRunBlocks` 里力矩和 kp/kd 越界会告警，位置不会。
|
||||
策略动作顶到限位时可能值得提示一次（`ROS_WARN_THROTTLE`，注意别刷屏）。
|
||||
|
||||
## P2：协议状态上报的剩余占位
|
||||
|
||||
@@ -67,6 +50,7 @@
|
||||
- [ ] `Charge / HES / Sleep`：没有充电状态、硬急停信号与休眠管理。
|
||||
- [ ] `HipY` 的正负号未与真实 App/M20 对照：如果 App 只是显示数值没问题，若渲染姿态需要核对。
|
||||
- [ ] 未实现指令：照明灯、休眠设置/查询、自主充电、速度指令（导航模式）、导航任务（Pro）。
|
||||
- [ ] 错误上报的 `component` 恒为 0（见文末遗留）：要定位到具体关节，需在 deploy 侧把错误位与关节号关联。
|
||||
|
||||
## P3：工程与清理
|
||||
|
||||
@@ -74,16 +58,25 @@
|
||||
deploy 已改为「等订阅者再发」且 hardware 在 `kMotorControl` 未使能时会补发使能，功能上已安全；
|
||||
但 hardware 循环线程处理 `kErrorReset` 的使能连发会阻塞约 80 ms，期间可能观察不到 `kEnable`。
|
||||
若要严格逐帧落地，可把连发改成分段非阻塞发送。
|
||||
- [ ] `[motor.gains].bench_*` 已被指令里的 kp/kd 取代，确认是否删除。
|
||||
- [ ] 构建镜像 `localhost/custom-ros-noetic` 缺 toml++，目前靠挂载外部安装树 + `CMAKE_PREFIX_PATH`。
|
||||
可考虑在镜像里装 toml++，或在仓库内 vendored。
|
||||
- [ ] `[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] 容器内构建 + 冒烟测试；修复启动时序丢失与订阅回调阻塞
|
||||
- [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`。
|
||||
|
||||
@@ -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`
|
||||
|
||||
|
||||
@@ -59,7 +59,9 @@ 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 # 断流后需连续这么久有新鲜可用帧才重新判健康（恢复去抖）
|
||||
|
||||
# ------------------------------------------------------------------ 每腿标定
|
||||
# 模型约定：摆臂角 θ_model = splay_sign · acos(h/L)。
|
||||
|
||||
@@ -62,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; ///< 相对本包目录、或绝对路径
|
||||
@@ -251,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);
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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_;}
|
||||
|
||||
@@ -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) {
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
@@ -72,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_; }
|
||||
|
||||
@@ -98,7 +106,8 @@ public:
|
||||
cmd_pub_.publish(msg);
|
||||
}
|
||||
|
||||
virtual void RefreshRobotData() {}
|
||||
/// 每个控制周期由状态机调一次：刷新 IMU 连续性判定（断流判坏 + 恢复去抖，见 RefreshImuHealth）
|
||||
virtual void RefreshRobotData() { RefreshImuHealth(); }
|
||||
|
||||
protected:
|
||||
// ------------------------------------------------------------------ 启动序列
|
||||
@@ -178,8 +187,12 @@ 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 did not provide orientation (covariance=-1), attitude observation invalid",
|
||||
@@ -190,6 +203,7 @@ 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) {
|
||||
has_imu_valid_ = false;
|
||||
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s quaternion norm near zero, ignoring this frame", cfg_.topic_imu.c_str());
|
||||
return;
|
||||
}
|
||||
@@ -198,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:
|
||||
@@ -206,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;
|
||||
|
||||
@@ -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 关节电机温度 [℃]
|
||||
*
|
||||
|
||||
@@ -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;
|
||||
@@ -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;
|
||||
|
||||
@@ -169,6 +169,11 @@ public:
|
||||
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;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# invicta_hardware
|
||||
|
||||
关节/IMU 数据源与关节指令执行端。已接 **SocketCAN + 智身关节模组 CANFD 协议**（协议见
|
||||
关节数据源与关节指令执行端。已接 **SocketCAN + 智身关节模组 CANFD 协议**（协议见
|
||||
[docs/zhishen_joint_module_canfd.md](../../docs/zhishen_joint_module_canfd.md)）。
|
||||
|
||||
## 它负责什么
|
||||
@@ -8,7 +8,7 @@
|
||||
以 `publish_rate_hz`（默认 200 Hz）为一个周期，循环做三件事：
|
||||
|
||||
1. `ReadHardware()` — 非阻塞收各路总线上的 12 字节反馈帧，更新关节位置/速度/力矩、温度、状态码、电压
|
||||
2. `PublishState()` — 发 `sensor_msgs/JointState` 与 `sensor_msgs/Imu`
|
||||
2. `PublishState()` — 发 `sensor_msgs/JointState`
|
||||
3. `WriteHardware()` — 使能后每周期发 MIT 运行帧（p / v / kp / kd / tau）
|
||||
|
||||
上游指令断流时（`command_timeout_s`，默认 100 ms）自动转**阻尼帧**（kp=0、速度/力矩 0、只留 `timeout_*_kd`），
|
||||
@@ -21,7 +21,6 @@
|
||||
| 话题 | 方向 | 消息 |
|
||||
|---|---|---|
|
||||
| `/joint_states` | 发布 | `sensor_msgs/JointState` |
|
||||
| `/imu/data` | 发布 | `sensor_msgs/Imu` |
|
||||
| `/joint_diagnostics` | 发布 | `invicta_msgs/JointDiagnostics`（默认 10 Hz） |
|
||||
| `/joint_commands` | 订阅 | `invicta_msgs/JointCommandArray` |
|
||||
|
||||
@@ -61,7 +60,7 @@
|
||||
- **本节点是 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：姿态四元数（**弧度**）、角速度 `[rad/s]`（机体系）、线加速度 `[m/s²]`（机体系）。⚠️ 目前**还没有 IMU 数据源**，本节点发的是单位四元数，等硬件接上再改。
|
||||
- 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`。
|
||||
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
# =============================================================================
|
||||
# invicta_hardware（关节/IMU 数据源 + 指令执行端）自己的配置
|
||||
# invicta_hardware（关节数据源 + 指令执行端）自己的配置
|
||||
# 只放本节点需要的参数：关节表与硬件标定、话题、发布频率、电机与 CAN
|
||||
# 运动学/策略/步态等属于 invicta_deploy，见 invicta_deploy/config/invicta_deploy.toml
|
||||
# =============================================================================
|
||||
@@ -114,14 +114,12 @@ 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）
|
||||
publish_rate_hz = 200.0 # 关节状态发布频率（RL 侧控制环 200 Hz）
|
||||
diagnostics_rate_hz = 10.0 # 诊断量频率：CAN 反馈每条指令都有，但话题按低频发
|
||||
imu_frame_id = "base_link"
|
||||
command_queue = 1 # 指令只取最新一帧
|
||||
# true：CAN 打不开就退出（真机默认，避免以为在控车其实没连上）
|
||||
# false：没有 CAN 也启动，只发零位状态与指令缓存（调试 / 无硬件冒烟测试）
|
||||
|
||||
@@ -1,10 +1,10 @@
|
||||
/**
|
||||
* @file hardware_bridge.h
|
||||
* @brief 关节/IMU 数据源与关节指令执行端（SocketCAN + 智身模组 CANFD 协议）
|
||||
* @brief 关节数据源与关节指令执行端（SocketCAN + 智身模组 CANFD 协议）
|
||||
*
|
||||
* 循环（publish_rate_hz，默认 200 Hz）：
|
||||
* 1. ReadHardware() —— 非阻塞收各路总线上的 12 字节反馈帧，更新关节状态/温度/状态码
|
||||
* 2. PublishState() —— 发 /joint_states 与 /imu/data
|
||||
* 2. PublishState() —— 发 /joint_states（IMU 不在本节点：由独立节点发布）
|
||||
* 3. WriteHardware() —— 按控制字发使能/失能，使能后每周期发 MIT 运行帧
|
||||
* 另按 diagnostics_rate_hz（默认 10 Hz）发 /joint_diagnostics。
|
||||
*
|
||||
@@ -26,7 +26,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
|
||||
#include <atomic>
|
||||
@@ -95,10 +94,8 @@ private:
|
||||
|
||||
// ---- 参数 ----
|
||||
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;
|
||||
@@ -119,7 +116,6 @@ private:
|
||||
// ---- ROS ----
|
||||
ros::NodeHandle nh_;
|
||||
ros::Publisher joint_state_pub_;
|
||||
ros::Publisher imu_pub_;
|
||||
ros::Publisher diagnostics_pub_;
|
||||
ros::Subscriber command_sub_;
|
||||
|
||||
@@ -140,9 +136,6 @@ private:
|
||||
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};
|
||||
|
||||
std::atomic<bool> running_{false};
|
||||
bool warned_no_command_ = false;
|
||||
|
||||
@@ -75,12 +75,11 @@ 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;
|
||||
double diagnostics_rate_hz = 10.0; ///< 温度/状态/电压发布频率（低频诊断，不进控制路径）
|
||||
std::string imu_frame_id = "base_link";
|
||||
int command_queue = 1;
|
||||
bool require_can = true; ///< false = 无 CAN 也启动（调试/冒烟测试）
|
||||
|
||||
@@ -259,14 +258,12 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
|
||||
|
||||
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) {
|
||||
@@ -283,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 joints, %zu legs, state publish %.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(),
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
<launch>
|
||||
<!-- 关节/IMU 数据源 + 关节指令执行端（SocketCAN）。参数来自本节点自己的 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">
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/**
|
||||
* @file invicta_hardware_node.cpp
|
||||
* @brief 关节/IMU 数据源 + 关节指令执行端（SocketCAN，智身模组 CANFD 协议）
|
||||
* @brief 关节数据源 + 关节指令执行端（SocketCAN，智身模组 CANFD 协议）
|
||||
*/
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
@@ -53,15 +53,13 @@ HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
|
||||
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);
|
||||
|
||||
ready_ = OpenBuses();
|
||||
|
||||
ROS_INFO("[invicta_hardware] %s -> %s/%s, subscribing %s @ %.0f Hz; diagnostics %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_);
|
||||
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() {
|
||||
@@ -117,10 +115,8 @@ void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
|
||||
// 参数来自本节点自己的 config/invicta_hardware.toml
|
||||
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;
|
||||
@@ -499,25 +495,6 @@ 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);
|
||||
}
|
||||
|
||||
void HardwareBridge::PublishDiagnostics() {
|
||||
|
||||
@@ -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
|
||||
@@ -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 # 机型标识
|
||||
|
||||
@@ -39,6 +39,7 @@
|
||||
#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"
|
||||
@@ -210,6 +211,7 @@ 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;
|
||||
}
|
||||
@@ -415,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();
|
||||
@@ -708,6 +724,8 @@ 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_;
|
||||
|
||||
Reference in New Issue
Block a user