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] 重复维护入待办

src/invicta_hardware/README.md

@@ -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`。
 

src/invicta_hardware/config/invicta_hardware.toml

@@ -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 也启动，只发零位状态与指令缓存（调试 / 无硬件冒烟测试）

src/invicta_hardware/include/invicta_hardware/hardware_bridge.h

@@ -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;

src/invicta_hardware/include/invicta_hardware/hardware_config.h

@@ -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(),

src/invicta_hardware/launch/invicta_hardware.launch

@@ -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">

src/invicta_hardware/src/invicta_hardware_node.cpp

@@ -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() {