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

docs/TODO.md

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

src/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`
 

src/invicta_deploy/config/invicta_deploy.toml

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

src/invicta_deploy/include/config/deploy_config.h

@@ -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;  ///< 相对本包目录、或绝对路径
@@ -163,17 +165,17 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
     try {
         tbl = toml::parse_file(path);
     } catch (const toml::parse_error& e) {
-        return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
+        return fail(std::string("TOML parse failed: ") + std::string(e.description()) + " @ " + path);
     }
 
     const toml::table* robot = detail::Section(tbl, "robot");
-    if (!robot) return fail("缺少 [robot] 段");
+    if (!robot) return fail("missing [robot] section");
     detail::GetStr(robot, "name", robot_name);
     detail::GetStr(robot, "version", robot_version);
     detail::GetStrArr(robot, "joint_names", joint_names);
-    if (joint_names.empty()) return fail("[robot].joint_names 为空");
+    if (joint_names.empty()) return fail("[robot].joint_names is empty");
     dof = int(joint_names.size());
-    if (dof % 2 != 0) return fail("joint_names 数量必须是偶数（每腿 摆臂+轮）");
+    if (dof % 2 != 0) return fail("joint_names count must be even (one swing + one wheel per leg)");
 
     std::vector<double> lower, upper, vel_limit;
     const toml::table* limits = detail::Section(tbl, "joint_limits");
@@ -181,7 +183,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
     detail::GetDoubleArr(limits, "upper", upper);
     detail::GetDoubleArr(limits, "vel_limit", vel_limit);
     if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
-        return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
+        return fail("[joint_limits] lower/upper/vel_limit length must equal joint count");
     }
     joints.assign(size_t(dof), JointSpec{});
     for (int i = 0; i < dof; ++i) {
@@ -204,7 +206,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
             leg.swing_index = IndexOf(leg.swing_joint);
             leg.wheel_index = IndexOf(leg.wheel_joint);
             if (leg.swing_index < 0 || leg.wheel_index < 0) {
-                return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
+                return fail("[[legs]] " + leg.name + " swing_joint/wheel_joint not in joint_names");
             }
             joints[size_t(leg.swing_index)].leg = int(legs.size());
             joints[size_t(leg.wheel_index)].leg = int(legs.size());
@@ -212,8 +214,8 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
             legs.push_back(leg);
         }
     }
-    if (legs.empty()) return fail("缺少 [[legs]] 段");
-    if (int(legs.size()) * 2 != dof) return fail("腿数与关节数不匹配（每腿 2 个关节）");
+    if (legs.empty()) return fail("missing [[legs]] section");
+    if (int(legs.size()) * 2 != dof) return fail("leg count does not match joint count (2 joints per leg)");
 
     const toml::table* gc = detail::Section(tbl, "leg");
     detail::Get(gc, "swing_link_len", swing_link_len);
@@ -235,12 +237,12 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
     detail::Get(gains, "damping_kd_wheel", damping_kd_wheel);
     detail::GetBool(gains, "wheel_kp_zero", wheel_kp_zero);
 
-    if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len 必须 > 0");
+    if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len must be > 0");
     for (const auto& pair : {std::make_pair("stand_height", stand_height),
                              std::make_pair("pre_height", pre_height),
                              std::make_pair("lie_height", lie_height)}) {
         if (pair.second > swing_link_len) {
-            return fail(std::string("[leg].") + pair.first + " 大于 swing_link_len，物理上不可达");
+            return fail(std::string("[leg].") + pair.first + " greater than swing_link_len, physically unreachable");
         }
     }
 
@@ -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);
@@ -264,14 +268,14 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
         detail::GetDoubleArr(pol, "kp", policy_kp);
         detail::GetDoubleArr(pol, "kd", policy_kd);
         if (policy_joint_order.empty()) policy_joint_order = joint_names;
-        if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order 长度必须等于关节数量");
+        if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order length must equal joint count");
         for (const auto& n : policy_joint_order) {
-            if (IndexOf(n) < 0) return fail("[policy].joint_order 里的 " + n + " 不在 joint_names 里");
+            if (IndexOf(n) < 0) return fail("[policy].joint_order entry " + n + " not in joint_names");
         }
         action_dim = dof;
         for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) {
             if (int(arr->size()) != action_dim) {
-                return fail("[policy] 的 action_scale/default_joint_pos/kp/kd 长度必须等于关节数量");
+                return fail("[policy] action_scale/default_joint_pos/kp/kd length must equal joint count");
             }
         }
         obs_dim = 9 + 3 * action_dim;  // 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角 + 关节速度 + 上一步动作
@@ -285,7 +289,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
                 try {
                     gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{});
                 } catch (const std::exception&) {
-                    return fail("[gait.policies] 的键必须能转成整数步态值");
+                    return fail("[gait.policies] key must convert to an integer gait value");
                 }
             }
         }
@@ -320,7 +324,7 @@ inline std::string DeployConfig::Summary() const {
     for (size_t i = 0; i < legs.size(); ++i) {
         const double sw = SwingAngleForHeight(stand_height, int(i));
         const double li = SwingAngleForHeight(lie_height, int(i));
-        snprintf(buf, sizeof(buf), "[config]   %s splay=%+.0f 站立摆臂=%+.1f° 趴下=%+.1f°\n",
+        snprintf(buf, sizeof(buf), "[config]   %s splay=%+.0f stand swing=%+.1f deg lie down=%+.1f deg\n",
                  legs[i].name.c_str(), legs[i].splay_sign, sw * 180. / M_PI, li * 180. / M_PI);
         s += buf;
     }

src/invicta_deploy/include/types/common_types.h

@@ -96,6 +96,9 @@ struct UserCommand {
     float reserved_scale;
     uint32_t gait = 0;        // 请求/生效的步态（协议 GaitParam），0 = 未请求
     uint8_t usage_mode = 0;   // 协议使用模式：0 常规 / 1 导航 / 2 辅助
+    // 控制器 → 上报通道：SafeController 每周期写入内部错误位掩码，
+    // MotionStateReporter 读它填 MotionState.error_code（位定义见 invicta_msgs/error_code.h）
+    uint32_t robot_error_code = 0;
 };
 
 enum FromType {

src/invicta_deploy/include/utils/motion_state_feedback.hpp

@@ -23,14 +23,6 @@ private:
     Vec3f cmd_vel_;
     Vec3f max_vel_;
 
-    union{
-        struct{
-            uint32_t joint_pos_limit : 1;
-            uint32_t posture_limit : 1;
-            uint32_t reserved : 30;
-        }error_code_bit_;
-        uint32_t error_code_;
-    };
 public:
     MotionStateFeedback(/* args */){
         memset(this, 0, sizeof(MotionStateFeedback));
@@ -54,13 +46,9 @@ public:
         }
     }
 
-    void ClearMotionError(){error_code_ = 0;}
-    void SetJointPosLimitError(){
-        error_code_bit_.joint_pos_limit = 1;
-    }
-    void SetPostureLimitError(){
-        error_code_bit_.posture_limit = 1;
-    }
+    // 错误码不再从这里走出去：内部错误位掩码的定义与展开见 invicta_msgs/error_code.h，
+    // 由 SafeController 每周期写入 UserCommand::robot_error_code，
+    // MotionStateReporter 读它填 MotionState.error_code（remote 再映射成协议 ErrorList）。
 
     inline uint8_t GetCurrentState(){return current_state_;}
     inline uint32_t GetCurrentGait(){return current_gait_;}

src/invicta_deploy/include/utils/safe_controller.hpp

@@ -36,9 +36,6 @@ private:
     std::thread judge_thread_;
     int run_cnt_ = 0;
 
-    double last_imu_ts_, imu_check_time_;
-    bool imu_check_flag_ = true;
-
     double ri_check_time_, last_ri_ts_;
     VecXf last_joint_pos_, last_joint_vel_, last_joint_tau_;
     std::vector<int> joint_data_same_cnt_;
@@ -60,27 +57,24 @@ private:
     std::map<uint8_t, uint16_t> error_driver_map_;
     uint32_t joint_update_state_;
 
+    /**
+     * IMU 检查。两件事分开：
+     *   ① 解算结果含 NaN → 返回 false，调用方置 safe_control_mode（急停语义）
+     *   ② 断流/姿态不可信 → 只置 imu_update_overtime 错误位，**不改 safe_control_mode**：
+     *      RL 退出要走「正常切出→趴下」（RLControlState::GetNextStateName），
+     *      而趴下状态自己会在 safe_control_mode != 0 时转阻尼（liedown_state），
+     *      所以这里一旦置 mode 就会把受控下山打断成就地垮下来。
+     * 连续性判定（阈值 rl_imu_timeout_s / 恢复 rl_imu_recover_s）由 RobotInterface 提供。
+     */
     bool IsImuDataNormal() {
-        double current_imu_ts_ = ri_ptr_->GetImuTimestamp();
         Vec3f rpy = ri_ptr_->GetImuRpy();
         Vec3f acc = ri_ptr_->GetImuAcc();
         Vec3f omg = ri_ptr_->GetImuOmega();
-        if (imu_check_flag_) {
-            last_imu_ts_ = current_imu_ts_;
-            imu_check_time_ = current_time_;
-            imu_check_flag_ = false;
-        }
+
+        robot_error_state_.imu_update_overtime = ri_ptr_->IsImuHealthy() ? 0 : 1;
 
         bool res = true;
-        robot_error_state_.imu_update_overtime = 0;
-        if (current_imu_ts_ != last_imu_ts_) {
-            last_imu_ts_ = current_imu_ts_;
-            imu_check_time_ = current_time_;
-        }
-        if (current_time_ - imu_check_time_ == 0.2) {
-            std::cout << "========== imu lag warning ========>> " << current_time_ - imu_check_time_ << std::endl;
-        }
-
+        robot_error_state_.imu_num_error = 0;
         for (int i = 0; i < 3; ++i) {
             if (std::isnan(rpy(i)) || std::isnan(acc(i)) || std::isnan(omg(i))) {
                 res = false;
@@ -199,7 +193,8 @@ private:
         }
 
         if (robot_error_state_.imu_update_overtime) {
-            std::cout << "IMU update overtime: no IMU data received since timestamp " << last_imu_ts_ << std::endl;
+            std::cout << "IMU update overtime: no usable IMU frame, age " << ri_ptr_->GetImuAgeSec() << " s"
+                      << std::endl;
         }
 
         if (robot_error_state_.command_heartbeat_overtime) {
@@ -216,7 +211,7 @@ private:
         }
 
         if (robot_error_state_.motor_heat_warn) {
-            std::cout << "Motor overheat warning: motor " << max_temp_idx_ << " reached " << max_temp_ << "°C"
+            std::cout << "Motor overheat warning: motor " << max_temp_idx_ << " reached " << max_temp_ << " C"
                       << std::endl;
         }
 
@@ -317,6 +312,10 @@ public:
                 std::cout << "IMU error!" << std::endl;
                 usr_cmd_->safe_control_mode = 2;
             }
+
+            // 把内部错误位掩码交给上报通道（MotionStateReporter 读它填 MotionState.error_code）
+            usr_cmd_->robot_error_code = robot_error_state_.error_code;
+
             if (last_error_code_ != robot_error_state_.error_code) {
                 if (robot_error_state_.error_code != 0) {
                     std::cout << "========== error_code ========>> " << std::hex << robot_error_state_.error_code

src/invicta_deploy/interface/motion_state_reporter.hpp

@@ -74,6 +74,8 @@ private:
             msg.charge = 0;  // 待硬件：充电状态
             msg.hes = 0;     // 待硬件：硬急停信号
             msg.sleep = 0;   // 待实现：休眠管理
+            // 内部错误位掩码（SafeController 每周期写入）；位定义见 invicta_msgs/error_code.h
+            msg.error_code = user_cmd_ ? user_cmd_->robot_error_code : 0u;
             msg.version = version_;
             pub_.publish(msg);
             std::this_thread::sleep_until(next);

src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp

@@ -16,6 +16,7 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/JointState.h>
 
+#include <limits>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -34,7 +35,7 @@ public:
     InvictaInterface(const std::string& robot_name, int dof_num)
         : RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
         if (dof_num_ != cfg_.dof) {
-            ROS_WARN("[InvictaInterface] 传入 dof=%d 与配置的 %d 不一致，按配置执行", dof_num_, cfg_.dof);
+            ROS_WARN("[InvictaInterface] passed dof=%d does not match configured %d, using config value", dof_num_, cfg_.dof);
         }
         joint_pos_ = VecXf::Zero(dof_num_);
         joint_vel_ = VecXf::Zero(dof_num_);
@@ -58,7 +59,7 @@ public:
         SendStartupSequence();
         std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> "
                   << cfg_.topic_joint_commands << std::endl;
-        std::cout << "[InvictaInterface] 模型帧透传：dir/offset 由 invicta_hardware 施加" << std::endl;
+        std::cout << "[InvictaInterface] model frame passthrough: dir/offset applied by invicta_hardware" << std::endl;
     }
 
     virtual void Stop() {}
@@ -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:
     // ------------------------------------------------------------------ 启动序列
@@ -124,21 +133,21 @@ protected:
             ros::Duration(0.01).sleep();
         }
         if (cmd_pub_.getNumSubscribers() == 0) {
-            std::cout << "[InvictaInterface] 警告：joint_commands 还没有订阅者，启动时序可能丢失" << std::endl;
+            std::cout << "[InvictaInterface] warning: joint_commands has no subscribers yet, startup sequence may be lost" << std::endl;
         }
         const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
         for (uint16_t word : seq) {
             PublishRawCommand(word);
             ros::Duration(0.01).sleep();
         }
-        std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)" << std::endl;
+        std::cout << "[InvictaInterface] startup control sequence complete (disable -> error_reset -> enable)" << std::endl;
     }
 
     // ------------------------------------------------------------------ 回调
     void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) {
         const size_t n = size_t(dof_num_);
         if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) {
-            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 长度应为 %d，收到 pos=%zu vel=%zu eff=%zu，丢帧",
+            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s length should be %d, got pos=%zu vel=%zu eff=%zu, dropping frame",
                               cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
                               msg->effort.size());
             return;
@@ -159,11 +168,11 @@ protected:
         }
         if (!by_name && !warned_name_mismatch_) {
             warned_name_mismatch_ = true;
-            ROS_WARN("[InvictaInterface] %s 未按约定给出 %d 个 name，按数组下标解析",
+            ROS_WARN("[InvictaInterface] %s did not provide the agreed %d names, resolving by array index",
                      cfg_.topic_joint_states.c_str(), dof_num_);
         }
         if (unknown > 0) {
-            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 有 %d 个关节名不认识，已忽略",
+            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s has %d unknown joint names, ignored",
                               cfg_.topic_joint_states.c_str(), unknown);
         }
 
@@ -178,11 +187,15 @@ protected:
     }
 
     void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
+        imu_arrival_ = ros::Time::now();
+        has_imu_ = true;
         // orientation_covariance[0] == -1 表示「不提供姿态」
         if (msg->orientation_covariance[0] == -1.0) {
+            // 帧在来但没有可用姿态：对 RL 而言等同断流（base_rpy 会停在旧值，不能当有效观测）
+            has_imu_valid_ = false;
             if (!warned_imu_no_orientation_) {
                 warned_imu_no_orientation_ = true;
-                ROS_WARN("[InvictaInterface] %s 没有提供 orientation（covariance=-1），姿态观测无效",
+                ROS_WARN("[InvictaInterface] %s did not provide orientation (covariance=-1), attitude observation invalid",
                          cfg_.topic_imu.c_str());
             }
             return;
@@ -190,7 +203,8 @@ protected:
         Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y),
                              float(msg->orientation.z));
         if (q.norm() < 1e-6f) {
-            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 四元数模长近零，忽略该帧", cfg_.topic_imu.c_str());
+            has_imu_valid_ = false;
+            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s quaternion norm near zero, ignoring this frame", cfg_.topic_imu.c_str());
             return;
         }
         q.normalize();
@@ -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;

src/invicta_deploy/interface/robot/robot_interface.h

@@ -72,6 +72,18 @@ public:
     /// IMU 数据时间戳 [s]
     virtual double GetImuTimestamp() { return 0.; }
 
+    /// 距最近一帧**可用** IMU 的时长 [s]；从未收到过返回极大值
+    virtual double GetImuAgeSec() { return 0.; }
+
+    /**
+     * @brief IMU 是否健康：最近有新鲜、可用的姿态帧，且断流之后已连续恢复
+     *
+     * 用于两件事：RL 运行中失去 IMU 时走「正常切出→趴下」，
+     * 以及恢复之前拒绝重新进入 RL（见 rl_control_state / standup_state）。
+     * 无 IMU 数据源的实现返回 true（本构型 RL 必须要姿态，台架可关 rl_require_imu）。
+     */
+    virtual bool IsImuHealthy() { return true; }
+
     /**
      * @brief 关节电机温度 [℃]
      *

src/invicta_deploy/interface/user_command/ros_command_interface.hpp

@@ -47,12 +47,12 @@ public:
 
     void Start() override {
         if (msfb_) msfb_->UpdateCurrentGait(uint32_t(default_gait_));
-        std::cout << "\n╔════════════════════════════════════════════════╗\n"
-                  << "║          INVICTA MOTION COMMAND LINK           ║\n"
-                  << "╚════════════════════════════════════════════════╝\n"
-                  << "  指令来源: " << topic_ << "（由 invicta_remote 发布）\n"
-                  << "  默认步态: " << GaitName(uint32_t(default_gait_)) << "\n"
-                  << "  已支持步态: " << SupportedGaitList() << "\n\n";
+        std::cout << "\n+==================================================+\n"
+                  << "|           INVICTA MOTION COMMAND LINK           |\n"
+                  << "+==================================================+\n"
+                  << "  Command source: " << topic_ << " (published by invicta_remote)\n"
+                  << "  Default gait: " << GaitName(uint32_t(default_gait_)) << "\n"
+                  << "  Supported gaits: " << SupportedGaitList() << "\n\n";
     }
 
     void Stop() override {}
@@ -61,7 +61,7 @@ public:
 
 private:
     std::string SupportedGaitList() const {
-        if (!loaded_gaits_ || loaded_gaits_->empty()) return "(无)";
+        if (!loaded_gaits_ || loaded_gaits_->empty()) return "(none)";
         std::string s;
         for (int gait : *loaded_gaits_) {
             char buf[16];
@@ -83,39 +83,39 @@ private:
 
         switch (int(target)) {
             case kMotionStanding:
-                name = "站立";
+                name = "stand up";
                 next = uint8_t(RobotMotionState::StandingUp);
                 accepted = (cur == RobotMotionState::WaitingForStand || cur == RobotMotionState::LieDown);
                 break;
             case kMotionJointDamping:
-                name = "关节阻尼";
+                name = "joint damping";
                 next = uint8_t(RobotMotionState::JointDamping);
                 accepted = true;
                 break;
             case kMotionBootDamping:
-                name = "开机阻尼(按关节阻尼处理)";
+                name = "boot damping (handled as joint damping)";
                 next = uint8_t(RobotMotionState::JointDamping);
                 accepted = true;
                 break;
             case kMotionLieDown:
-                name = "趴下";
+                name = "lie down";
                 next = uint8_t(RobotMotionState::LieDown);
                 accepted = (cur == RobotMotionState::StandingUp || cur == RobotMotionState::RLControlMode);
                 break;
             case kMotionRlControl:
-                name = "RL控制";
+                name = "RL control";
                 next = uint8_t(RobotMotionState::RLControlMode);
                 accepted = (cur == RobotMotionState::StandingUp);
                 break;
             case kMotionIdle:
                 // 状态机没有「直接回空闲」的路径（RL / 站立只能经阻尼或趴下退出），不作为请求
                 if (last_request_ != kMotionIdle) {
-                    std::cout << "[MODE] 空闲(0)：不作为切换请求（退出 RL 请先切关节阻尼或趴下）" << std::endl;
+                    std::cout << "[MODE] idle (0): not treated as a switch request (to exit RL, switch to joint damping or lie down first)" << std::endl;
                 }
                 last_request_ = kMotionIdle;
                 return false;
             default:
-                std::cout << "[MODE] 未知目标状态 " << int(target) << "（来自 " << src << "）" << std::endl;
+                std::cout << "[MODE] unknown target state " << int(target) << " (from " << src << ")" << std::endl;
                 return false;
         }
 
@@ -127,9 +127,9 @@ private:
 
         if (accepted) {
             usr_cmd_->target_mode = next;
-            if (last_request_ != target) std::cout << "[MODE] " << name << "（来自 " << src << "）" << std::endl;
+            if (last_request_ != target) std::cout << "[MODE] " << name << " (from " << src << ")" << std::endl;
         } else if (last_request_ != target) {
-            std::cout << "[MODE] 拒绝 " << name << "（来自 " << src << "，当前状态 " << int(cur) << "）" << std::endl;
+            std::cout << "[MODE] reject " << name << " (from " << src << ", current state " << int(cur) << ")" << std::endl;
         }
         last_request_ = target;
         return accepted;
@@ -145,14 +145,14 @@ private:
         if (gait == 0 || gait == last_gait_request_) return;
         // 只有"启动时真的加载了模型"的步态才接受；否则状态会上报未切换，而不是假成功
         if (!loaded_gaits_ || loaded_gaits_->count(int(gait)) == 0) {
-            std::cout << "[GAIT] 拒绝 " << GaitName(gait) << "（来自 " << src
-                      << "）：该步态没有对应的 RL 模型（启动时未加载，见 gait/policy_map）" << std::endl;
+            std::cout << "[GAIT] reject " << GaitName(gait) << " (from " << src
+                      << "): this gait has no corresponding RL model (not loaded at startup, see gait/policy_map)" << std::endl;
             last_gait_request_ = gait;
             return;
         }
         usr_cmd_->gait = gait;
         if (msfb_) msfb_->UpdateCurrentGait(gait);
-        std::cout << "[GAIT] " << GaitName(gait) << "（来自 " << src << "），实际生效" << std::endl;
+        std::cout << "[GAIT] " << GaitName(gait) << " (from " << src << "), now effective" << std::endl;
         last_gait_request_ = gait;
     }
 
@@ -160,7 +160,7 @@ private:
         const std::string src = "invicta_remote";
         if (!has_command_) {
             has_command_ = true;
-            std::cout << "[MODE] 收到第一条运动指令（" << topic_ << "）" << std::endl;
+            std::cout << "[MODE] received first motion command (" << topic_ << ")" << std::endl;
         }
 
         // 轴指令：协议里是比例 [-1,1]，策略直接用这三个分量
@@ -171,7 +171,7 @@ private:
 
         if (msg->usage_mode != usr_cmd_->usage_mode) {
             usr_cmd_->usage_mode = msg->usage_mode;
-            std::cout << "[MODE] 使用模式 -> " << int(msg->usage_mode) << "（0 常规 / 1 导航 / 2 辅助）" << std::endl;
+            std::cout << "[MODE] usage mode -> " << int(msg->usage_mode) << " (0 normal / 1 navigation / 2 assist)" << std::endl;
         }
 
         RequestMotionState(msg->target_state, src);

src/invicta_deploy/main.cpp

@@ -29,7 +29,7 @@ int main(int argc, char** argv) {
     std::string err;
     auto& cfg = invicta::Config();
     if (!cfg.Load(config_path, &err)) {
-        ROS_FATAL("[invicta_deploy] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
+        ROS_FATAL("[invicta_deploy] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
         return 1;
     }
     std::cout << cfg.Summary() << std::flush;

src/invicta_deploy/run_policy/invicta_policy_runner.hpp

@@ -37,7 +37,7 @@ public:
         action_dim_ = cfg_.action_dim;
         obs_dim_ = cfg_.obs_dim;
         if (action_dim_ <= 0 || obs_dim_ <= 0) {
-            throw std::runtime_error("策略维度为 0：配置未加载或 [policy] 段不完整");
+            throw std::runtime_error("policy dimension is 0: config not loaded or [policy] section incomplete");
         }
 
         robot2policy_idx_ = Permutation(cfg_.joint_names, cfg_.policy_joint_order);

src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp

@@ -88,7 +88,7 @@ public:
         current_state_name_ = kIdle;
         next_state_name_ = kIdle;
 
-        std::cout << "control period: " << cfg.control_period_ms << " ms, 默认模型: " << policy_path << std::endl;
+        std::cout << "control period: " << cfg.control_period_ms << " ms, default model: " << policy_path << std::endl;
 
         // ---- 状态回报（供 invicta_remote 判前提 + 状态上报）----
         state_reporter_ = std::make_shared<interface::MotionStateReporter>(

src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp

@@ -34,13 +34,13 @@ public:
 
     virtual void OnEnter() {
         start_time_ = std::chrono::steady_clock::now();
-        std::cout << "[DAMPING] 进入关节阻尼，持续 " << invicta::Config().damping_hold_s << " s 后转入空闲"
+        std::cout << "[DAMPING] entering joint damping, duration " << invicta::Config().damping_hold_s << " s then transition to idle"
                   << std::endl;
         msfb_.UpdateCurrentState(RobotMotionState::JointDamping);
     }
 
     virtual void OnExit() {
-        std::cout << "[DAMPING] 结束（实际 " << Elapsed() << " s），转入空闲" << std::endl;
+        std::cout << "[DAMPING] exit (actual " << Elapsed() << " s), transition to idle" << std::endl;
     }
 
     virtual void Run() { ri_ptr_->SetJointCommand(joint_cmd_); }

src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp

@@ -44,7 +44,7 @@ public:
             joint_cmd_(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel);
 
             char buf[160];
-            snprintf(buf, sizeof(buf), "[GAIT] 趴下: %s 目标高度 %.3f m → 摆臂 %+.1f°\n", cfg.legs[l].name.c_str(),
+            snprintf(buf, sizeof(buf), "[GAIT] lie down: %s target height %.3f m -> swing %+.1f deg\n", cfg.legs[l].name.c_str(),
                      cfg.lie_height, target_swing_(int(l)) * 180. / M_PI);
             std::cout << buf;
         }

src/invicta_deploy/state_machine/quadruped_wheel/rl_control_state.hpp

@@ -48,6 +48,7 @@ private:
     std::thread run_policy_thread_;
     bool start_flag_ = true;
     std::atomic<bool> policy_output_bad_{false};  ///< 推理输出异常（NaN/Inf）
+    bool warned_imu_exit_ = false;                ///< IMU 丢失退出 RL 的日志只打一次
 
     Eigen::MatrixXf acc_rot = Eigen::MatrixXf::Zero(20, 3);
     int acc_rot_count = 0;
@@ -88,8 +89,8 @@ private:
                 auto next = PolicyForGait(gait, &switched);
                 if (switched) {
                     next->OnEnter();  // 清 last_action / run_cnt，避免跨模型残留
-                    std::cout << "[GAIT] 切换模型 -> " << invicta_msgs::GaitName(gait)
-                              << (gait_runners_.count(int(gait)) ? "（已注册模型）" : "（默认模型）") << std::endl;
+                    std::cout << "[GAIT] switch model -> " << invicta_msgs::GaitName(gait)
+                              << (gait_runners_.count(int(gait)) ? " (registered model)" : " (default model)") << std::endl;
                 }
                 active_policy_ = next;
                 active_gait_ = int(gait);
@@ -101,7 +102,7 @@ private:
                 MatXf res = ra.ConvertToMat();
                 if (!res.allFinite()) {
                     policy_output_bad_ = true;
-                    std::cout << "[RL] 推理输出含 NaN/Inf，停止下发并转入关节阻尼" << std::endl;
+                    std::cout << "[RL] policy output contains NaN/Inf, stopping commands and switching to joint damping" << std::endl;
                 } else {
                     ri_ptr_->SetJointCommand(res);
                 }
@@ -123,13 +124,13 @@ public:
             try {
                 gait_paths_[std::stoi(kv.first)] = kv.second;
             } catch (const std::exception&) {
-                ROS_WARN("[RLControlState] 步态表键 '%s' 不是整数，已忽略", kv.first.c_str());
+                ROS_WARN("[RLControlState] gait map key '%s' is not an integer, ignored", kv.first.c_str());
             }
         }
 
         default_policy_ = std::make_shared<InvictaPolicyRunner>("invicta_policy", policy_path);
         active_policy_ = default_policy_;
-        std::cout << "[GAIT] 默认模型: " << policy_path << std::endl;
+        std::cout << "[GAIT] default model: " << policy_path << std::endl;
 
         // 默认步态 = 默认模型
         if (loaded_gaits_) loaded_gaits_->insert(default_gait_);
@@ -138,14 +139,14 @@ public:
         for (const auto& kv : gait_paths_) {
             if (kv.first == default_gait_) continue;  // 默认步态已在上面处理
             if (kv.second.empty()) {
-                std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first))
-                          << " 未提供模型路径，该步态不可用" << std::endl;
+                std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first))
+                          << " has no model path, gait unavailable" << std::endl;
                 continue;
             }
             if (kv.second == policy_path) {
                 gait_runners_[kv.first] = default_policy_;
                 if (loaded_gaits_) loaded_gaits_->insert(kv.first);
-                std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) << " 复用默认模型"
+                std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first)) << " reuses default model"
                           << std::endl;
                 continue;
             }
@@ -153,11 +154,11 @@ public:
                 gait_runners_[kv.first] = std::make_shared<InvictaPolicyRunner>("gait_" + std::to_string(kv.first),
                                                                                 kv.second);
                 if (loaded_gaits_) loaded_gaits_->insert(kv.first);
-                std::cout << "[GAIT] 预加载步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) << " 模型 "
+                std::cout << "[GAIT] preload gait " << invicta_msgs::GaitName(uint32_t(kv.first)) << " model "
                           << kv.second << std::endl;
             } catch (const std::exception& e) {
-                std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first))
-                          << " 模型加载失败（该步态将不可用）：" << e.what() << std::endl;
+                std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first))
+                          << " failed to load model (gait unavailable): " << e.what() << std::endl;
             }
         }
         policy_output_bad_ = false;
@@ -168,6 +169,7 @@ public:
 
     virtual void OnEnter() {
         policy_output_bad_ = false;
+        warned_imu_exit_ = false;
         active_gait_ = -1;  // 强制按当前步态重选模型
         state_run_cnt_ = -1;
         start_flag_ = true;
@@ -198,6 +200,24 @@ public:
 
     virtual StateName GetNextStateName() {
         if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) return StateName::kJointDamping;
+
+        // IMU 断流 / 姿态不可信：走「正常切出」——交给趴下状态做受控下降。
+        // 刻意不用 safe_control_mode 表达，否则 kLieDown 自己会立刻转阻尼（见 liedown_state），
+        // 变成就地垮下来，正是这里要避免的。恢复之前 RL 也进不来（见 StandUpState::ReadyForRl）。
+        if (invicta::Config().rl_require_imu && !ri_ptr_->IsImuHealthy()) {
+            if (!warned_imu_exit_) {
+                warned_imu_exit_ = true;
+                const double age = ri_ptr_->GetImuAgeSec();
+                if (age > 1e6) {
+                    std::cout << "[RL] no usable IMU frame yet: leaving RL via normal exit -> lie down" << std::endl;
+                } else {
+                    std::cout << "[RL] IMU lost (age " << age << " s): leaving RL via normal exit -> lie down"
+                              << std::endl;
+                }
+            }
+            return StateName::kLieDown;
+        }
+
         if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::LieDown))
             return StateName::kLieDown;
         return StateName::kRLControl;

src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp

@@ -58,7 +58,7 @@ public:
             joint_cmd_(cfg.legs[l].wheel_index, 0) = 0.f;  // 轮 kp 恒 0
 
             char buf[160];
-            snprintf(buf, sizeof(buf), "[GAIT] 起立: %s 收腿(%.3f m) %+.1f° → 站立(%.3f m) %+.1f°\n",
+            snprintf(buf, sizeof(buf), "[GAIT] stand up: %s tuck leg(%.3f m) %+.1f deg -> stand(%.3f m) %+.1f deg\n",
                      cfg.legs[l].name.c_str(), cfg.pre_height, tuck_swing_(int(l)) * 180. / M_PI, cfg.stand_height,
                      stand_swing_(int(l)) * 180. / M_PI);
             std::cout << buf;
@@ -134,12 +134,12 @@ public:
     bool ReadyForRl() {
         const auto& cfg = invicta::Config();
         if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) {
-            LogNotReady("安全标志非 0");
+            LogNotReady("safe flag is not 0");
             return false;
         }
         for (int i = 0; i < int(current_joint_pos_.size()); ++i) {
             if (std::isnan(current_joint_pos_(i)) || std::isnan(current_joint_vel_(i))) {
-                LogNotReady("关节数据含 NaN");
+                LogNotReady("joint data contains NaN");
                 return false;
             }
         }
@@ -147,11 +147,11 @@ public:
             for (size_t l = 0; l < cfg.legs.size(); ++l) {
                 const int si = cfg.legs[l].swing_index;
                 if (std::fabs(current_joint_pos_(si) - stand_pose_(si)) > cfg.rl_joint_tolerance) {
-                    LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 未到站立位姿");
+                    LogNotReady("joint " + cfg.joint_names[size_t(si)] + " not at stand pose");
                     return false;
                 }
                 if (std::fabs(current_joint_vel_(si)) > cfg.rl_velocity_tolerance) {
-                    LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 速度过大");
+                    LogNotReady("joint " + cfg.joint_names[size_t(si)] + " velocity too large");
                     return false;
                 }
             }
@@ -161,12 +161,17 @@ public:
             const Vec3f acc = ri_ptr_->GetImuAcc();
             for (int i = 0; i < 3; ++i) {
                 if (std::isnan(rpy(i)) || std::isnan(acc(i))) {
-                    LogNotReady("IMU 数据含 NaN");
+                    LogNotReady("IMU data contains NaN");
                     return false;
                 }
             }
             if (acc.norm() < 0.1f * gravity || acc.norm() > 3.0f * gravity) {
-                LogNotReady("IMU 加速度异常");
+                LogNotReady("IMU acceleration abnormal");
+                return false;
+            }
+            // 连续性：断流或刚恢复还没稳够时间，都不许进 RL（恢复去抖见 InvictaInterface::RefreshImuHealth）
+            if (!ri_ptr_->IsImuHealthy()) {
+                LogNotReady("IMU unhealthy: no fresh usable frame, or still recovering");
                 return false;
             }
         }
@@ -176,7 +181,7 @@ public:
     void LogNotReady(const std::string& why) {
         if (run_time_ - last_ready_log_ < 1.0) return;
         last_ready_log_ = run_time_;
-        std::cout << "[RL] 暂不进入 RL：" << why << std::endl;
+        std::cout << "[RL] not entering RL yet: " << why << std::endl;
     }
 
     virtual bool LoseControlJudge() {
@@ -200,7 +205,7 @@ public:
             if (elapsed >= ready_time + cfg.rl_enter_delay && ReadyForRl()) {
                 if (!rl_enter_logged_) {
                     rl_enter_logged_ = true;
-                    std::cout << "[RL] 站立完成且自检通过，进入 RL 控制" << std::endl;
+                    std::cout << "[RL] stand up complete and self-check passed, entering RL control" << std::endl;
                 }
                 return StateName::kRLControl;
             }

src/invicta_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 也启动（调试/冒烟测试）
 
@@ -148,13 +147,13 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
     try {
         tbl = toml::parse_file(path);
     } catch (const toml::parse_error& e) {
-        return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
+        return fail(std::string("failed to parse TOML: ") + std::string(e.description()) + " @ " + path);
     }
 
     const toml::table* robot = detail::Section(tbl, "robot");
-    if (!robot) return fail("缺少 [robot] 段");
+    if (!robot) return fail("missing [robot] section");
     detail::GetStrArr(robot, "joint_names", joint_names);
-    if (joint_names.empty()) return fail("[robot].joint_names 为空");
+    if (joint_names.empty()) return fail("[robot].joint_names is empty");
     dof = int(joint_names.size());
 
     std::vector<double> lower, upper, vel_limit, torque_limit;
@@ -165,7 +164,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
     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 长度必须等于关节数量");
+        return fail("[joint_limits] lower/upper/vel_limit/torque_limit length must equal joint count");
     }
     joints.assign(size_t(dof), JointSpec{});
     for (int i = 0; i < dof; ++i) {
@@ -188,7 +187,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
             leg.swing_index = IndexOf(leg.swing_joint);
             leg.wheel_index = IndexOf(leg.wheel_joint);
             if (leg.swing_index < 0 || leg.wheel_index < 0) {
-                return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
+                return fail("[[legs]] " + leg.name + " swing_joint/wheel_joint not in joint_names");
             }
             const toml::table* sw = detail::Section(*lt, "swing");
             const toml::table* wh = detail::Section(*lt, "wheel");
@@ -206,7 +205,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
             legs.push_back(leg);
         }
     }
-    if (legs.empty()) return fail("缺少 [[legs]] 段");
+    if (legs.empty()) return fail("missing [[legs]] section");
 
     if (const toml::table* mj = detail::Section(tbl, "motor")) {
         const auto load_motor = [](const toml::table* m, MotorSpec& out) {
@@ -247,34 +246,32 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
         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]");
+            return fail("[joint_limits] " + j.name + " position limit outside normalized bounds [p_min, p_max]");
         }
         if (j.vel_limit > std::min(std::fabs(m.v_max), std::fabs(m.v_min))) {
-            return fail("[joint_limits] " + j.name + " 的速度限幅超出归一化边界 [v_min, v_max]");
+            return fail("[joint_limits] " + j.name + " velocity limit outside normalized bounds [v_min, v_max]");
         }
         if (j.torque_limit > std::min(std::fabs(m.t_max), std::fabs(m.t_min))) {
-            return fail("[joint_limits] " + j.name + " 的力矩限幅超出归一化边界 [t_min, t_max]");
+            return fail("[joint_limits] " + j.name + " torque limit outside normalized bounds [t_min, t_max]");
         }
     }
 
     const toml::table* topics = detail::Section(tbl, "topics");
     detail::GetStr(topics, "joint_states", topic_joint_states);
-    detail::GetStr(topics, "imu", topic_imu);
     detail::GetStr(topics, "joint_commands", topic_joint_commands);
     detail::GetStr(topics, "joint_diagnostics", topic_joint_diagnostics);
 
     const toml::table* hw = detail::Section(tbl, "hardware");
     detail::Get(hw, "publish_rate_hz", publish_rate_hz);
     detail::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 (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz must be > 0");
+    if (diagnostics_rate_hz <= 0.) return fail("[hardware].diagnostics_rate_hz must be > 0");
+    if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s must not be negative");
 
     if (error) error->clear();
     return true;
@@ -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 关节，%zu 条腿，状态发布 %.0f Hz，IMU frame=%s\n", dof, legs.size(),
-             publish_rate_hz, imu_frame_id.c_str());
+    snprintf(buf, sizeof(buf), "[config] %d joints, %zu legs, state publish %.0f Hz\n", dof, legs.size(),
+             publish_rate_hz);
     s += buf;
     for (const auto& leg : legs) {
         snprintf(buf, sizeof(buf), "[config]   %s: swing %s(%s#%d) wheel %s(%s#%d)\n", leg.name.c_str(),
@@ -292,11 +289,11 @@ inline std::string HardwareConfig::Summary() const {
                  leg.wheel_bus.c_str(), leg.wheel_canid);
         s += buf;
     }
-    snprintf(buf, sizeof(buf), "[config] 电机: 关节 %s(p±%.1f v±%.1f t±%.0f) 轮 %s(p±%.1f v±%.1f t±%.0f)\n",
+    snprintf(buf, sizeof(buf), "[config] motor: joint %s(p+/-%.1f v+/-%.1f t+/-%.0f) wheel %s(p+/-%.1f v+/-%.1f t+/-%.0f)\n",
              motor_joint.model.c_str(), motor_joint.p_max, motor_joint.v_max, motor_joint.t_max,
              motor_wheel.model.c_str(), motor_wheel.p_max, motor_wheel.v_max, motor_wheel.t_max);
     s += buf;
-    snprintf(buf, sizeof(buf), "[config] CAN: %zu 路 %s.. @%.1fM/%.1fM, 使能 %d 帧 x %.3f s\n",
+    snprintf(buf, sizeof(buf), "[config] CAN: %zu channels %s.. @%.1fM/%.1fM, enable %d frames x %.3f s\n",
              can.channels.size(), can.channels.empty() ? "-" : can.channels.front().c_str(), can.bitrate / 1e6,
              can.data_bitrate / 1e6, can.enable_frames, can.enable_interval_s);
     s += buf;

src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h

@@ -84,14 +84,14 @@ struct MotorFeedback {
 /// 状态码非零故障位的中文名，用于日志
 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 "组合故障";
+        case 0x00: return "none";
+        case 0x01: return "overvoltage";
+        case 0x02: return "overcurrent";
+        case 0x04: return "overtemperature";
+        case 0x08: return "overspeed";
+        case 0x10: return "encoder";
+        case 0x20: return "undervoltage/pre-driver";
+        default: return "combined fault";
     }
 }
 

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，订阅 %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_);
+    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() {
@@ -69,7 +67,7 @@ bool HardwareBridge::OpenBuses() {
     buses_.clear();
 
     if (cfg.can.channels.empty()) {
-        ROS_ERROR("[invicta_hardware] 配置里没有 CAN 通道（[motor.can].channels）");
+        ROS_ERROR("[invicta_hardware] no CAN channels in config ([motor.can].channels)");
         return !require_can_;
     }
 
@@ -79,11 +77,11 @@ bool HardwareBridge::OpenBuses() {
 
         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",
+            ROS_ERROR("[invicta_hardware] failed to open %s: %s", name.c_str(), err.c_str());
+            ROS_ERROR("[invicta_hardware] configure the bus first: sudo ip link set %s up type can bitrate %.0f dbitrate %.0f fd on",
                       name.c_str(), cfg.can.bitrate, cfg.can.data_bitrate);
             if (require_can_) return false;
-            ROS_WARN("[invicta_hardware] require_can=false，跳过 %s，只发布状态、不下发电机指令", name.c_str());
+            ROS_WARN("[invicta_hardware] require_can=false, skipping %s, state published only, no motor commands sent", name.c_str());
             continue;
         }
 
@@ -93,7 +91,7 @@ bool HardwareBridge::OpenBuses() {
             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,
+            ROS_ERROR("[invicta_hardware] invalid CANID on %s (max %d, protocol requires 1~%d)", name.c_str(), bus.max_canid,
                       kMaxMotorsPerFrame);
             if (require_can_) return false;
             continue;
@@ -105,7 +103,7 @@ bool HardwareBridge::OpenBuses() {
         }
         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);
+        ROS_INFO("[invicta_hardware] %s opened (CANID 1~%d, sending %d bytes)", name.c_str(), bus.max_canid, bus.frame_len);
         buses_.push_back(std::move(bus));
     }
 
@@ -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;
@@ -135,11 +131,11 @@ void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
     joint_names_ = cfg.joint_names;  // 顺序必须与 invicta_deploy 一致（同一份配置，天然一致）
 
     if (publish_rate_hz_ <= 0.) {
-        ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0，回退到 200 Hz");
+        ROS_ERROR("[invicta_hardware] publish_rate_hz must be > 0, falling back to 200 Hz");
         publish_rate_hz_ = 200.0;
     }
     if (diagnostics_rate_hz_ <= 0.) {
-        ROS_ERROR("[invicta_hardware] diagnostics_rate_hz 必须 > 0，回退到 10 Hz");
+        ROS_ERROR("[invicta_hardware] diagnostics_rate_hz must be > 0, falling back to 10 Hz");
         diagnostics_rate_hz_ = 10.0;
     }
 }
@@ -182,18 +178,18 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>
         case invicta_msgs::kDisable:
             SendCodeAll(kCodeDisable, 1);
             enabled_ = false;
-            ROS_INFO("[invicta_hardware] control_word -> kDisable：已发失能（0xFD），线圈断开");
+            ROS_INFO("[invicta_hardware] control_word -> kDisable: disable sent (0xFD), coils open");
             break;
         case invicta_msgs::kEnable:
             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);
+            ROS_INFO("[invicta_hardware] control_word -> kEnable: enable sent (0xFC x %d)", Config().can.enable_frames);
             break;
         case invicta_msgs::kErrorReset:
             // 协议没有独立的清错指令（手册 §6：故障后靠重新使能或重启恢复）
             SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
             enabled_ = true;
-            ROS_INFO("[invicta_hardware] control_word -> kErrorReset：本协议无清错指令，按重新使能处理");
+            ROS_INFO("[invicta_hardware] control_word -> kErrorReset: no fault-reset command in this protocol, handled as re-enable");
             break;
         case invicta_msgs::kMotorControl:
             if (!enabled_) {
@@ -201,13 +197,13 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>
                 // 协议要求先使能再运行，这里补一次使能，而不是永远不发帧。
                 SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
                 enabled_ = true;
-                ROS_WARN("[invicta_hardware] 收到 kMotorControl 但未使能：已补发使能序列（上游启动时序可能丢失）");
+                ROS_WARN("[invicta_hardware] kMotorControl received but not enabled: enable sequence re-sent (upstream startup sequence may have been lost)");
             }
-            ROS_INFO("[invicta_hardware] control_word -> kMotorControl：进入 kp/kd + 前馈控制");
+            ROS_INFO("[invicta_hardware] control_word -> kMotorControl: entering kp/kd + feedforward control");
             break;
         case invicta_msgs::kGetStatus:
             // 每条指令都会回 12 字节状态帧，无需专门请求
-            ROS_INFO("[invicta_hardware] control_word -> kGetStatus：状态随每条指令自动返回，无需请求");
+            ROS_INFO("[invicta_hardware] control_word -> kGetStatus: status returns automatically with every command, no request needed");
             break;
         case invicta_msgs::kSetZero: {
             // 把当前机械位置写成模组零位（0xEF）。只作用于指令 name 选中的关节子集，
@@ -221,17 +217,17 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>
                 if (mask[i]) sel += (sel.empty() ? "" : ",") + joint_names_[i];
             }
             if (sel.empty()) {
-                ROS_WARN("[invicta_hardware] kSetZero：选中的关节里没有可置零的摆臂，忽略");
+                ROS_WARN("[invicta_hardware] kSetZero: no swing joints to set zero among the selected joints, ignoring");
                 break;
             }
             SendCodeAll(kCodeZero, 1, 0.0, &mask);
-            ROS_WARN("[invicta_hardware] control_word -> kSetZero：已发置零（0xEF）到 [%s]；"
-                     "此后这些摆臂该位姿的反馈角度约为 0，请把对应 offset_deg 设为 0（或据此重标）",
+            ROS_WARN("[invicta_hardware] control_word -> kSetZero: set zero sent (0xEF) to [%s]; "
+                     "from now on the feedback angle of these swing joints at this pose is about 0, set the corresponding offset_deg to 0 (or recalibrate accordingly)",
                      sel.c_str());
             break;
         }
         default:
-            ROS_WARN("[invicta_hardware] 未知 control_word=%u，忽略", word);
+            ROS_WARN("[invicta_hardware] unknown control_word=%u, ignore", word);
             break;
     }
 }
@@ -263,7 +259,7 @@ void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s, co
             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,
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send control code 0x%02X: %s", bus.name.c_str(), code,
                                   err.c_str());
             }
         }
@@ -299,7 +295,7 @@ bool HardwareBridge::SendTimeoutFrame() {
 
         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());
+            ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send damping frame: %s", bus.name.c_str(), err.c_str());
         }
     }
     return true;
@@ -318,12 +314,12 @@ bool HardwareBridge::ReadHardware() {
             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(),
+                ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s received unknown frame id=0x%X len=%u, ignore", bus.name.c_str(),
                                   unsigned(frame.id), unsigned(frame.len));
                 continue;
             }
             if (frame.len < 12) {
-                ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s CANID=%d 反馈只有 %u 字节（协议要求 12）", bus.name.c_str(),
+                ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s CANID=%d feedback only %u bytes (protocol requires 12)", bus.name.c_str(),
                                   canid, unsigned(frame.len));
                 continue;
             }
@@ -344,7 +340,7 @@ bool HardwareBridge::ReadHardware() {
             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 ℃）",
+                ROS_ERROR("[invicta_hardware] %s fault: %s (status=0x%02X, temperature %.0f C)",
                           joint_names_[size_t(joint)].c_str(), FaultName(fb.status), unsigned(fb.status), fb.temperature);
             }
             last_status_[size_t(joint)] = fb.status;
@@ -362,7 +358,7 @@ bool HardwareBridge::WriteHardware() {
         if (!has_cmd_) {
             if (!warned_no_command_) {
                 warned_no_command_ = true;
-                ROS_WARN("[invicta_hardware] 还没收到 %s，不下发任何指令", command_topic_.c_str());
+                ROS_WARN("[invicta_hardware] %s not received yet, not sending any command", command_topic_.c_str());
             }
             return false;
         }
@@ -376,7 +372,7 @@ bool HardwareBridge::WriteHardware() {
     const std::vector<uint8_t> selected = SelectionMask(cmd.name, &had_unknown);
     if (had_unknown && !warned_unknown_joint_) {
         warned_unknown_joint_ = true;
-        ROS_WARN("[invicta_hardware] %s 的 name 里有不认识的关节名，已忽略（只控制能识别的）",
+        ROS_WARN("[invicta_hardware] unrecognized joint name in %s name list, ignored (only recognized joints are controlled)",
                  command_topic_.c_str());
     }
 
@@ -387,13 +383,13 @@ bool HardwareBridge::WriteHardware() {
     }
     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);
+        ROS_WARN_THROTTLE(5.0, "[invicta_hardware] command array length does not match %zu joints, dropping frame", n);
         return false;
     }
     for (size_t i = 0; i < n; ++i) {
         if (!std::isfinite(cmd.position[i]) || !std::isfinite(cmd.velocity[i]) || !std::isfinite(cmd.effort[i]) ||
             !std::isfinite(cmd.kp[i]) || !std::isfinite(cmd.kd[i])) {
-            ROS_WARN_THROTTLE(1.0, "[invicta_hardware] 指令含非有限值，丢弃该帧");
+            ROS_WARN_THROTTLE(1.0, "[invicta_hardware] command contains non-finite value, dropping frame");
             return false;
         }
     }
@@ -410,14 +406,14 @@ bool HardwareBridge::WriteHardware() {
         if (age > command_timeout_s_) {
             if (!timeout_active_) {
                 timeout_active_ = true;
-                ROS_ERROR("[invicta_hardware] %.0f ms 未收到 %s，转阻尼（kp=0, kd=%.1f/%.1f）",
+                ROS_ERROR("[invicta_hardware] %.0f ms without %s, switching to damping (kp=0, kd=%.1f/%.1f)",
                           age * 1000.0, command_topic_.c_str(), timeout_joint_kd_, timeout_wheel_kd_);
             }
             return SendTimeoutFrame();
         }
         if (timeout_active_) {
             timeout_active_ = false;
-            ROS_WARN("[invicta_hardware] 指令恢复，退出阻尼");
+            ROS_WARN("[invicta_hardware] command recovered, exiting damping");
         }
     }
 
@@ -453,7 +449,7 @@ bool HardwareBridge::WriteHardware() {
             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(),
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s feedforward torque %.1f N*m exceeds clamp %.1f, clamped", name.c_str(),
                                   tau, tlim);
                 tau = std::min(std::max(tau, -tlim), tlim);
             }
@@ -466,13 +462,13 @@ bool HardwareBridge::WriteHardware() {
             // 越界会被编码器静默饱和，这里显式夹紧并把配置错误报出来
             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(),
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s kp=%.1f out of module range [0, %.1f], clamped", name.c_str(),
                                   kp, range.kp_max);
                 kp = std::min(std::max(kp, 0.0), range.kp_max);
             }
             double kd = cmd.kd[j];
             if (kd < 0. || kd > range.kd_max) {
-                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 的 kd=%.2f 超出模组范围 [0, %.1f]，已夹紧", name.c_str(),
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s kd=%.2f out of module range [0, %.1f], clamped", name.c_str(),
                                   kd, range.kd_max);
                 kd = std::min(std::max(kd, 0.0), range.kd_max);
             }
@@ -484,7 +480,7 @@ bool HardwareBridge::WriteHardware() {
 
         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());
+            ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send run frame: %s", bus.name.c_str(), err.c_str());
         }
     }
     return true;
@@ -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() {
@@ -572,14 +549,14 @@ int main(int argc, char** argv) {
     if (config_path.empty()) config_path = ros::package::getPath("invicta_hardware") + "/config/invicta_hardware.toml";
     std::string err;
     if (!invicta_hardware::Config().Load(config_path, &err)) {
-        ROS_FATAL("[invicta_hardware] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
+        ROS_FATAL("[invicta_hardware] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
         return 1;
     }
     std::cout << invicta_hardware::Config().Summary() << std::flush;
 
     invicta_hardware::HardwareBridge bridge(pnh);
     if (!bridge.Ready()) {
-        ROS_FATAL("[invicta_hardware] CAN 初始化失败，退出（无硬件调试可设 require_can:=false）");
+        ROS_FATAL("[invicta_hardware] CAN init failed, exiting (for hardware-free debug set require_can:=false)");
         return 1;
     }
 

src/invicta_msgs/include/invicta_msgs/error_code.h

@@ -0,0 +1,94 @@
+/**
+ * @file error_code.h
+ * @brief 机器人错误位与协议错误码的共用定义，供 invicta_deploy（产生）与 invicta_remote（上报）共用
+ *
+ * 上游对应关系：invicta_deploy 的 `RobotErrorState`（safe_controller.hpp）是一个 32 位位域
+ * union，位序**必须**与本文件的 RobotErrorBit 一致（那里是位域声明顺序，这里是显式掩码）。
+ * deploy 通过 invicta_msgs/MotionState.msg 的 `error_code` 字段把掩码发出来，
+ * remote 用 RobotErrorList() 展开成协议 1002/3 的 ErrorList（proto.md §3.4）。
+ */
+#pragma once
+
+#include <cstdint>
+#include <vector>
+
+namespace invicta_msgs {
+
+/// 机器人内部错误位。位序与 invicta_deploy `RobotErrorState` 的位域声明顺序一一对应。
+enum RobotErrorBit : uint32_t {
+    kRobotErrImuNum = 1u << 0,               ///< IMU 数据含 NaN
+    kRobotErrImuOvertime = 1u << 1,          ///< IMU 断流 / 不可信（超过 rl_imu_timeout_s 无新鲜有效帧）
+    kRobotErrCommandHeartbeatOvertime = 1u << 2,  ///< 与上位机心跳超时
+    kRobotErrDriver = 1u << 3,               ///< 关节驱动器状态字异常
+    kRobotErrMotorHeat = 1u << 4,            ///< 电机过温预警
+    kRobotErrJointNum = 1u << 5,             ///< 关节数据含 NaN / 数量不符
+    kRobotErrJointUpdate = 1u << 6,          ///< 关节数据不更新
+    kRobotErrCpuHeat = 1u << 7,              ///< CPU 过温
+    kRobotErrCpuFreq = 1u << 8,              ///< CPU 降频
+};
+
+/// 协议错误码（proto.md §3.4，仅列本工程会用到的部分）
+enum ProtocolErrorCode : int {
+    kProtoErrNone = 0,
+    kProtoErrMotorTempWarn = 0x8001,   ///< 电机温度预警
+    kProtoErrDriverCommTimeout = 0x8012, ///< 与关节驱动器通讯超时
+    kProtoErrEncoderNoValue = 0x8016,  ///< 编码器无值
+    kProtoErrBodyPosture = 0x8027,     ///< 机身姿态错误
+    kProtoErrMotionPosture = 0x8029,   ///< 运动姿态错误
+    kProtoErrCpuTempWarn = 0x8202,     ///< CPU 温度过高预警
+    kProtoErrCpuFreqProtect = 0x8211,  ///< CPU 占用率过高保护
+};
+
+/// 协议 1002/3 ErrorList 的一项
+struct RobotErrorItem {
+    int error_code;  ///< 协议 errorCode
+    int component;   ///< bit 位标识部件（关节/电池）；本工程暂无按关节定位的信息，填 0 = 整机
+    const char* bit_name;  ///< 触发的内部错误位名（日志用）
+};
+
+inline const char* RobotErrorBitName(uint32_t bit) {
+    switch (bit) {
+        case kRobotErrImuNum: return "IMU data contains NaN";
+        case kRobotErrImuOvertime: return "IMU stale or unusable";
+        case kRobotErrCommandHeartbeatOvertime: return "command heartbeat overtime";
+        case kRobotErrDriver: return "driver fault";
+        case kRobotErrMotorHeat: return "motor overheat warning";
+        case kRobotErrJointNum: return "joint data invalid";
+        case kRobotErrJointUpdate: return "joint data not updating";
+        case kRobotErrCpuHeat: return "CPU overheat warning";
+        case kRobotErrCpuFreq: return "CPU frequency warning";
+        default: return "unknown";
+    }
+}
+
+/// 内部错误位 → 协议 errorCode。协议表里没有对应项时返回 kProtoErrNone（只记日志不上报）。
+inline int ProtocolCodeForRobotError(uint32_t bit) {
+    switch (bit) {
+        // IMU 不新鲜/姿态不可信，对上位机表现为机身姿态错误；解算结果含 NaN 则算运动姿态错误
+        case kRobotErrImuOvertime: return kProtoErrBodyPosture;
+        case kRobotErrImuNum: return kProtoErrMotionPosture;
+        case kRobotErrMotorHeat: return kProtoErrMotorTempWarn;
+        case kRobotErrDriver: return kProtoErrDriverCommTimeout;
+        case kRobotErrJointNum:
+        case kRobotErrJointUpdate: return kProtoErrEncoderNoValue;
+        case kRobotErrCpuHeat: return kProtoErrCpuTempWarn;
+        case kRobotErrCpuFreq: return kProtoErrCpuFreqProtect;
+        // 心跳超时在 proto.md §3.4 的错误码表里没有对应项：只记日志，不编造错误码
+        case kRobotErrCommandHeartbeatOvertime: return kProtoErrNone;
+        default: return kProtoErrNone;
+    }
+}
+
+/// 把内部错误位掩码展开成协议 ErrorList，跳过没有对应协议码的位。
+inline std::vector<RobotErrorItem> RobotErrorList(uint32_t mask) {
+    std::vector<RobotErrorItem> items;
+    for (uint32_t bit = 1u; bit != 0u; bit <<= 1) {
+        if ((mask & bit) == 0u) continue;
+        const int code = ProtocolCodeForRobotError(bit);
+        if (code == kProtoErrNone) continue;
+        items.push_back(RobotErrorItem{code, 0, RobotErrorBitName(bit)});
+    }
+    return items;
+}
+
+}  // namespace invicta_msgs

src/invicta_msgs/include/invicta_msgs/motion_defs.h

@@ -37,23 +37,23 @@ enum UsageMode : uint8_t {
 
 inline const char* MotionParamName(uint8_t v) {
     switch (v) {
-        case kMotionIdle: return "空闲";
-        case kMotionStanding: return "站立";
-        case kMotionJointDamping: return "关节阻尼";
-        case kMotionBootDamping: return "开机阻尼";
-        case kMotionLieDown: return "趴下";
-        case kMotionRlControl: return "RL控制";
-        default: return "未知";
+        case kMotionIdle: return "Idle";
+        case kMotionStanding: return "Standing";
+        case kMotionJointDamping: return "Joint damping";
+        case kMotionBootDamping: return "Boot damping";
+        case kMotionLieDown: return "Lie down";
+        case kMotionRlControl: return "RL control";
+        default: return "Unknown";
     }
 }
 
 inline const char* GaitName(uint32_t v) {
     switch (v) {
-        case kGaitBasic: return "基础(平地)";
-        case kGaitStairs: return "楼梯";
-        case kGaitFlatAgile: return "平地(敏捷)";
-        case kGaitStairsAgile: return "楼梯(敏捷)";
-        default: return "未知";
+        case kGaitBasic: return "Basic (flat)";
+        case kGaitStairs: return "Stairs";
+        case kGaitFlatAgile: return "Flat (agile)";
+        case kGaitStairsAgile: return "Stairs (agile)";
+        default: return "Unknown";
     }
 }
 

src/invicta_msgs/msg/MotionState.msg

@@ -12,4 +12,5 @@ uint8  usage_mode     # 使用模式：0 常规 · 1 导航 · 2 辅助
 uint8  charge         # 0 空闲 · 1 前往充电桩 · 2 充电中 · 3 退出充电桩 · 4 异常 · 5 在桩未充电
 uint8  hes            # 硬急停：0 未触发 · 1 已触发
 uint8  sleep          # 0 未休眠 · 1 已休眠 · 2 进入休眠中
+uint32 error_code     # 机器人内部错误位掩码，0 = 正常；位定义见 invicta_msgs/error_code.h
 string version        # 机型标识

src/invicta_remote/include/invicta_remote/heartbeat.h

@@ -52,7 +52,7 @@ public:
         c.port = port;
         c.last_seen = std::chrono::steady_clock::now();
         clients_.push_back(c);
-        std::cout << "[GAMEPAD] 新客户端 " << ip << ":" << port << std::endl;
+        std::cout << "[GAMEPAD] new client " << ip << ":" << port << std::endl;
     }
 
     /// 记录某客户端订阅了哪一路状态上报
@@ -91,8 +91,8 @@ private:
                 const double idle =
                     std::chrono::duration_cast<std::chrono::duration<double>>(now - it->last_seen).count();
                 if (idle > timeout_sec_) {
-                    std::cout << "[GAMEPAD] 客户端 " << it->ip << ":" << it->port << " 超过 " << timeout_sec_
-                              << " s 无报文，标记离线" << std::endl;
+                    std::cout << "[GAMEPAD] client " << it->ip << ":" << it->port << " exceeded " << timeout_sec_
+                              << " s without packets, marked offline" << std::endl;
                     it = clients_.erase(it);
                     offline_logged = true;
                 } else {

src/invicta_remote/include/invicta_remote/remote_config.h

@@ -72,15 +72,15 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
     try {
         tbl = toml::parse_file(path);
     } catch (const toml::parse_error& e) {
-        return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
+        return fail(std::string("TOML parse failed: ") + std::string(e.description()) + " @ " + path);
     }
 
     const toml::table* proto = detail::Section(tbl, "protocol");
-    if (!proto) return fail("缺少 [protocol] 段");
+    if (!proto) return fail("missing [protocol] section");
     detail::Get(proto, "udp_port", udp_port);
     detail::Get(proto, "heartbeat_timeout_s", heartbeat_timeout_s);
     detail::GetBool(proto, "unsupported_as_success", unsupported_as_success);
-    if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port 非法");
+    if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port out of range");
 
     const toml::table* topics = detail::Section(tbl, "topics");
     detail::GetStr(topics, "motion_command", topic_motion_command);
@@ -103,11 +103,11 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
 
 inline std::string RemoteConfig::Summary() const {
     char buf[256];
-    snprintf(buf, sizeof(buf), "[config] UDP %d, 心跳超时 %.1f s, 未实现项回 0xE00A=%s\n", udp_port,
-             heartbeat_timeout_s, unsupported_as_success ? "否(回 0)" : "是");
+    snprintf(buf, sizeof(buf), "[config] UDP %d, heartbeat timeout %.1f s, unsupported item returns 0xE00A=%s\n", udp_port,
+             heartbeat_timeout_s, unsupported_as_success ? "no (returns 0)" : "yes");
     std::string s = buf;
-    snprintf(buf, sizeof(buf), "[config] 状态上报: %s 基础 %.0f Hz / 运控 %.0f Hz / 设备 %.0f Hz / 异常 %.0f Hz\n",
-             telemetry_enabled ? "开" : "关", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz,
+    snprintf(buf, sizeof(buf), "[config] telemetry: %s basic %.0f Hz / motion %.0f Hz / device %.0f Hz / error %.0f Hz\n",
+             telemetry_enabled ? "on" : "off", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz,
              telemetry_error_hz);
     s += buf;
     return s;

src/invicta_remote/include/invicta_remote/remote_node.h

@@ -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"
@@ -86,13 +87,13 @@ public:
         udp_server_ = std::make_unique<UdpServer>([this](const Frame& f) { return OnFrame(f); }, udp_port_);
         StartTelemetry();
 
-        ROS_INFO_STREAM("[invicta_remote] 协议节点就绪：" << motion_command_topic_ << " <- " << motion_state_topic_);
-        ROS_INFO("[invicta_remote] 已实现：心跳 / 轴指令(2,21) / 运动状态(2,22) / 步态(2,23) / "
-                 "使用模式(1101,5 仅常规) / 按键(1101,12)");
-        ROS_INFO("[invicta_remote] 状态上报：1002/6 %.0f Hz、1002/4 %.0f Hz、1002/5 %.0f Hz、1002/3 %.0f Hz"
-                 "（%s）", basic_hz_, motion_hz_, device_hz_, error_hz_,
-                 require_subscription_ ? "需先发查询订阅" : "主动推送");
-        ROS_INFO("[invicta_remote] 未实现：照明灯 休眠 充电 导航 速度指令（回 0xE00A）");
+        ROS_INFO_STREAM("[invicta_remote] protocol node ready: " << motion_command_topic_ << " <- " << motion_state_topic_);
+        ROS_INFO("[invicta_remote] implemented: heartbeat / axis command(2,21) / motion state(2,22) / gait(2,23) / "
+                 "usage mode(1101,5 normal only) / key(1101,12)");
+        ROS_INFO("[invicta_remote] telemetry: 1002/6 %.0f Hz, 1002/4 %.0f Hz, 1002/5 %.0f Hz, 1002/3 %.0f Hz"
+                 "(%s)", basic_hz_, motion_hz_, device_hz_, error_hz_,
+                 require_subscription_ ? "query subscription required" : "push autonomously");
+        ROS_INFO("[invicta_remote] not implemented: light, sleep, charge, navigation, velocity command (reply 0xE00A)");
     }
 
 private:
@@ -122,7 +123,7 @@ private:
         try {
             doc = json::parse(frame.body);
         } catch (const std::exception& e) {
-            std::cout << "[GAMEPAD] JSON 解析失败：" << e.what() << std::endl;
+            std::cout << "[GAMEPAD] JSON parse failed: " << e.what() << std::endl;
             return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
         }
 
@@ -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;
     }
@@ -356,7 +358,7 @@ private:
         double value[kM20Joints];
         FillM20Joints(value);
         if (!have_joints_ && std::chrono::steady_clock::now() - start_time_ > std::chrono::seconds(2)) {
-            ROS_WARN_THROTTLE(30.0, "[invicta_remote] 尚未收到 %s，1002/4 的关节按 0 上报",
+            ROS_WARN_THROTTLE(30.0, "[invicta_remote] %s not received yet, 1002/4 joints reported as 0",
                               joint_states_topic_.c_str());
         }
         motor["Joint"] = std::vector<double>(value, value + kM20Joints);
@@ -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();
@@ -427,7 +443,7 @@ private:
         heartbeat_->SetSubscribed(frame.ip, frame.port, command);
         if (!subscribed_logged_.count({frame.ip, command})) {
             subscribed_logged_.insert({frame.ip, command});
-            std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " 查询状态上报 1002/" << command
+            std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " query telemetry 1002/" << command
                       << std::endl;
         }
         switch (command) {
@@ -501,7 +517,7 @@ private:
         const auto now = std::chrono::steady_clock::now();
         if (now - last_heartbeat_log_ > std::chrono::seconds(2)) {
             last_heartbeat_log_ = now;
-            std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << "（状态上报已开始推送）"
+            std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << " (telemetry push started)"
                       << std::endl;
         }
         return Result::Ok();
@@ -562,8 +578,8 @@ private:
             std::lock_guard<std::mutex> lock(state_mutex_);
             if (have_state_ && actual_motion_state_ != kMotionRlControl) {
                 return Result::Err(kErrNotAllowedOperation,
-                                   std::string("Not allowed operation: 步态切换仅 RL 控制状态下有效（当前 ") +
-                                       MotionParamName(actual_motion_state_) + "）");
+                                   std::string("Not allowed operation: gait switch only valid in RL control state (current ") +
+                                       MotionParamName(actual_motion_state_) + ")");
             }
         }
         latest_.gait = uint32_t(gait);
@@ -583,7 +599,7 @@ private:
             PublishMotionCommand();
             return Result::Ok();
         }
-        return Unsupported("使用模式 " + std::to_string(mode) + "（仅支持常规模式）");
+        return Unsupported("usage mode " + std::to_string(mode) + " (only normal mode supported)");
     }
 
     /// 1101/12 按键事件：新版 App（≥ V1.5.11）用按键，这里映射到同一套运动状态
@@ -601,7 +617,7 @@ private:
                     latest_.target_state = it->second;
                     PublishMotionCommand();
                 } else {
-                    std::cout << "[GAMEPAD] 未知按键 " << name << std::endl;
+                    std::cout << "[GAMEPAD] unknown key " << name << std::endl;
                 }
             }
         }
@@ -611,10 +627,10 @@ private:
     /// 未实现的功能：统一返回 0xE00A（可用参数改成回 0）
     Result Unsupported(const std::string& what) {
         if (unsupported_as_success_) {
-            std::cout << "[GAMEPAD] " << what << " 未实现，按 unsupported_as_success=true 回 0" << std::endl;
+            std::cout << "[GAMEPAD] " << what << " not implemented, reply 0 per unsupported_as_success=true" << std::endl;
             return Result::Ok();
         }
-        std::cout << "[GAMEPAD] " << what << " 未实现，回 0xE00A" << std::endl;
+        std::cout << "[GAMEPAD] " << what << " not implemented, reply 0xE00A" << std::endl;
         return Result::Err(kErrUnsupportedFunction, "Unsupported function: " + what);
     }
 
@@ -625,18 +641,18 @@ private:
         handlers_[{kTypeMotion, kCmdAxisControl}] = [this](json& i, const Frame& f) { return HandleAxis(i, f); };
         handlers_[{kTypeMotion, kCmdMotionState}] = [this](json& i, const Frame& f) { return HandleMotionState(i, f); };
         handlers_[{kTypeMotion, kCmdGait}] = [this](json& i, const Frame& f) { return HandleGait(i, f); };
-        handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("自主充电 2/24"); };
+        handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("autonomous charging 2/24"); };
         handlers_[{kTypeMotion, kCmdVelocity}] = [this](json&, const Frame&) {
-            return Unsupported("速度指令 2/25（导航模式）");
+            return Unsupported("velocity command 2/25 (navigation mode)");
         };
 
         handlers_[{kTypeDevice, kCmdLight}] = [this](json&, const Frame&) {
-            return Unsupported("照明灯 1101/2（本平台无灯硬件）");
+            return Unsupported("light 1101/2 (no light hardware on this platform)");
         };
         handlers_[{kTypeDevice, kCmdUsageMode}] = [this](json& i, const Frame& f) { return HandleUsageMode(i, f); };
-        handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("休眠设置 1101/6"); };
+        handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("sleep set 1101/6"); };
         handlers_[{kTypeDevice, kCmdSleepQuery}] = [this](json&, const Frame&) {
-            return Unsupported("休眠查询 1101/7");
+            return Unsupported("sleep query 1101/7");
         };
         handlers_[{kTypeDevice, kCmdKey}] = [this](json& i, const Frame& f) { return HandleKey(i, f); };
 
@@ -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_;

src/invicta_remote/include/invicta_remote/udp_server.h

@@ -154,7 +154,7 @@ private:
             uint8_t from_type = kFromTypeUser;
             bool format_ok = true;
             if (ParseHeader(buffer.data(), int(recvLen), body_len, data_id, from_type, format_ok) < 0) {
-                std::cout << "[UdpServer] 非法帧头，丢弃（长度 " << recvLen << "）" << std::endl;
+                std::cout << "[UdpServer] invalid frame header, drop (length " << recvLen << ")" << std::endl;
                 continue;
             }
 

src/invicta_remote/src/invicta_remote_node.cpp

@@ -19,7 +19,7 @@ int main(int argc, char** argv) {
     if (config_path.empty()) config_path = ros::package::getPath("invicta_remote") + "/config/invicta_remote.toml";
     std::string err;
     if (!invicta_remote::Config().Load(config_path, &err)) {
-        ROS_FATAL("[invicta_remote] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
+        ROS_FATAL("[invicta_remote] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
         return 1;
     }
     std::cout << invicta_remote::Config().Summary() << std::flush;