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_deploy/interface/robot/hardware/invicta_interface.hpp

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