diff --git a/README.md b/README.md
index bb6e78d..60d2e2c 100644
--- a/README.md
+++ b/README.md
@@ -5,6 +5,7 @@
 - 上游参考代码：`sdk_deploy/src/M20_sdk_deploy`（ROS2 / ament / drdds）
 - 本工程：ROS1 / catkin / Noetic / Ubuntu 20.04
 - 协议依据：`proto.md`（山猫 M20 basic_server V1.2.0，UDP + JSON）
+- 关节模组 CANFD 协议：[`docs/zhishen_joint_module_canfd.md`](docs/zhishen_joint_module_canfd.md)（智身模组手册 V1.2）；文档索引见 [`docs/README.md`](docs/README.md)
 
 > 代码源自 [DeepRoboticsLab/sdk_deploy](https://github.com/DeepRoboticsLab/sdk_deploy) 的 M20 sim2real 部分，遵循 BSD-3-Clause（见 `LICENSE`）；已按 ROS1、厂家完整协议与本机器人形态重构。
 
diff --git a/docs/README.md b/docs/README.md
new file mode 100644
index 0000000..58d4398
--- /dev/null
+++ b/docs/README.md
@@ -0,0 +1,8 @@
+# 文档索引
+
+| 文档 | 内容 |
+| --- | --- |
+| [zhishen_joint_module_canfd.md](zhishen_joint_module_canfd.md) | 智身关节模组 CANFD 通信协议（整理自厂家手册 V1.2）：通信参数、帧格式、使能/失能/运行/置零指令、状态与故障位、数值编码、各型号阈值 |
+| [TODO.md](TODO.md) | 待办清单：每项写清原因与验收方式，已完成项在文末 |
+
+厂家原始资料：《智身关节模组使用手册 V1.2（CANFD 版）》，2026-04-22。
diff --git a/docs/TODO.md b/docs/TODO.md
new file mode 100644
index 0000000..4621c0e
--- /dev/null
+++ b/docs/TODO.md
@@ -0,0 +1,88 @@
+# TODO
+
+本文件记录**当前会话中确认但本期不做**的事项。按优先级分组，每项尽量写清「为什么」和「验收方式」，
+避免下次重新推导。已完成的部分见各包 README。
+
+## P0：安全链路（真机跑 RL 前必须处理）
+
+- [ ] **异常数据接入 deploy 的保护链路**
+  硬件已在 `/joint_diagnostics` 提供温度/状态字/电压（10 Hz），但 deploy 侧仍是常量：
+  `robot_interface.h` 的 `GetMotorTemperture()` 返回 30 ℃、`GetDriverStatusWord()` 返回全 1，
+  所以 `SafeController` 的过温检查和驱动报错检查恒不触发。
+  做法：`InvictaInterface` 订阅 `/joint_diagnostics` 并覆盖这两个虚函数。
+  注意：协议 Byte9 的 bit0 是「使能中」，而 `IsDriverStatusNormal()` 判断 `status_word != 1` 报错，
+  失能但健康的电机回 `0x00` 会误判；建议在接口层归一化成「无故障位就填 1」。
+  验收：人为把某个关节温度改成 101 ℃，deploy 进入 `safe_control_mode=2` 并切 `kJointDamping`。
+
+- [ ] **诊断超时判断**
+  诊断话题 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。
+
+## P1：标定与配置一致性（上真机前）
+
+- [ ] **`dir`/`offset` 的施加方未定**
+  hardware 现在收发的都是**原始电机量**，`dir`/`offset_deg` 由 deploy 的 `InvictaInterface` 施加；
+  但 hardware 的 README 原设计与 `hardware_bridge.h` 注释写的是「由本节点施加」。
+  两边都施加时 `dir=±1` 会二次相乘抵消、`offset` 会翻倍（当前 offset 全 0，尚未暴露）。
+  二选一并同步修改另一侧配置与文档；同时注意 `[joint_limits]` 在两份 TOML 里重复维护。
+
+- [ ] **反馈帧仲裁 ID 需真机确认**
+  `ReadHardware` 按「反馈使用关节自身 CANID」实现（手册只写了从站 CANID 1~6）。
+  上电后 `candump can0` 核对；若不是 1~4，需要改匹配逻辑。
+
+- [ ] **`lie_height = 0` 与位置限位矛盾**
+  `invicta_deploy.toml` 里 `lie_height = 0.00` 反解 `θ = acos(0) = 90° = 1.571 rad`，
+  超过同文件 `[joint_limits].upper = 1.40`。确认是笔误还是限位需要放宽。
+
+- [ ] **轮的位置限位占位值**
+  `[joint_limits]` 中轮的 `lower/upper = ±100` 既不在归一化边界内，也不表示限幅
+  （轮不支持位置控制，手册 §4.4）。加载校验时已跳过，但语义上应改成 ±12.5 或显式标注「仅占位」。
+
+- [ ] **摆臂速度限幅复核**
+  `vel_limit = 22.40 rad/s` 是额定 15.5 的 1.45 倍，比轮的 100/81.2（1.23 倍）更激进。
+  如果当初是估的，建议按实测重新定。
+
+- [ ] **位置指令越界目前静默夹紧**
+  力矩和 kp/kd 越界会告警，位置不会。策略动作顶到限位时可能值得提示一次（注意别刷屏）。
+
+## P2：协议状态上报的剩余占位
+
+- [ ] `MotionStatus` 的 `LinearX / LinearY / Height / RemainMile`：需要里程计、运动学估计与电池数据。
+- [ ] `1002/5` 的电池（电压/电量/温度/serial）、CPU、GPS：无数据源。
+- [ ] `Charge / HES / Sleep`：没有充电状态、硬急停信号与休眠管理。
+- [ ] `HipY` 的正负号未与真实 App/M20 对照：如果 App 只是显示数值没问题，若渲染姿态需要核对。
+- [ ] 未实现指令：照明灯、休眠设置/查询、自主充电、速度指令（导航模式）、导航任务（Pro）。
+
+## P3：工程与清理
+
+- [ ] **启动时序的 `kEnable` 可能被采样合并**
+  deploy 已改为「等订阅者再发」且 hardware 在 `kMotorControl` 未使能时会补发使能，功能上已安全；
+  但 hardware 循环线程处理 `kErrorReset` 的使能连发会阻塞约 80 ms，期间可能观察不到 `kEnable`。
+  若要严格逐帧落地，可把连发改成分段非阻塞发送。
+- [ ] `[motor.gains].bench_*` 已被指令里的 kp/kd 取代，确认是否删除。
+- [ ] 构建镜像 `localhost/custom-ros-noetic` 缺 toml++，目前靠挂载外部安装树 + `CMAKE_PREFIX_PATH`。
+  可考虑在镜像里装 toml++，或在仓库内 vendored。
+- [ ] 真机联调清单：`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] 容器内构建 + 冒烟测试；修复启动时序丢失与订阅回调阻塞
diff --git a/docs/zhishen_joint_module_canfd.md b/docs/zhishen_joint_module_canfd.md
new file mode 100644
index 0000000..c1c530a
--- /dev/null
+++ b/docs/zhishen_joint_module_canfd.md
@@ -0,0 +1,282 @@
+# 智身关节模组 CANFD 通信协议
+
+> 整理自《智身关节模组使用手册 V1.2（CANFD 版）》，智身新创（苏州）智能科技有限公司，2026-04-22。
+> 本文覆盖手册第 3 章协议主体，并补充第 1 章阻抗控制模型与第 2 章中影响协议使用的约束。
+> 手册 V1.2 的变更：修改 P65、P85MAX-S 转矩阈值；新增 W110-S、P100MAX-S 关节通信阈值。
+
+## 1. 适用范围
+
+- 硬件：支持 CAN FD 的关节模组控制器与主控设备。
+- 场景：电机控制、状态监测、参数配置。
+- 控制模式：**阻抗控制模式**。电机使能后默认进入该模式，内部最终执行力矩闭环：
+
+```
+力矩指令 = Kp × (位置指令 − 反馈位置)
+         + Kd × (速度指令 − 反馈速度)
+         + 前馈力矩指令
+```
+
+该模型无积分项。Kp 越大位置刚度越高；Kd 越大阻尼越强、动态响应越慢。位置、速度、转矩、阻尼四种“模式”只是同一模型在不同参数配置下的表现：
+
+| 控制方式 | 参数配置 | 说明 |
+| --- | --- | --- |
+| 位置控制 | 给定 `P_cmd`，`Kp > 0`，建议 `Kd > 0` | 围绕目标位置的弹性约束，允许位置偏差；轮毂电机不支持 |
+| 速度控制 | 给定 `V_cmd`，`Kd > 0`，`Kp = 0` | Kd 相当于比例系数，存在稳态速度误差 |
+| 转矩控制 | 给定 `T_ff`，其余为 0 | 直接输出恒定转矩；负载小于给定转矩时会持续加速 |
+| 阻尼控制 | 仅 `Kd > 0`，其余为 0 | 输出与速度反向、大小成比例的阻力，不约束位置 |
+
+## 2. CANFD 通信参数
+
+| 类别 | 参数项 | 参数说明 |
+| --- | --- | --- |
+| 参数配置 | 波特率（仲裁段） | 1 Mbps |
+| 参数配置 | 波特率（数据段） | 5 Mbps |
+| 参数配置 | 帧格式 | 标准帧 |
+| 参数配置 | 数据长度（主站发送） | 64 字节（可配置） |
+| 参数配置 | 数据长度（关节返回） | 12 字节 |
+| CANID | 主站 CANID | `0x00` |
+| CANID | 从站关节 CANID | `0x0X`（X = 1~6） |
+| CANID | 关节出厂默认 CANID | `0x03` |
+| 通信速率 | 主机与关节通信速率 | 最大支持 1 kHz |
+
+多字节字段全部为 **小端（低字节在前）**。下文示例中的 `00 80` 即 uint16 的 `0x8000`。
+
+## 3. 帧格式
+
+### 3.1 主机下发帧
+
+一帧最多控制 6 个关节，每个关节占 10 字节，按 CANID 顺序排列：
+
+| Byte | 功能 | 说明 |
+| --- | --- | --- |
+| Byte0–Byte1 | MSGID | 主机发送，从机原样返回；用于匹配请求帧，不用可填 0 |
+| Byte2–Byte11 | CANID=1 指令数据 | 见第 4 节 |
+| Byte12–Byte21 | CANID=2 指令数据 | 同上 |
+| Byte22–Byte31 | CANID=3 指令数据 | 同上 |
+| Byte32–Byte41 | CANID=4 指令数据 | 同上 |
+| Byte42–Byte51 | CANID=5 指令数据 | 同上 |
+| Byte52–Byte61 | CANID=6 指令数据 | 同上 |
+| Byte62–Byte63 | 保留 | 手册未定义 |
+
+每个关节块的起始字节为 `2 + 10 × (CANID − 1)`。
+
+**下发长度**可按最大 CANID 裁剪：
+
+| 最大关节 CANID | 下发最小字节数 |
+| ---: | ---: |
+| 1 | 12 |
+| 2 | 24 |
+| 3 | 32 |
+| 4 | 48 |
+| 5 / 6 | 64 |
+
+- 最大 CANID=3 时下发 Byte0–Byte31（32 字节）；最大 CANID=1 时下发 Byte0–Byte11（12 字节）。
+- 只控制不连续关节时，仍需按最大 CANID 的长度下发：例如只控 CANID=3，则发 64 字节，仅给 Byte22–Byte31 赋值，其余填 0 或保持上次值；控 CANID=1 和 4 时，给 Byte2–Byte11 与 Byte32–Byte41 赋值。
+- CANID=4 时手册要求 48 字节，而实际数据只用到 Byte41（42 字节），以手册要求为准。
+
+### 3.2 关节反馈帧
+
+对所有指令，关节均返回固定的 12 字节状态帧：
+
+| Byte | 功能 | 说明 |
+| --- | --- | --- |
+| Byte0–Byte1 | MSGID | 等于主机发送值 |
+| Byte2–Byte3 | 关节输出端当前位置 | uint16，见第 5 节映射 |
+| Byte4–Byte5 | 关节输出端当前速度 | uint16，rad/s |
+| Byte6–Byte7 | 关节输出端当前扭矩 | uint16，N·m |
+| Byte8 | 电机当前温度 | uint8，`0~255` 对应 `−40~215 °C`（即 `T = raw − 40`） |
+| Byte9 | 电机当前状态码 | 位定义见第 6 节 |
+| Byte10 | 电机当前电压 | uint8，`0~255` 直接对应 `0~255 V` |
+| Byte11 | 预留 | — |
+
+## 4. 指令数据（每关节 10 字节）
+
+### 4.1 电机使能
+
+| Byte | 说明 | 取值 |
+| --- | --- | --- |
+| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
+| Byte11 | 使能指令码 | `0xFC` |
+
+示例（使能所有关节，MSGID = `0x0100`）：
+
+```
+Byte0-1   : 00 01
+Byte2-11  : FF FF FF FF FF FF FF FF FF FC
+Byte12-21 : FF FF FF FF FF FF FF FF FF FC
+...
+Byte52-61 : FF FF FF FF FF FF FF FF FF FC
+Byte62-63 : xx xx
+```
+
+### 4.2 电机失能
+
+| Byte | 说明 | 取值 |
+| --- | --- | --- |
+| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
+| Byte11 | 使能指令码 | `0xFD` |
+
+失能后所有 MOS 管关闭，电机线圈处于断开状态。
+
+### 4.3 电机运行
+
+必须在使能之后发送。运行指令块内容：
+
+| Byte | 说明 | 取值 |
+| --- | --- | --- |
+| Byte2–Byte3 | 位置指令 | uint16，`0~65535` 映射到 `P_MIN~P_MAX`（rad），`0x8000` = 0 rad |
+| Byte4–Byte5 | 速度指令 | uint16，`0~65535` 映射到 `V_MIN~V_MAX`（rad/s），`0x8000` = 0 rad/s |
+| Byte6–Byte7 | 刚度 Kp 指令 | uint16，`0~65535` 映射到 `0~KP_MAX`，`0x0000` = 0 |
+| Byte8–Byte9 | 阻尼 Kd 指令 | uint16，`0~65535` 映射到 `0~KD_MAX`，`0x0000` = 0 |
+| Byte10–Byte11 | 前馈力矩指令 | uint16，`0~65535` 映射到 `T_MIN~T_MAX`（N·m），`0x8000` = 0 N·m |
+
+示例（所有关节 0 位置、0 速度、0 Kp、0 Kd、0 力矩）：
+
+```
+Byte0-1   : 00 01
+Byte2-11  : 00 80 00 80 00 00 00 00 00 80
+Byte12-21 : 00 80 00 80 00 00 00 00 00 80
+...
+Byte52-61 : 00 80 00 80 00 00 00 00 00 80
+```
+
+### 4.4 设置当前位置为机械零位
+
+| Byte | 说明 | 取值 |
+| --- | --- | --- |
+| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
+| Byte11 | 零位指令码 | `0xEF` |
+
+设置成功且关节角度未变化时，当前反馈位置为 0。电机在任意位置的上电角度上报范围为 `(−π, π)` rad。
+
+> 完成机械零点标定后，关节具备单圈绝对位置记忆：断电及人为转动输出端后重新上电，仍可恢复当前单圈绝对位置。**轮毂电机不支持该功能，也不支持位置控制。**
+
+## 5. 数值编码
+
+所有物理量以 uint16 线性映射。手册写作 `0~65535 → MIN~MAX`。
+
+编码（物理量 → 原始值）：
+
+```
+raw = round((x - MIN) / (MAX - MIN) × 65535)
+```
+
+解码（原始值 → 物理量）：
+
+```
+x = MIN + raw / 65535 × (MAX - MIN)
+```
+
+说明：
+
+- 位置、速度、前馈力矩为对称范围，因此 `0x8000`（32768）对应 0。按上式，`raw = 32768` 时 `x ≈ MIN + 0.5000076 × (MAX − MIN)`，与 0 的偏差来自 65536 个码值无法整除；工程上通常直接按 `32768` 作为零点，或用 `(raw − 32768) / 32768 × MAX` 计算对称量。
+- Kp、Kd 为非负范围，`0x0000` 对应 0。
+- 下发前必须按关节型号的 `P/V/T/KP/KD` 阈值限幅，见第 8 节。
+
+Python 参考实现：
+
+```python
+def encode(x, lo, hi):
+    r = round((x - lo) / (hi - lo) * 65535)
+    return max(0, min(65535, r))
+
+def decode(raw, lo, hi):
+    return lo + raw / 65535 * (hi - lo)
+
+def pack_u16(v):
+    return bytes((v & 0xFF, (v >> 8) & 0xFF))  # 小端
+
+def run_block(p, v, kp, kd, t, lim):
+    return b"".join((
+        pack_u16(encode(p, lim["p_min"], lim["p_max"])),
+        pack_u16(encode(v, lim["v_min"], lim["v_max"])),
+        pack_u16(encode(kp, 0.0, lim["kp_max"])),
+        pack_u16(encode(kd, 0.0, lim["kd_max"])),
+        pack_u16(encode(t, lim["t_min"], lim["t_max"])),
+    ))
+```
+
+## 6. 状态码与故障保护
+
+Byte9 状态码按位定义（1 有效）：
+
+| bit | 功能 | 触发条件 | 恢复措施 |
+| ---: | --- | --- | --- |
+| 0 | 电机使能 | 使能为 1，失能为 0 | — |
+| 1 | 过压故障 | 母线电压超过阈值 | 重启或重新使能 |
+| 2 | 过流故障 | 电流超过阈值 | 重启或重新使能 |
+| 3 | 过温故障 | 电机温度超过阈值 | 重启或重新使能；状态位低于 85 °C 自动恢复 |
+| 4 | 过速故障 | 输出端速度超过阈值 | 重启或重新使能 |
+| 5 | 编码器故障 | 编码器测量失效 | 重启或重新使能 |
+| 6 | 欠压故障 / 预驱故障 | 母线电压低于阈值，或检测到预驱故障 | 重启或重新使能 |
+| 7 | 预留 | — | — |
+
+发生故障时电机会自动失能，同时对应状态位置 1。主站应持续检查 Byte9 的 bit1–bit6，非零即进入故障处理。
+
+## 7. 关节型号与阈值
+
+| 关节型号 | P_MAX | P_MIN | V_MAX | V_MIN | T_MAX | T_MIN | KP_MAX | KD_MAX |
+| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
+| P55 | 12.5 | −12.5 | 65 | −65 | 30 | −30 | 500 | 5 |
+| P65 | 12.5 | −12.5 | 65 | −65 | 50 | −50 | 500 | 5 |
+| P65MAX-S | 12.5 | −12.5 | 65 | −65 | 100 | −100 | 500 | 5 |
+| P85MAX-S | 12.5 | −12.5 | 65 | −65 | 300 | −300 | 500 | 50 |
+| W150-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
+| W190-S | 12.5 | −12.5 | 200 | −200 | 50 | −50 | 500 | 5 |
+| W110-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
+| P100MAX-S | 12.5 | −12.5 | 65 | −65 | 650 | −650 | 500 | 50 |
+
+单位：位置 rad，速度 rad/s，转矩 N·m；Kp、Kd 无量纲。轮毂电机（W 系列）速度阈值明显高于摆臂电机。
+
+### 7.1 额定参数与额定速度（厂家参数页补充）
+
+| 型号 | 额定功率 (W) | 额定扭矩 (N·m) | 峰值扭矩 (N·m) | 推算额定速度 (rad/s) |
+| --- | ---: | ---: | ---: | ---: |
+| P55 | 78.5 | 6.5 | 21 | 12.1 |
+| P65 | 330 | 11.5 | 48 | 28.7 |
+| P65MAX-S | 330 | 21.3 | 75 | 15.5 |
+| P85MAX-S | 706 | 43 | 180 | 16.4 |
+| W150-S | 495 | 3.5 | 11 | 141.4 |
+| W190-S | 706 | 8.7 | 28.68 | 81.2 |
+
+额定速度 = 额定功率 / 额定扭矩。它和上表的 `V_MAX` 不是一回事：`V_MAX` 是**通信编码阈值**（决定 uint16 的满量程），额定速度是持续工作能力。本机取的三层关系：
+
+| 关节 | 额定速度 | 人为限幅（`[joint_limits].vel_limit`） | 通信阈值（`[motor.*].v_max`） |
+| --- | ---: | ---: | ---: |
+| 摆臂 P65MAX-S | 15.5 | 22.4 | 65 |
+| 轮 W190-S | 81.2 | 100 | 200 |
+
+人为限幅略高于额定、远低于阈值；实际下发按 `min(阈值, 限幅)` 饱和。注意阈值/额定的比值并不统一（P65MAX-S ≈ 4.2×，W190-S ≈ 2.5×），不能用其中一个推另一个。
+
+## 8. 调试给定值参考
+
+手册给出的典型增益（`P_cmd`, `V_cmd`, `T_ff`, `Kp`, `Kd`）：
+
+| 关节型号 | 位置控制 | 速度控制 | 转矩控制 |
+| --- | --- | --- | --- |
+| P55 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
+| P65 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
+| P65MAX-S | `3.14, 0, 0, 3, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
+| P85MAX-S | `3.14, 0, 0, 5, 1` | `0, 5, 0, 0, 2` | `0, 0, 3, 0, 0` |
+| W150-S | — | `0, 5, 0, 0, 0.1` | `0, 0, 0.3, 0, 0` |
+| W190-S | — | `0, 5, 0, 0, 0.5` | `0, 0, 0.2, 0, 0` |
+
+阻尼控制只需 `Kd > 0`，其余参数置 0。
+
+使用注意：
+
+1. 目标位置与当前位置差值不宜过大，否则会产生瞬时大转矩和大电流；建议给定值缓慢增加。
+2. `Kp`、`Kd` 的比值不宜过大；速度控制时 `Kd` 不宜过大。
+3. 速度项启动转矩由速度给定与 `Kd` 决定，突变会产生电流峰值或振荡。
+4. 转矩控制下若负载小于给定转矩，转速会一直上升到最大转速甚至失控。
+5. 写入参数后需断电重启才能生效；上位机装入参数后需再次“写入参数”。
+
+## 9. 通信时序要点
+
+1. 上电后先发使能（`0xFC`），再发运行指令；失能（`0xFD`）后线圈断开。
+2. 需要绝对零位时，在装配完成后发一次置零（`0xEF`），之后反馈位置以该点为 0。
+3. 主机发送周期最大支持 1 kHz；实际控制周期按上层需求设定，并保证每帧携带同一 MSGID 以匹配反馈。
+4. 一帧可同时控制最多 6 个关节；不同 CANID 的关节共用一个 64 字节帧，按 `2 + 10 × (CANID − 1)` 定位各自数据块。
+5. 每个关节独立返回 12 字节状态帧，主站需按 MSGID 关联请求与反馈。
+6. 反馈中的位置、速度、转矩均使用与下发相同的 `MIN~MAX` 映射，解码时必须使用对应关节型号的阈值。
+7. ⚠️ 编码满量程只能取第 7 节的型号阈值，不能取摇杆/工作范围。台架脚本曾用 `float_to_uint(v, V_MAX=10, ...)` 下发轮速，而 W190-S 的阈值是 ±200：同一 raw 值固件按 ±200 解码，实际轮速被放大 20 倍。工作上限（如 100）应作为独立限幅先夹紧，再按阈值编码。
diff --git a/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp b/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp
index 991ffc9..36e07d9 100644
--- a/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp
+++ b/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp
@@ -121,7 +121,16 @@ protected:
     }
 
     /// 使能时序：disable → error reset → enable；此后运行期恒为 motor control
+    /// ⚠ ROS1 的发布没有 latch：订阅端连接建立前发出去的消息会被直接丢掉。
+    ///   若 hardware 节点后启动（或本节点重启），一次性时序帧会整段丢失，
+    ///   所以这里先等订阅者出现再发。
     void SendStartupSequence() {
+        for (int i = 0; i < 200 && cmd_pub_.getNumSubscribers() == 0 && ros::ok(); ++i) {
+            ros::Duration(0.01).sleep();
+        }
+        if (cmd_pub_.getNumSubscribers() == 0) {
+            std::cout << "[InvictaInterface] 警告：joint_commands 还没有订阅者，启动时序可能丢失" << std::endl;
+        }
         const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
         for (uint16_t word : seq) {
             PublishRawCommand(word);
diff --git a/src/invicta_deploy/scripts/smoke_test.sh b/src/invicta_deploy/scripts/smoke_test.sh
index 5b47623..962968b 100755
--- a/src/invicta_deploy/scripts/smoke_test.sh
+++ b/src/invicta_deploy/scripts/smoke_test.sh
@@ -16,8 +16,8 @@ echo "=== 2. 启动 roscore ==="
 nohup roscore >/tmp/roscore.log 2>&1 &
 sleep 8
 
-echo "=== 3. 启动 invicta_hardware（骨架：发零位状态）==="
-nohup roslaunch invicta_hardware invicta_hardware.launch >/tmp/hw.log 2>&1 &
+echo "=== 3. 启动 invicta_hardware（require_can:=false：无 CAN 时只发零位状态）==="
+nohup roslaunch invicta_hardware invicta_hardware.launch require_can:=false >/tmp/hw.log 2>&1 &
 sleep 4
 
 echo "--- 话题 ---"
diff --git a/src/invicta_hardware/CMakeLists.txt b/src/invicta_hardware/CMakeLists.txt
index 7d26cf1..1225767 100644
--- a/src/invicta_hardware/CMakeLists.txt
+++ b/src/invicta_hardware/CMakeLists.txt
@@ -42,6 +42,9 @@ endif()
 
 add_executable(invicta_hardware_node
   src/invicta_hardware_node.cpp
+  include/invicta_hardware/can_bus.h
+  include/invicta_hardware/joint_module_protocol.h
+  include/invicta_hardware/hardware_bridge.h
   ${NODE_CONFIG_HEADER}
 )
 
diff --git a/src/invicta_hardware/README.md b/src/invicta_hardware/README.md
index 9fab623..0f58c79 100644
--- a/src/invicta_hardware/README.md
+++ b/src/invicta_hardware/README.md
@@ -1,58 +1,92 @@
 # invicta_hardware
 
-关节/IMU 数据源与关节指令执行端。**当前是骨架**：结构、话题、参数都齐了，只有 `ReadHardware()` / `WriteHardware()` 是待实现的 TODO。
+关节/IMU 数据源与关节指令执行端。已接 **SocketCAN + 智身关节模组 CANFD 协议**（协议见
+[docs/zhishen_joint_module_canfd.md](../../docs/zhishen_joint_module_canfd.md)）。
 
 ## 它负责什么
 
 以 `publish_rate_hz`（默认 200 Hz）为一个周期，循环做三件事：
 
-1. `ReadHardware()` — 从电机/IMU 读回状态（TODO）
-2. `PublishState()` — 发布 `sensor_msgs/JointState` 与 `sensor_msgs/Imu`（已实现）
-3. `WriteHardware()` — 把最新指令下发到电机（TODO，含长度与有限性校验、控制字处理）
+1. `ReadHardware()` — 非阻塞收各路总线上的 12 字节反馈帧，更新关节位置/速度/力矩、温度、状态码、电压
+2. `PublishState()` — 发 `sensor_msgs/JointState` 与 `sensor_msgs/Imu`
+3. `WriteHardware()` — 使能后每周期发 MIT 运行帧（p / v / kp / kd / tau）
+
+上游指令断流时（`command_timeout_s`，默认 100 ms）自动转**阻尼帧**（kp=0、速度/力矩 0、只留 `timeout_*_kd`），
+避免 deploy 挂掉/卡住后本节点一直重发最后一帧。指令恢复后自动退出阻尼。
+
+另按 `diagnostics_rate_hz`（默认 10 Hz）发 `invicta_msgs/JointDiagnostics`（温度/状态/电压）。
+
+**反馈不需要主动查询**：模组对每条指令都回一帧状态（手册 §3.2），所以"发了指令就有反馈"，读回和写入天然同频。
 
 | 话题 | 方向 | 消息 |
 |---|---|---|
 | `/joint_states` | 发布 | `sensor_msgs/JointState` |
 | `/imu/data` | 发布 | `sensor_msgs/Imu` |
+| `/joint_diagnostics` | 发布 | `invicta_msgs/JointDiagnostics`（默认 10 Hz） |
 | `/joint_commands` | 订阅 | `invicta_msgs/JointCommandArray` |
 
+## 控制字 → CAN 帧
+
+| `control_word` | 实际动作 |
+|---|---|
+| `kDisable`(1) | 每关节块 `0xFF×9 + 0xFD`，线圈断开，之后不再发运行帧 |
+| `kEnable`(2) | `0xFF×9 + 0xFC`，按 `enable_frames`/`enable_interval_s` 连续刷几帧 |
+| `kMotorControl`(3) | 发运行块；**未使能时拒发**（手册 §9.1 要求先使能再运行） |
+| `kErrorReset`(4) | 本协议没有独立清错指令（手册 §6：故障后靠重新使能/重启），按"重发使能"处理 |
+| `kGetStatus`(5) | 不需要发帧：状态随每条指令自动返回 |
+
+同一路总线上最多 4 个关节（CANID 1~4），合成一帧下发：Byte0-1 是 MSGID `0x0100`，每个 CANID 占
+10 字节，块起点 `2 + 10×(CANID−1)`，长度按手册裁剪表（本机 4 个 ID → 48 字节）。
+
 ## 约定（最重要的部分）
 
-**硬件约定（方向、零点、单位、CAN 帧格式）在本节点内消化，话题上流动的必须是 policy 约定下的量**：
-
 - 关节角 `[rad]`、角速度 `[rad/s]`、力矩 `[Nm]`，按 `joint_names` 的固定顺序。
-- IMU：姿态四元数（**弧度**）、角速度 `[rad/s]`（机体系）、线加速度 `[m/s²]`（机体系）。
-- ⚠️ 上游 M20 的 `/IMU_DATA` 里 `rpy` 是**度**，不要照搬；本工程统一弧度。四元数能避免单位歧义，这也是选 `sensor_msgs/Imu` 的原因之一。
-- 姿态确实拿不到时，必须把 `orientation_covariance[0]` 置 `-1`（而不是发单位四元数假装有效），deploy 侧会据此丢弃该帧。
+- **边界和限幅是两件事**：`[motor.*]` 的 p/v/t 是**归一化边界**——只决定 CAN 帧里 `0~65535` 映射到多少物理量（编码/解码必须用同一套，手册 §5/§9.6），不是运行限幅；`[joint_limits]` 才是唯一实际生效的**限幅**。下发顺序是「先按限幅夹紧 → 再归一化编码」。加载时会校验限幅不超过边界，超了直接报错，不靠编码饱和兜着。
+- **kp/kd 不是策略算的**，来自 deploy 的 `[policy].kp/kd` 与 `[leg.gains]`（用户配置）。越界时本节点会夹到 `[0, kp_max]` / `[0, kd_max]` 并打警告——那种日志说明配置写错了，不要靠饱和兜着。
+- 前馈力矩按 `[joint_limits].torque_limit`（默认取厂家峰值扭矩）夹紧，超过也告警。
+- **本节点收发的都是原始电机量**（电机坐标系/电机单位），`dir`/`offset_deg` 由 `invicta_deploy` 的 `InvictaInterface` 施加。如果改成由本节点施加，必须同时把 deploy 的 `[legs].dir/offset` 置为恒等——否则 `dir=±1` 会二次相乘抵消、`offset_deg` 会翻倍。
+- IMU：姿态四元数（**弧度**）、角速度 `[rad/s]`（机体系）、线加速度 `[m/s²]`（机体系）。⚠️ 目前**还没有 IMU 数据源**，本节点发的是单位四元数，等硬件接上再改。
+- 温度取自反馈帧 Byte8：`T = raw − 40`（0~255 ↔ −40~215 ℃）；电压 Byte10 直接是伏。
+- 状态码 Byte9：bit0 使能，bit1~6 故障（过压/过流/过温/过速/编码器/欠压）。故障位出现时本节点会打 ERROR 日志，并原样放进 `/joint_diagnostics`。
 
-方向/零点换算参考（M20 原车，角度制）：`joint_dir = [1,1,-1,1, 1,-1,1,-1, -1,1,-1,1, -1,-1,1,-1]`，`joint_offset_deg = [-25,-131,160,0, 25,-131,160,0, -25,131,-160,0, 25,131,-160,0]`。新机器人用自己的值。
+## 总线前置条件
 
-**同一套换算只能在一边施加**：本节点做了，`invicta_deploy` 的 `calibration/*` 就保持恒等（默认如此）；反之则要把参数填上并打开 `auto_calibrate`。
+位率由系统配置，本节点只负责 socket 收发：
 
-## 待实现清单
+```bash
+sudo ip link set can0 up type can bitrate 1000000 dbitrate 5000000 fd on
+sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
+```
 
-- [ ] `ReadHardware()`：SocketCAN（或你的总线）读回 16 关节的 position/velocity/effort + IMU，换算到 policy 约定。读失败返回 `false`，保留上一帧值，**不要发 NaN**。
-- [ ] `WriteHardware()`：按 `control_word` 落实使能/失能/清错，把目标角/角速度/前馈力矩/`kp`/`kd` 组装成帧下发；`tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff` 由驱动器算还是本节点算，取决于关节模组协议，选一种并写清楚。
-- [ ] 上电顺序与安全：使能前的自检、通信超时（多久没指令就卸力）、过温/过流保护。注意 deploy 在 `idle` 状态也会以 200 Hz 发零增益指令，所以「按控制字判断是否使能」比「按有无指令判断」更稳。
-- [ ] 关节顺序与 `invicta_deploy` 共用同一份配置里的 `joint_names`，不会不一致。
+`[motor.can].bitrate/data_bitrate` 只用于出错时的提示，不能透过 SocketCAN 设置。
+
+## 无硬件调试开关
+
+| 位置 | 默认 | 作用 |
+|---|---|---|
+| `[hardware].require_can` | `true` | CAN 打不开就退出，避免"以为在控车其实没连上" |
+| `roslaunch ... require_can:=false` | — | 无 CAN 也启动：照常发零位状态、缓存指令，但不下发电机指令 |
+
+`src/invicta_deploy/scripts/smoke_test.sh` 用的就是 `require_can:=false`，因此容器/podman 里没有 CAN 也能跑通。
 
 ## 参数（本节点自己的 `config/invicta_hardware.toml`）
 
 | TOML 段 | 内容 |
 |---|---|
 | `[robot].joint_names` | 关节名与顺序（8 个：`fl/fr/hl/hr × swing/wheel`） |
-| `[joint_limits]` | 逐关节限位（下发前自检用） |
-| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `dir`、`offset_deg`、`bus`、`canid` |
-| `[motor.joint]` / `[motor.wheel]` | 电机量程：`p/v/t/kp/kd` 上下限（P65MAXS / W190S） |
-| `[motor.can]` | 总线名、仲裁 ID、1M/5M 比特率、上电使能帧数与间隔 |
-| `[motor.gains]` | 台架脚本当前增益（进 RL 前以 `invicta_deploy` 的 `[leg.gains]`/策略为准） |
-| `[topics]` `[hardware]` | 本节点话题、发布频率、IMU frame |
+| `[joint_limits]` | 人为限幅：位置 `[rad]`、速度 `[rad/s]`、前馈力矩 `[N·m]`（下发前夹紧） |
+| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `bus`、`canid`（`dir`/`offset_deg` 目前由 deploy 施加） |
+| `[motor.joint]` / `[motor.wheel]` | 归一化边界（0~65535 ↔ 物理量），编码/解码必须一致（P65MAX-S / W190-S） |
+| `[motor.can]` | 总线名、仲裁 ID、1M/5M、上电使能帧数与间隔 |
+| `[motor.gains]` | `bench_*` 仅作参考（实际 kp/kd 由指令给）；`timeout_*_kd` 是看门狗阻尼 |
+| `[topics]` `[hardware]` | 本节点话题、200 Hz 状态频率、10 Hz 诊断频率、`require_can`、`command_timeout_s` |
 
-启动时打印关节表、每条腿的总线/CANID 与电机量程，便于接线核对。
 ## 运行
 
 ```bash
-roslaunch invicta_hardware invicta_hardware.launch
+roslaunch invicta_hardware invicta_hardware.launch                  # 真机
+roslaunch invicta_hardware invicta_hardware.launch require_can:=false  # 无 CAN 调试
 ```
 
-骨架状态下会打印告警，并且只发布零位状态、只缓存指令，**不会驱动电机**。
+自查：`rostopic echo /joint_diagnostics` 看温度/状态；`candump can0` 看反馈帧的仲裁 ID（当前按
+"反馈使用关节自身 CANID" 实现，若模组回帧 ID 不同需改 `ReadHardware` 的匹配）。
diff --git a/src/invicta_hardware/config/invicta_hardware.toml b/src/invicta_hardware/config/invicta_hardware.toml
index 1a2c9aa..8ae1494 100644
--- a/src/invicta_hardware/config/invicta_hardware.toml
+++ b/src/invicta_hardware/config/invicta_hardware.toml
@@ -13,13 +13,19 @@ joint_names = [
   "hr_swing", "hr_wheel",
 ]
 
-# ------------------------------------------------------------------ 关节限位
-# 顺序与 joint_names 一致；轮关节的 lower/upper 填 ±100 表示不限（用 vel_limit 约束）
-# 本节点实现 I/O 时用它做下发前的自检
+# ------------------------------------------------------------------ 关节限位（唯一实际生效的限幅）
+# 顺序与 joint_names 一致。这里才是「限幅」：机械/使用上的可用范围，下发前按它夹紧。
+# 与 [motor.*] 的关系：那里是「归一化边界」，只决定 0~65535 映射到多少物理量，不是限幅。
+# 加载时会校验本节限幅不超过归一化边界；超了直接报错，不靠编码饱和悄悄压回去。
+# 轮关节：轮毂不支持位置控制（手册 §4.4），lower/upper 是占位值，只用 vel_limit；
+# 轮 vel_limit = 100 [rad/s] 是人为限幅：略高于额定速度（额定功率/扭矩推得 81.2 rad/s）。
 [joint_limits]
 lower     = [-1.40, -100.0,  -1.40, -100.0,  -1.40, -100.0,  -1.40, -100.0]   # [rad]
 upper     = [ 1.40,  100.0,   1.40,  100.0,   1.40,  100.0,   1.40,  100.0]   # [rad]
 vel_limit = [ 22.40, 100.0,  22.40, 100.0,  22.40, 100.0,  22.40, 100.0]      # [rad/s]
+# 前馈力矩限幅 [N·m]：取厂家参数页的峰值扭矩（摆臂 P65MAX-S 75、轮 W190-S 28.68），
+# 比驱动器 T_MAX（100 / 50）更保守。超了就夹紧并告警。
+torque_limit = [75.0, 28.68,  75.0, 28.68,  75.0, 28.68,  75.0, 28.68]
 
 # ------------------------------------------------------------------ 每腿的硬件标定与总线
 # 指令换算：q_motor = (q_model − offset_deg) · dir；反馈反向。
@@ -56,7 +62,10 @@ swing = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 2 }
 wheel = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 4 }
 
 # ------------------------------------------------------------------ 电机与总线
-# 数值取自当前 CANFD 台架脚本（遥控 → can0/can1 双路），实现 ReadHardware/WriteHardware 时按此驱动
+# p/v/t/kp/kd 是「归一化边界」（厂家手册 V1.2 §7，见 docs/zhishen_joint_module_canfd.md）：
+# 它只决定 CAN 帧里 0~65535 映射到多少物理量，编码与解码必须用同一套（§5/§9.6），
+# 不是运行限幅——限幅在 [joint_limits]。必须与模组实际配置一致。
+# CAN 参数（总线名/仲裁 ID/波特率/使能帧）取自台架脚本。
 [motor.joint]                # 摆臂关节电机
 model  = "P65MAXS"
 p_max  = 12.5                # [rad]
@@ -72,8 +81,10 @@ kd_max = 5.0
 model  = "W190S"
 p_max  = 12.5
 p_min  = -12.5
-v_max  = 10.0                # [rad/s]
-v_min  = -10.0
+v_max  = 200.0               # [rad/s] 手册 §7 的 W190-S 阈值，编码/解码满量程
+v_min  = -200.0              # 注意：这不是工作上限（工作上限是 [joint_limits] 的 100）。
+                             # 台架脚本曾把摇杆范围 ±10 当满量程，换算后轮速被放大 20×，
+                             # 属用法错误，不要照搬
 t_max  = 50.0
 t_min  = -50.0
 kp_max = 500.0
@@ -91,14 +102,31 @@ enable_interval_s = 0.02
 bench_joint_kp = 150.0
 bench_joint_kd = 2.0
 bench_wheel_kd = 0.5
+# 指令看门狗触发后的阻尼（kp=0、给定速度 0，只留 Kd 抵抗外力）
+timeout_joint_kd = 4.0
+timeout_wheel_kd = 1.0
 
 # ------------------------------------------------------------------ 本节点的话题与节奏
 [topics]
-joint_states   = "joint_states"      # 发布 sensor_msgs/JointState
-imu            = "imu/data"          # 发布 sensor_msgs/Imu
-joint_commands = "joint_commands"    # 订阅 invicta_msgs/JointCommandArray
+joint_states      = "joint_states"       # 发布 sensor_msgs/JointState
+imu               = "imu/data"           # 发布 sensor_msgs/Imu
+joint_commands    = "joint_commands"     # 订阅 invicta_msgs/JointCommandArray
+joint_diagnostics = "joint_diagnostics"  # 发布 invicta_msgs/JointDiagnostics（温度/状态/电压）
 
 [hardware]
-publish_rate_hz = 200.0              # 关节/IMU 状态发布频率（RL 侧控制环 200 Hz）
-imu_frame_id    = "base_link"
-command_queue   = 1                  # 指令只取最新一帧
+publish_rate_hz     = 200.0              # 关节/IMU 状态发布频率（RL 侧控制环 200 Hz）
+diagnostics_rate_hz = 10.0               # 诊断量频率：CAN 反馈每条指令都有，但话题按低频发
+imu_frame_id        = "base_link"
+command_queue       = 1                  # 指令只取最新一帧
+# true：CAN 打不开就退出（真机默认，避免以为在控车其实没连上）
+# false：没有 CAN 也启动，只发零位状态与指令缓存（调试 / 无硬件冒烟测试）
+# launch 里可用 require_can:=false 覆盖，例如 smoke_test.sh
+require_can         = true
+# 指令看门狗：超过这么久没收到 /joint_commands 就转阻尼（kp=0 + [motor.gains].timeout_*_kd），
+# 避免上游进程挂掉后一直重发最后一帧。0 = 关闭。
+command_timeout_s   = 0.1
+
+# ------------------------------------------------------------------ 总线前置条件（不在本节点里设置）
+# 位率必须由系统配置好，本节点只负责 socket 收发：
+#   sudo ip link set can0 up type can bitrate 1000000 dbitrate 5000000 fd on
+#   sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
diff --git a/src/invicta_hardware/include/invicta_hardware/can_bus.h b/src/invicta_hardware/include/invicta_hardware/can_bus.h
new file mode 100644
index 0000000..180825e
--- /dev/null
+++ b/src/invicta_hardware/include/invicta_hardware/can_bus.h
@@ -0,0 +1,176 @@
+/**
+ * @file can_bus.h
+ * @brief SocketCAN / CANFD 收发封装（Linux 原生接口，无第三方依赖）
+ *
+ * 只负责 socket 层的打开、非阻塞收发；帧内编码见 joint_module_protocol.h。
+ * 比特率由系统配置，本文件不动（需要 root）：
+ *
+ *   sudo ip link set can0 up type can bitrate 1000000 dbitrate 5000000 fd on
+ *   sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
+ *
+ * 配置里的 bitrate / data_bitrate 只用于日志提示，不能透过 SocketCAN 设置。
+ */
+#pragma once
+
+#include <fcntl.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <cerrno>
+#include <cstring>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace invicta_hardware {
+
+/// 与 canfd_frame 同形的载体，让上层不必直接依赖 linux/can.h
+struct CanFrame {
+    uint32_t id = 0;    ///< 仲裁 ID（标准帧，不含标志位）
+    uint8_t len = 0;    ///< 数据长度
+    bool fd = false;    ///< 是否 CANFD 帧
+    uint8_t data[64] = {};
+};
+
+class CanBus {
+public:
+    CanBus() = default;
+    explicit CanBus(std::string name) : name_(std::move(name)) {}
+    ~CanBus() { Close(); }
+
+    CanBus(const CanBus&) = delete;
+    CanBus& operator=(const CanBus&) = delete;
+    CanBus(CanBus&& other) noexcept : fd_(other.fd_), name_(std::move(other.name_)) { other.fd_ = -1; }
+    CanBus& operator=(CanBus&& other) noexcept {
+        if (this != &other) {
+            Close();
+            fd_ = other.fd_;
+            name_ = std::move(other.name_);
+            other.fd_ = -1;
+        }
+        return *this;
+    }
+
+    /// 打开并绑定一路 CAN 接口，开启 FD 帧与非阻塞读
+    bool Open(const std::string& ifname, std::string* err = nullptr) {
+        Close();
+        const auto fail = [&](const std::string& what) {
+            if (err) *err = what + ": " + std::strerror(errno);
+            Close();
+            return false;
+        };
+
+        const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
+        if (fd < 0) return fail("socket(PF_CAN)");
+
+        struct ifreq ifr {};
+        std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
+        if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
+            ::close(fd);
+            errno = ENODEV;
+            return fail("ioctl(SIOCGIFINDEX) " + ifname);
+        }
+
+        const int enable_fd = 1;
+        if (::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_fd, sizeof(enable_fd)) < 0) {
+            ::close(fd);
+            return fail("setsockopt(CAN_RAW_FD_FRAMES)");
+        }
+
+        struct sockaddr_can addr {};
+        addr.can_family = AF_CAN;
+        addr.can_ifindex = ifr.ifr_ifindex;
+        if (::bind(fd, reinterpret_cast<struct sockaddr*>(&addr), sizeof(addr)) < 0) {
+            ::close(fd);
+            return fail("bind(" + ifname + ")");
+        }
+
+        const int flags = ::fcntl(fd, F_GETFL, 0);
+        if (::fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
+            ::close(fd);
+            return fail("fcntl(O_NONBLOCK)");
+        }
+
+        fd_ = fd;
+        name_ = ifname;
+        return true;
+    }
+
+    void Close() {
+        if (fd_ >= 0) {
+            ::close(fd_);
+            fd_ = -1;
+        }
+    }
+
+    bool is_open() const { return fd_ >= 0; }
+    const std::string& name() const { return name_; }
+
+    /// 发送一帧。fd=true 时走 CANFD（并置 BRS，对应 5 Mbps 数据段）
+    bool Send(uint32_t id, const uint8_t* data, uint8_t len, bool fd, std::string* err = nullptr) {
+        if (fd_ < 0) {
+            if (err) *err = "bus not open";
+            return false;
+        }
+        if (len > 64) {
+            if (err) *err = "payload > 64 bytes";
+            return false;
+        }
+
+        if (fd) {
+            struct canfd_frame raw {};
+            raw.can_id = id & CAN_SFF_MASK;
+            raw.len = len;
+            raw.flags = CANFD_BRS;
+            std::memcpy(raw.data, data, len);
+            if (::write(fd_, &raw, sizeof(raw)) != static_cast<ssize_t>(sizeof(raw))) {
+                if (err) *err = std::string("write: ") + std::strerror(errno);
+                return false;
+            }
+        } else {
+            struct can_frame raw {};
+            raw.can_id = id & CAN_SFF_MASK;
+            raw.can_dlc = len;
+            std::memcpy(raw.data, data, len);
+            if (::write(fd_, &raw, sizeof(raw)) != static_cast<ssize_t>(sizeof(raw))) {
+                if (err) *err = std::string("write: ") + std::strerror(errno);
+                return false;
+            }
+        }
+        return true;
+    }
+
+    /// 非阻塞读，最多取 max_frames 帧；返回实际帧数（0 = 当前没数据）
+    size_t Recv(std::vector<CanFrame>* out, size_t max_frames = 128) {
+        if (fd_ < 0 || !out) return 0;
+        size_t count = 0;
+        while (count < max_frames) {
+            struct canfd_frame raw {};
+            const ssize_t got = ::recv(fd_, &raw, CANFD_MTU, MSG_DONTWAIT);
+            if (got < 0) {
+                if (errno == EINTR) continue;
+                break;  // EAGAIN/EWOULDBLOCK：这一轮读空了
+            }
+            if (got != CAN_MTU && got != CANFD_MTU) continue;
+
+            CanFrame frame;
+            frame.fd = (got == CANFD_MTU);
+            frame.id = (raw.can_id & CAN_EFF_FLAG) ? (raw.can_id & CAN_EFF_MASK) : (raw.can_id & CAN_SFF_MASK);
+            frame.len = static_cast<uint8_t>(raw.len > 64 ? 64 : raw.len);
+            std::memcpy(frame.data, raw.data, frame.len);
+            out->push_back(frame);
+            ++count;
+        }
+        return count;
+    }
+
+private:
+    int fd_ = -1;
+    std::string name_;
+};
+
+}  // namespace invicta_hardware
diff --git a/src/invicta_hardware/include/invicta_hardware/hardware_bridge.h b/src/invicta_hardware/include/invicta_hardware/hardware_bridge.h
index ff6cca6..a5d8509 100644
--- a/src/invicta_hardware/include/invicta_hardware/hardware_bridge.h
+++ b/src/invicta_hardware/include/invicta_hardware/hardware_bridge.h
@@ -1,19 +1,21 @@
 /**
  * @file hardware_bridge.h
- * @brief 关节/IMU 数据源与关节指令执行端（骨架，硬件 I/O 待实现）
+ * @brief 关节/IMU 数据源与关节指令执行端（SocketCAN + 智身模组 CANFD 协议）
  *
- * 这个节点是「关节状态发布 + 关节命令接收」合一的那一个：
- *   - 以 publish_rate_hz（默认 200 Hz）向 /joint_states 与 /imu/data 发布状态
- *   - 订阅 /joint_commands，缓存最新一帧，交给 writeMotors() 下发
+ * 循环（publish_rate_hz，默认 200 Hz）：
+ *   1. ReadHardware()  —— 非阻塞收各路总线上的 12 字节反馈帧，更新关节状态/温度/状态码
+ *   2. PublishState()  —— 发 /joint_states 与 /imu/data
+ *   3. WriteHardware() —— 按控制字发使能/失能，使能后每周期发 MIT 运行帧
+ * 另按 diagnostics_rate_hz（默认 10 Hz）发 /joint_diagnostics。
  *
- * 约定（重要，见 README）：
- *   硬件约定（关节方向 dir、零点 offset、单位、CAN 帧格式）全部由本节点负责，
- *   话题上流动的必须是 policy 约定下的量：关节角 [rad]、角速度 [rad/s]、力矩 [Nm]、
- *   IMU 姿态为四元数且角度为弧度。这样 invicta_deploy 侧的 calibration 参数保持恒等。
+ * 反馈不需要主动查询：模组对每条指令都回一帧状态（手册 §3.2），所以本节点
+ * 「发了指令就有反馈」，读回与写入天然同频。
  *
- * 当前 ReadHardware()/WriteHardware() 是空的 TODO，节点只会：
- *   - 发布全零的关节状态与单位四元数姿态（说明数据源还没接上）
- *   - 缓存指令并打印控制字变化，不会真的驱动电机
+ * 与 invicta_deploy 的分工（重要）：
+ *   本节点在话题上收发的都是**原始电机量**（电机坐标系、电机单位），
+ *   dir / offset 由 invicta_deploy 的 InvictaInterface 施加（它已经这么做了）。
+ *   如果改成由本节点施加，必须同时把 deploy 的 [legs].dir/offset 置为恒等，
+ *   否则 dir=±1 会二次相乘抵消、offset_deg 会翻倍。
  */
 #pragma once
 
@@ -29,7 +31,10 @@
 #include <thread>
 #include <vector>
 
+#include "invicta_hardware/can_bus.h"
+#include "invicta_hardware/joint_module_protocol.h"
 #include "invicta_msgs/JointCommandArray.h"
+#include "invicta_msgs/JointDiagnostics.h"
 
 namespace invicta_hardware {
 
@@ -37,67 +42,91 @@ class HardwareBridge {
 public:
     explicit HardwareBridge(const ros::NodeHandle& pnh);
 
-    /// 主循环：以 publish_rate_hz 周期跑「读硬件 → 发状态 → 下发指令」，阻塞
+    /// CAN 总线是否全部打开成功（false 时 main 直接退出）
+    bool Ready() const { return ready_; }
+
+    /// 主循环，阻塞
     void Run();
 
     void Stop();
 
 private:
+    /// 一路总线：通道名 + 帧长 + CANID → 关节下标
+    struct BusRuntime {
+        std::string name;
+        CanBus can;
+        int max_canid = 0;
+        int frame_len = 0;
+        std::vector<int> canid_to_joint;  ///< 下标即 CANID，-1 表示该 ID 未使用
+    };
+
     void LoadParams(const ros::NodeHandle& pnh);
+    bool OpenBuses();
 
     void CommandCallback(const invicta_msgs::JointCommandArray::ConstPtr& msg);
 
-    /// 发布一帧关节状态与 IMU
     void PublishState();
+    void PublishDiagnostics();
 
-    /// 控制字变化时的处理（使能/失能/清错），后续在 WriteHardware 中落实
+    /// 控制字变化：使能/失能按语义落成 CAN 帧
     void HandleControlWord(uint16_t word);
 
-    /**
-     * @brief 从电机读回状态
-     *
-     * TODO(硬件)：接 SocketCAN 读回各关节（关节表见 invicta.toml）的
-     *   position[rad] / velocity[rad/s] / effort[Nm]
-     * 并做 raw → policy 约定的换算（方向、零点、单位）。
-     * 同时读回 IMU：姿态四元数（弧度）、角速度[rad/s]（机体系）、线加速度[m/s²]（机体系）。
-     * 注意 M20 原车 /IMU_DATA 的 rpy 是「度」，这里约定统一用弧度，不要照搬。
-     * 读失败时返回 false，本帧沿用上一次的值（不要发 NaN）。
-     */
-    bool ReadHardware();
+    /// 向所有总线各发若干帧「9×0xFF + code」指令块
+    void SendCodeAll(uint8_t code, int frames = 1, double interval_s = 0.0);
 
-    /**
-     * @brief 把最新指令下发给电机
-     *
-     * TODO(硬件)：把 policy 约定的量换算成 raw（零点、方向、单位），
-     * 按 invicta_msgs/control_word.h 的语义设置使能状态，组装 CAN 帧写下去。
-     * torque = kp*(q_des - q) + kd*(dq_des - dq) + tau_ff 由驱动器或本节点计算，
-     * 取决于关节模组协议，二选一并写清楚。
-     */
+    /// 指令超时：kp=0、速度 0、只留阻尼，防止上游挂掉后一直重发最后一帧
+    bool SendTimeoutFrame();
+
+    bool ReadHardware();
     bool WriteHardware();
 
     // ---- 参数 ----
     std::string joint_state_topic_ = "joint_states";
     std::string imu_topic_ = "imu/data";
     std::string command_topic_ = "joint_commands";
+    std::string diagnostics_topic_ = "joint_diagnostics";
     std::string imu_frame_id_ = "base_link";
     std::vector<std::string> joint_names_;
     double publish_rate_hz_ = 200.0;
+    double diagnostics_rate_hz_ = 10.0;
     int command_queue_ = 1;
+    /// true：CAN 打不开就退出（真机默认）；false：无 CAN 也启动，只发状态（调试/冒烟测试）
+    bool require_can_ = true;
+
+    MotorRange joint_range_, wheel_range_;
+
+    // ---- 指令看门狗 ----
+    double command_timeout_s_ = 0.1;
+    double timeout_joint_kd_ = 4.0;
+    double timeout_wheel_kd_ = 1.0;
+    /// 最近一次收到指令的时刻（steady_clock 纳秒）；回调线程写、循环线程读，用原子量避免数据竞争
+    std::atomic<int64_t> last_cmd_ns_{0};
+    bool timeout_active_ = false;
 
     // ---- ROS ----
     ros::NodeHandle nh_;
     ros::Publisher joint_state_pub_;
     ros::Publisher imu_pub_;
+    ros::Publisher diagnostics_pub_;
     ros::Subscriber command_sub_;
 
     // ---- 指令缓存 ----
     std::mutex cmd_mutex_;
     invicta_msgs::JointCommandArray latest_cmd_;
     bool has_cmd_ = false;
-    uint16_t active_control_word_ = 0;
+    uint16_t applied_control_word_ = 0;  ///< 已在循环线程里落地的控制字
+    std::atomic<bool> enabled_{false};  ///< 是否已使能（未使能不发运行帧）
 
-    // ---- 状态缓存（policy 约定）----
+    // ---- CAN ----
+    std::mutex can_mutex_;  ///< 使能序列（回调线程）与运行帧（循环线程）互斥
+    std::vector<BusRuntime> buses_;
+    bool ready_ = false;
+
+    // ---- 状态缓存（原始电机量）----
     std::vector<double> joint_position_, joint_velocity_, joint_effort_;
+    std::vector<double> temperature_, voltage_;
+    std::vector<uint8_t> driver_status_, last_status_;
+    std::vector<uint8_t> has_feedback_;  ///< 该关节是否收到过反馈（诊断只报有数据的）
     double imu_orientation_[4] = {1.0, 0.0, 0.0, 0.0};  ///< w,x,y,z
     double imu_angular_velocity_[3] = {0.0, 0.0, 0.0};
     double imu_linear_acceleration_[3] = {0.0, 0.0, 0.0};
diff --git a/src/invicta_hardware/include/invicta_hardware/hardware_config.h b/src/invicta_hardware/include/invicta_hardware/hardware_config.h
index c36a55d..102f7aa 100644
--- a/src/invicta_hardware/include/invicta_hardware/hardware_config.h
+++ b/src/invicta_hardware/include/invicta_hardware/hardware_config.h
@@ -10,6 +10,7 @@
 #include <toml++/toml.hpp>
 
 #include <algorithm>
+#include <cmath>
 #include <string>
 #include <vector>
 
@@ -22,6 +23,7 @@ struct JointSpec {
     int leg = -1;
     double lower = 0., upper = 0.;  ///< 位置限位 [rad]（下发前自检用）
     double vel_limit = 0.;          ///< 速度限位 [rad/s]
+    double torque_limit = 0.;       ///< 前馈力矩限幅 [N·m]（0 = 不额外限制）
     double dir = 1.;                ///< 硬件方向 ±1
     double offset_deg = 0.;         ///< 硬件零点偏置 [deg]
 };
@@ -68,9 +70,17 @@ public:
     // ---- 本节点话题与频率 ----
     std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
     std::string topic_joint_commands = "joint_commands";
+    std::string topic_joint_diagnostics = "joint_diagnostics";
     double publish_rate_hz = 200.0;
+    double diagnostics_rate_hz = 10.0;  ///< 温度/状态/电压发布频率（低频诊断，不进控制路径）
     std::string imu_frame_id = "base_link";
     int command_queue = 1;
+    bool require_can = true;  ///< false = 无 CAN 也启动（调试/冒烟测试）
+
+    // ---- 指令看门狗 ----
+    double command_timeout_s = 0.1;    ///< 超过这么久没收到 /joint_commands 就转阻尼；0 = 关闭
+    double timeout_joint_kd = 4.0;     ///< 超时后的摆臂阻尼
+    double timeout_wheel_kd = 1.0;     ///< 超时后的轮阻尼
 
     bool Load(const std::string& path, std::string* error = nullptr);
 
@@ -140,13 +150,15 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
     if (joint_names.empty()) return fail("[robot].joint_names 为空");
     dof = int(joint_names.size());
 
-    std::vector<double> lower, upper, vel_limit;
+    std::vector<double> lower, upper, vel_limit, torque_limit;
     const toml::table* limits = detail::Section(tbl, "joint_limits");
     detail::GetDoubleArr(limits, "lower", lower);
     detail::GetDoubleArr(limits, "upper", upper);
     detail::GetDoubleArr(limits, "vel_limit", vel_limit);
-    if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
-        return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
+    detail::GetDoubleArr(limits, "torque_limit", torque_limit);
+    if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof ||
+        int(torque_limit.size()) != dof) {
+        return fail("[joint_limits] 的 lower/upper/vel_limit/torque_limit 长度必须等于关节数量");
     }
     joints.assign(size_t(dof), JointSpec{});
     for (int i = 0; i < dof; ++i) {
@@ -154,6 +166,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
         joints[size_t(i)].lower = lower[size_t(i)];
         joints[size_t(i)].upper = upper[size_t(i)];
         joints[size_t(i)].vel_limit = vel_limit[size_t(i)];
+        joints[size_t(i)].torque_limit = torque_limit[size_t(i)];
     }
 
     legs.clear();
@@ -215,18 +228,46 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
         detail::Get(mg, "bench_joint_kp", bench_joint_kp);
         detail::Get(mg, "bench_joint_kd", bench_joint_kd);
         detail::Get(mg, "bench_wheel_kd", bench_wheel_kd);
+        detail::Get(mg, "timeout_joint_kd", timeout_joint_kd);
+        detail::Get(mg, "timeout_wheel_kd", timeout_wheel_kd);
+    }
+
+    // 校验人为限幅落在「归一化边界」内。
+    // [motor.*] 的 p/v/t 只决定 0~65535 的映射关系，不是限幅；用户限幅超出它说明配置写错
+    // （或模组阈值被改过），在加载期直接失败，不靠编码饱和兜着。
+    // 轮关节没有位置控制，lower/upper 是占位值，跳过位置校验。
+    for (int i = 0; i < dof; ++i) {
+        const JointSpec& j = joints[size_t(i)];
+        const MotorSpec& m = j.is_wheel ? motor_wheel : motor_joint;
+        if (!j.is_wheel && (j.lower < m.p_min || j.upper > m.p_max)) {
+            return fail("[joint_limits] " + j.name + " 的位置限幅超出归一化边界 [p_min, p_max]");
+        }
+        if (j.vel_limit > std::min(std::fabs(m.v_max), std::fabs(m.v_min))) {
+            return fail("[joint_limits] " + j.name + " 的速度限幅超出归一化边界 [v_min, v_max]");
+        }
+        if (j.torque_limit > std::min(std::fabs(m.t_max), std::fabs(m.t_min))) {
+            return fail("[joint_limits] " + j.name + " 的力矩限幅超出归一化边界 [t_min, t_max]");
+        }
     }
 
     const toml::table* topics = detail::Section(tbl, "topics");
     detail::GetStr(topics, "joint_states", topic_joint_states);
     detail::GetStr(topics, "imu", topic_imu);
     detail::GetStr(topics, "joint_commands", topic_joint_commands);
+    detail::GetStr(topics, "joint_diagnostics", topic_joint_diagnostics);
 
     const toml::table* hw = detail::Section(tbl, "hardware");
     detail::Get(hw, "publish_rate_hz", publish_rate_hz);
+    detail::Get(hw, "diagnostics_rate_hz", diagnostics_rate_hz);
     detail::GetStr(hw, "imu_frame_id", imu_frame_id);
     detail::Get(hw, "command_queue", command_queue);
+    detail::Get(hw, "command_timeout_s", command_timeout_s);
+    if (hw) {
+        if (auto v = (*hw)["require_can"].value<bool>()) require_can = *v;
+    }
     if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz 必须 > 0");
+    if (diagnostics_rate_hz <= 0.) return fail("[hardware].diagnostics_rate_hz 必须 > 0");
+    if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s 不能为负");
 
     if (error) error->clear();
     return true;
diff --git a/src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h b/src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h
new file mode 100644
index 0000000..1520eaf
--- /dev/null
+++ b/src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h
@@ -0,0 +1,127 @@
+/**
+ * @file joint_module_protocol.h
+ * @brief 智身关节模组 CANFD 协议的编解码（docs/zhishen_joint_module_canfd.md）
+ *
+ * 下发帧：64 字节以内，Byte0-1 是 MSGID，之后每个 CANID 占 10 字节，
+ *         块起点 = 2 + 10 × (CANID − 1)，多字节小端。
+ * 反馈帧：每个关节独立返回 12 字节（位置/速度/扭矩/温度/状态/电压）。
+ *
+ * ⚠ 编码与解码都用「归一化边界」（手册 §7 的 P/V/T/KP/KD）；它只决定 0~65535 映射到多少物理量，
+ *   不是运行限幅。限幅在 [joint_limits] 里先夹紧，再送进 Encode()（见 invicta_hardware/README.md）。
+ */
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <cstring>
+
+namespace invicta_hardware {
+
+/// 归一化边界：决定 0~65535 与物理量的映射，不是限幅
+struct MotorRange {
+    double p_min = 0., p_max = 0.;
+    double v_min = 0., v_max = 0.;
+    double t_min = 0., t_max = 0.;
+    double kp_max = 0., kd_max = 0.;
+};
+
+constexpr int kBlockBytes = 10;         ///< 每个关节在下发帧里占 10 字节
+constexpr int kMaxMotorsPerFrame = 6;   ///< 一帧最多 6 个关节
+constexpr uint16_t kMsgId = 0x0100;     ///< 主机 MSGID，从机原样回传
+
+constexpr uint8_t kCodeEnable = 0xFC;   ///< 使能（9 字节 0xFF + 本码）
+constexpr uint8_t kCodeDisable = 0xFD;  ///< 失能，MOS 管全关
+constexpr uint8_t kCodeZero = 0xEF;     ///< 把当前位置设为机械零位
+
+/// 物理量 → uint16（0~65535 线性映射到 MIN~MAX，手册 §5）
+inline uint16_t Encode(double x, double lo, double hi) {
+    if (hi <= lo) return 0;
+    if (!std::isfinite(x)) return uint16_t(Encode(0.0, lo, hi));
+    const double r = std::round((x - lo) / (hi - lo) * 65535.0);
+    return static_cast<uint16_t>(std::min(65535.0, std::max(0.0, r)));
+}
+
+/// uint16 → 物理量（必须与 Encode 用同一套量程）
+inline double Decode(uint16_t raw, double lo, double hi) { return lo + double(raw) / 65535.0 * (hi - lo); }
+
+inline uint16_t ReadU16(const uint8_t* p) { return uint16_t(p[0] | (uint16_t(p[1]) << 8)); }
+inline void WriteU16(uint8_t* p, uint16_t v) {
+    p[0] = uint8_t(v & 0xFF);
+    p[1] = uint8_t(v >> 8);
+}
+
+/// 运行块：位置 / 速度 / Kp / Kd / 前馈力矩，各 2 字节小端
+inline void PackRunBlock(uint8_t* p, double pos, double vel, double kp, double kd, double tau,
+                         const MotorRange& r) {
+    WriteU16(p + 0, Encode(pos, r.p_min, r.p_max));
+    WriteU16(p + 2, Encode(vel, r.v_min, r.v_max));
+    WriteU16(p + 4, Encode(kp, 0.0, r.kp_max));
+    WriteU16(p + 6, Encode(kd, 0.0, r.kd_max));
+    WriteU16(p + 8, Encode(tau, r.t_min, r.t_max));
+}
+
+/// 使能/失能/置零块：前 9 字节固定 0xFF，最后 1 字节是指令码
+inline void PackCodeBlock(uint8_t* p, uint8_t code) {
+    std::memset(p, 0xFF, 9);
+    p[9] = code;
+}
+
+/// 12 字节状态帧（手册 §3.2）
+struct MotorFeedback {
+    double position = 0.;     ///< [rad]
+    double velocity = 0.;     ///< [rad/s]
+    double torque = 0.;       ///< [N·m]
+    double temperature = 0.;  ///< [℃]（raw − 40）
+    double voltage = 0.;      ///< [V]（raw 直接是伏）
+    uint8_t status = 0;       ///< bit0 使能，bit1..6 故障
+
+    bool enabled() const { return (status & 0x01) != 0; }
+    bool fault() const { return ((status >> 1) & 0x3F) != 0; }
+};
+
+/// 状态码非零故障位的中文名，用于日志
+inline const char* FaultName(uint8_t status) {
+    switch ((status >> 1) & 0x3F) {
+        case 0x00: return "无";
+        case 0x01: return "过压";
+        case 0x02: return "过流";
+        case 0x04: return "过温";
+        case 0x08: return "过速";
+        case 0x10: return "编码器";
+        case 0x20: return "欠压/预驱";
+        default: return "组合故障";
+    }
+}
+
+/**
+ * @brief 解一帧 12 字节反馈
+ * @param msgid 可选输出：帧内 MSGID（用于和请求配对）
+ */
+inline bool DecodeFeedback(const uint8_t* d, size_t len, const MotorRange& r, MotorFeedback* out,
+                           uint16_t* msgid = nullptr) {
+    if (!d || !out || len < 12) return false;
+    if (msgid) *msgid = ReadU16(d);
+    out->position = Decode(ReadU16(d + 2), r.p_min, r.p_max);
+    out->velocity = Decode(ReadU16(d + 4), r.v_min, r.v_max);
+    out->torque = Decode(ReadU16(d + 6), r.t_min, r.t_max);
+    out->temperature = double(d[8]) - 40.0;
+    out->status = d[9];
+    out->voltage = double(d[10]);
+    return true;
+}
+
+/// 下发长度（手册 §3.1 的裁剪表：按本路最大 CANID 取最小字节数）
+inline int FrameLenForMaxCanId(int max_canid) {
+    switch (max_canid) {
+        case 0: return 0;
+        case 1: return 12;
+        case 2: return 24;
+        case 3: return 32;
+        case 4: return 48;
+        default: return 64;
+    }
+}
+
+}  // namespace invicta_hardware
diff --git a/src/invicta_hardware/launch/invicta_hardware.launch b/src/invicta_hardware/launch/invicta_hardware.launch
index aac879b..b88bf84 100644
--- a/src/invicta_hardware/launch/invicta_hardware.launch
+++ b/src/invicta_hardware/launch/invicta_hardware.launch
@@ -1,6 +1,9 @@
 <launch>
-  <!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数来自本节点自己的 config/invicta_hardware.toml -->
+  <!-- 关节/IMU 数据源 + 关节指令执行端（SocketCAN）。参数来自本节点自己的 config/invicta_hardware.toml -->
+  <!-- require_can:=false 用于无 CAN 的调试/冒烟测试：节点照常发状态，但不下发电机指令 -->
+  <arg name="require_can" default="true" doc="true = CAN 打不开就退出；false = 无 CAN 也启动"/>
   <node pkg="invicta_hardware" type="invicta_hardware_node" name="invicta_hardware" output="screen">
     <param name="config_file" value="$(find invicta_hardware)/config/invicta_hardware.toml"/>
+    <param name="require_can" value="$(arg require_can)"/>
   </node>
 </launch>
diff --git a/src/invicta_hardware/src/invicta_hardware_node.cpp b/src/invicta_hardware/src/invicta_hardware_node.cpp
index d530be9..2e7e138 100644
--- a/src/invicta_hardware/src/invicta_hardware_node.cpp
+++ b/src/invicta_hardware/src/invicta_hardware_node.cpp
@@ -1,43 +1,136 @@
 /**
  * @file invicta_hardware_node.cpp
- * @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口）
+ * @brief 关节/IMU 数据源 + 关节指令执行端（SocketCAN，智身模组 CANFD 协议）
  */
 #include <ros/package.h>
 #include <ros/ros.h>
 
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstring>
+#include <iostream>
 #include <thread>
 
-#include "invicta_hardware/hardware_config.h"
 #include "invicta_hardware/hardware_bridge.h"
+#include "invicta_hardware/hardware_config.h"
 #include "invicta_msgs/control_word.h"
 
 namespace invicta_hardware {
 
+namespace {
+
+MotorRange ToRange(const MotorSpec& m) {
+    MotorRange r;
+    r.p_min = m.p_min;
+    r.p_max = m.p_max;
+    r.v_min = m.v_min;
+    r.v_max = m.v_max;
+    r.t_min = m.t_min;
+    r.t_max = m.t_max;
+    r.kp_max = m.kp_max;
+    r.kd_max = m.kd_max;
+    return r;
+}
+
+}  // namespace
+
 HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
     LoadParams(pnh);
-    joint_position_.assign(joint_names_.size(), 0.0);
-    joint_velocity_.assign(joint_names_.size(), 0.0);
-    joint_effort_.assign(joint_names_.size(), 0.0);
+
+    const auto& cfg = Config();
+    const size_t n = joint_names_.size();
+    joint_position_.assign(n, 0.0);
+    joint_velocity_.assign(n, 0.0);
+    joint_effort_.assign(n, 0.0);
+    temperature_.assign(n, 0.0);
+    voltage_.assign(n, 0.0);
+    driver_status_.assign(n, 0);
+    last_status_.assign(n, 0);
+    has_feedback_.assign(n, 0);
+
+    joint_range_ = ToRange(cfg.motor_joint);
+    wheel_range_ = ToRange(cfg.motor_wheel);
 
     joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>(joint_state_topic_, 1);
     imu_pub_ = nh_.advertise<sensor_msgs::Imu>(imu_topic_, 1);
+    diagnostics_pub_ = nh_.advertise<invicta_msgs::JointDiagnostics>(diagnostics_topic_, 1);
     command_sub_ = nh_.subscribe(command_topic_, command_queue_, &HardwareBridge::CommandCallback, this);
 
-    ROS_WARN("[invicta_hardware] 这是骨架节点：ReadHardware/WriteHardware 尚未实现，");
-    ROS_WARN("[invicta_hardware] 只会发布零位状态并缓存指令，不会驱动电机。实现见 hardware_bridge.h 的 TODO。");
-    ROS_INFO("[invicta_hardware] %s -> %s/%s，订阅 %s @ %.0f Hz", joint_state_topic_.c_str(), imu_topic_.c_str(),
-             imu_frame_id_.c_str(), command_topic_.c_str(), publish_rate_hz_);
+    ready_ = OpenBuses();
+
+    ROS_INFO("[invicta_hardware] %s -> %s/%s，订阅 %s @ %.0f Hz；诊断 %s @ %.0f Hz", joint_state_topic_.c_str(),
+             imu_topic_.c_str(), imu_frame_id_.c_str(), command_topic_.c_str(), publish_rate_hz_,
+             diagnostics_topic_.c_str(), diagnostics_rate_hz_);
+}
+
+bool HardwareBridge::OpenBuses() {
+    const auto& cfg = Config();
+    buses_.clear();
+
+    if (cfg.can.channels.empty()) {
+        ROS_ERROR("[invicta_hardware] 配置里没有 CAN 通道（[motor.can].channels）");
+        return !require_can_;
+    }
+
+    for (const auto& name : cfg.can.channels) {
+        BusRuntime bus;
+        bus.name = name;
+
+        std::string err;
+        if (!bus.can.Open(name, &err)) {
+            ROS_ERROR("[invicta_hardware] 打开 %s 失败：%s", name.c_str(), err.c_str());
+            ROS_ERROR("[invicta_hardware] 请先配置总线：sudo ip link set %s up type can bitrate %.0f dbitrate %.0f fd on",
+                      name.c_str(), cfg.can.bitrate, cfg.can.data_bitrate);
+            if (require_can_) return false;
+            ROS_WARN("[invicta_hardware] require_can=false，跳过 %s，只发布状态、不下发电机指令", name.c_str());
+            continue;
+        }
+
+        // CANID → 关节下标（同一 CANID 在 can0/can1 上各有一份，互不冲突）
+        for (const auto& leg : cfg.legs) {
+            if (leg.swing_bus == name) bus.max_canid = std::max(bus.max_canid, leg.swing_canid);
+            if (leg.wheel_bus == name) bus.max_canid = std::max(bus.max_canid, leg.wheel_canid);
+        }
+        if (bus.max_canid <= 0 || bus.max_canid > kMaxMotorsPerFrame) {
+            ROS_ERROR("[invicta_hardware] %s 上的 CANID 非法（最大 %d，协议要求 1~%d）", name.c_str(), bus.max_canid,
+                      kMaxMotorsPerFrame);
+            if (require_can_) return false;
+            continue;
+        }
+        bus.canid_to_joint.assign(size_t(bus.max_canid) + 1, -1);
+        for (const auto& leg : cfg.legs) {
+            if (leg.swing_bus == name && leg.swing_canid > 0) bus.canid_to_joint[size_t(leg.swing_canid)] = leg.swing_index;
+            if (leg.wheel_bus == name && leg.wheel_canid > 0) bus.canid_to_joint[size_t(leg.wheel_canid)] = leg.wheel_index;
+        }
+        bus.frame_len = FrameLenForMaxCanId(bus.max_canid);
+
+        ROS_INFO("[invicta_hardware] %s 已打开（CANID 1~%d，下发 %d 字节）", name.c_str(), bus.max_canid, bus.frame_len);
+        buses_.push_back(std::move(bus));
+    }
+
+    if (buses_.empty() && require_can_) return false;
+    return true;
 }
 
 void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
     // 参数来自本节点自己的 config/invicta_hardware.toml
-    const auto& cfg = invicta_hardware::Config();
+    const auto& cfg = Config();
     joint_state_topic_ = cfg.topic_joint_states;
     imu_topic_ = cfg.topic_imu;
     command_topic_ = cfg.topic_joint_commands;
+    diagnostics_topic_ = cfg.topic_joint_diagnostics;
     imu_frame_id_ = cfg.imu_frame_id;
     publish_rate_hz_ = cfg.publish_rate_hz;
+    diagnostics_rate_hz_ = cfg.diagnostics_rate_hz;
     command_queue_ = cfg.command_queue;
+    require_can_ = cfg.require_can;
+    command_timeout_s_ = cfg.command_timeout_s;
+    timeout_joint_kd_ = cfg.timeout_joint_kd;
+    timeout_wheel_kd_ = cfg.timeout_wheel_kd;
+
+    // launch 参数可覆盖配置（冒烟测试/无硬件调试用）
+    pnh.param<bool>("require_can", require_can_, require_can_);
 
     joint_names_ = cfg.joint_names;  // 顺序必须与 invicta_deploy 一致（同一份配置，天然一致）
 
@@ -45,42 +138,279 @@ void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
         ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0，回退到 200 Hz");
         publish_rate_hz_ = 200.0;
     }
+    if (diagnostics_rate_hz_ <= 0.) {
+        ROS_ERROR("[invicta_hardware] diagnostics_rate_hz 必须 > 0，回退到 10 Hz");
+        diagnostics_rate_hz_ = 10.0;
+    }
 }
 
 void HardwareBridge::CommandCallback(const invicta_msgs::JointCommandArray::ConstPtr& msg) {
+    // 只缓存，不做任何阻塞 I/O：ROS1 的 spinner 是单线程的，回调里发使能序列（约 80 ms）
+    // 会把紧随其后的 kEnable/kMotorControl 挤掉。控制字在循环线程的 WriteHardware 里处理。
     std::lock_guard<std::mutex> lock(cmd_mutex_);
-    if (msg->control_word != active_control_word_) {
-        HandleControlWord(msg->control_word);
-        active_control_word_ = msg->control_word;
-    }
     latest_cmd_ = *msg;
     has_cmd_ = true;
+    last_cmd_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                       std::chrono::steady_clock::now().time_since_epoch())
+                       .count();
 }
 
+// ---------------------------------------------------------------- 控制字 → CAN
 void HardwareBridge::HandleControlWord(uint16_t word) {
     switch (word) {
         case invicta_msgs::kDisable:
-            ROS_INFO("[invicta_hardware] control_word -> kDisable(1)：清使能，电机自由");
+            SendCodeAll(kCodeDisable, 1);
+            enabled_ = false;
+            ROS_INFO("[invicta_hardware] control_word -> kDisable：已发失能（0xFD），线圈断开");
             break;
         case invicta_msgs::kEnable:
-            ROS_INFO("[invicta_hardware] control_word -> kEnable(2)：使能并保持当前角");
-            break;
-        case invicta_msgs::kMotorControl:
-            ROS_INFO("[invicta_hardware] control_word -> kMotorControl(3)：进入 PD+前馈控制");
+            SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
+            enabled_ = true;
+            ROS_INFO("[invicta_hardware] control_word -> kEnable：已发使能（0xFC × %d）", Config().can.enable_frames);
             break;
         case invicta_msgs::kErrorReset:
-            ROS_INFO("[invicta_hardware] control_word -> kErrorReset(4)：清驱动错误");
+            // 协议没有独立的清错指令（手册 §6：故障后靠重新使能或重启恢复）
+            SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
+            enabled_ = true;
+            ROS_INFO("[invicta_hardware] control_word -> kErrorReset：本协议无清错指令，按重新使能处理");
+            break;
+        case invicta_msgs::kMotorControl:
+            if (!enabled_) {
+                // 上游的启动时序可能丢（ROS1 发布无 latch，或本节点比上游后启动）。
+                // 协议要求先使能再运行，这里补一次使能，而不是永远不发帧。
+                SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
+                enabled_ = true;
+                ROS_WARN("[invicta_hardware] 收到 kMotorControl 但未使能：已补发使能序列（上游启动时序可能丢失）");
+            }
+            ROS_INFO("[invicta_hardware] control_word -> kMotorControl：进入 kp/kd + 前馈控制");
             break;
         case invicta_msgs::kGetStatus:
-            ROS_INFO("[invicta_hardware] control_word -> kGetStatus(5)：上报驱动状态");
+            // 每条指令都会回 12 字节状态帧，无需专门请求
+            ROS_INFO("[invicta_hardware] control_word -> kGetStatus：状态随每条指令自动返回，无需请求");
             break;
         default:
             ROS_WARN("[invicta_hardware] 未知 control_word=%u，忽略", word);
             break;
     }
-    // TODO(硬件)：把上面这些语义落到电机使能序列上（上电顺序、清错指令、超时保护）。
 }
 
+void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s) {
+    std::lock_guard<std::mutex> lock(can_mutex_);
+    for (int f = 0; f < std::max(1, frames); ++f) {
+        for (auto& bus : buses_) {
+            if (!bus.can.is_open()) continue;
+            uint8_t payload[64] = {};
+            WriteU16(payload, kMsgId);
+            bool any = false;
+            for (int canid = 1; canid <= bus.max_canid; ++canid) {
+                if (bus.canid_to_joint[size_t(canid)] < 0) continue;
+                PackCodeBlock(payload + 2 + kBlockBytes * (canid - 1), code);
+                any = true;
+            }
+            if (!any) continue;
+            std::string err;
+            if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 发送控制码 0x%02X 失败: %s", bus.name.c_str(), code,
+                                  err.c_str());
+            }
+        }
+        if (f + 1 < std::max(1, frames) && interval_s > 0.) {
+            std::this_thread::sleep_for(std::chrono::duration<double>(interval_s));
+        }
+    }
+}
+
+// ---------------------------------------------------------------- 看门狗
+bool HardwareBridge::SendTimeoutFrame() {
+    std::lock_guard<std::mutex> lock(can_mutex_);
+    for (auto& bus : buses_) {
+        if (!bus.can.is_open()) continue;
+
+        uint8_t payload[64] = {};
+        WriteU16(payload, kMsgId);
+        bool any = false;
+        for (int canid = 1; canid <= bus.max_canid; ++canid) {
+            const int joint = bus.canid_to_joint[size_t(canid)];
+            if (joint < 0) continue;
+            const bool is_wheel = Config().IsWheel(joint);
+            const MotorRange& range = is_wheel ? wheel_range_ : joint_range_;
+            const double kd = is_wheel ? timeout_wheel_kd_ : timeout_joint_kd_;
+            // kp=0、速度与力矩都给 0：只留阻尼抵抗外力，不保持位置
+            PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), 0.0, 0.0, 0.0, kd, 0.0, range);
+            any = true;
+        }
+        if (!any) continue;
+
+        std::string err;
+        if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
+            ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 发送阻尼帧失败: %s", bus.name.c_str(), err.c_str());
+        }
+    }
+    return true;
+}
+
+// ---------------------------------------------------------------- 读回
+bool HardwareBridge::ReadHardware() {
+    bool updated = false;
+    std::vector<CanFrame> frames;
+
+    for (auto& bus : buses_) {
+        if (!bus.can.is_open()) continue;
+        frames.clear();
+        bus.can.Recv(&frames, 256);
+        for (const auto& frame : frames) {
+            const int canid = int(frame.id);
+            const int joint = (canid > 0 && canid <= bus.max_canid) ? bus.canid_to_joint[size_t(canid)] : -1;
+            if (joint < 0) {
+                ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s 收到未知帧 id=0x%X len=%u，忽略", bus.name.c_str(),
+                                  unsigned(frame.id), unsigned(frame.len));
+                continue;
+            }
+            if (frame.len < 12) {
+                ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s CANID=%d 反馈只有 %u 字节（协议要求 12）", bus.name.c_str(),
+                                  canid, unsigned(frame.len));
+                continue;
+            }
+
+            MotorFeedback fb;
+            if (!DecodeFeedback(frame.data, frame.len, Config().IsWheel(joint) ? wheel_range_ : joint_range_, &fb)) {
+                continue;
+            }
+
+            joint_position_[size_t(joint)] = fb.position;
+            joint_velocity_[size_t(joint)] = fb.velocity;
+            joint_effort_[size_t(joint)] = fb.torque;
+            temperature_[size_t(joint)] = fb.temperature;
+            voltage_[size_t(joint)] = fb.voltage;
+            driver_status_[size_t(joint)] = fb.status;
+            has_feedback_[size_t(joint)] = 1;
+
+            if (fb.fault() && last_status_[size_t(joint)] != fb.status) {
+                ROS_ERROR("[invicta_hardware] %s 故障：%s（status=0x%02X，温度 %.0f ℃）",
+                          joint_names_[size_t(joint)].c_str(), FaultName(fb.status), unsigned(fb.status), fb.temperature);
+            }
+            last_status_[size_t(joint)] = fb.status;
+            updated = true;
+        }
+    }
+    return updated;
+}
+
+// ---------------------------------------------------------------- 写
+bool HardwareBridge::WriteHardware() {
+    invicta_msgs::JointCommandArray cmd;
+    {
+        std::lock_guard<std::mutex> lock(cmd_mutex_);
+        if (!has_cmd_) {
+            if (!warned_no_command_) {
+                warned_no_command_ = true;
+                ROS_WARN("[invicta_hardware] 还没收到 %s，不下发任何指令", command_topic_.c_str());
+            }
+            return false;
+        }
+        cmd = latest_cmd_;
+    }
+
+    // 控制字变化在这里处理（循环线程），不阻塞订阅回调
+    if (cmd.control_word != applied_control_word_) {
+        HandleControlWord(cmd.control_word);
+        applied_control_word_ = cmd.control_word;
+    }
+
+    const size_t n = joint_names_.size();
+    if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n ||
+        cmd.kd.size() != n) {
+        ROS_WARN_THROTTLE(5.0, "[invicta_hardware] 指令数组长度与 %zu 个关节不匹配，丢弃该帧", n);
+        return false;
+    }
+    for (size_t i = 0; i < n; ++i) {
+        if (!std::isfinite(cmd.position[i]) || !std::isfinite(cmd.velocity[i]) || !std::isfinite(cmd.effort[i]) ||
+            !std::isfinite(cmd.kp[i]) || !std::isfinite(cmd.kd[i])) {
+            ROS_WARN_THROTTLE(1.0, "[invicta_hardware] 指令含非有限值，丢弃该帧");
+            return false;
+        }
+    }
+
+    // 协议要求先使能再发运行帧（手册 §9.1）
+    if (!enabled_) return false;
+
+    // 指令看门狗：上游（deploy）挂掉/卡住时不能一直重发最后一帧
+    if (command_timeout_s_ > 0.) {
+        const int64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                                   std::chrono::steady_clock::now().time_since_epoch())
+                                   .count();
+        const double age = double(now_ns - last_cmd_ns_.load()) / 1e9;
+        if (age > command_timeout_s_) {
+            if (!timeout_active_) {
+                timeout_active_ = true;
+                ROS_ERROR("[invicta_hardware] %.0f ms 未收到 %s，转阻尼（kp=0, kd=%.1f/%.1f）",
+                          age * 1000.0, command_topic_.c_str(), timeout_joint_kd_, timeout_wheel_kd_);
+            }
+            return SendTimeoutFrame();
+        }
+        if (timeout_active_) {
+            timeout_active_ = false;
+            ROS_WARN("[invicta_hardware] 指令恢复，退出阻尼");
+        }
+    }
+
+    std::lock_guard<std::mutex> lock(can_mutex_);
+    for (auto& bus : buses_) {
+        if (!bus.can.is_open()) continue;
+
+        uint8_t payload[64] = {};
+        WriteU16(payload, kMsgId);
+        bool any = false;
+        for (int canid = 1; canid <= bus.max_canid; ++canid) {
+            const int joint = bus.canid_to_joint[size_t(canid)];
+            if (joint < 0) continue;
+
+            const auto& spec = Config().Joint(joint);
+            const MotorRange& range = Config().IsWheel(joint) ? wheel_range_ : joint_range_;
+            const std::string& name = joint_names_[size_t(joint)];
+            const size_t j = size_t(joint);
+
+            // 先按人为限幅夹紧，再交给编码；[motor.*] 只是归一化边界，不参与取最小
+            const double pos = std::min(std::max(cmd.position[j], spec.lower), spec.upper);
+            const double vlim = spec.vel_limit > 0. ? spec.vel_limit : std::abs(range.v_max);
+            const double vel = std::min(std::max(cmd.velocity[j], -vlim), vlim);
+            const double tlim = spec.torque_limit > 0. ? spec.torque_limit : std::abs(range.t_max);
+            double tau = cmd.effort[j];
+            if (std::abs(tau) > tlim) {
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 前馈力矩 %.1f N·m 超过限幅 %.1f，已夹紧", name.c_str(),
+                                  tau, tlim);
+                tau = std::min(std::max(tau, -tlim), tlim);
+            }
+
+            // kp/kd 来自配置（[policy].kp/kd、[leg.gains]），不是策略算出来的：
+            // 越界会被编码器静默饱和，这里显式夹紧并把配置错误报出来
+            double kp = cmd.kp[j];
+            if (kp < 0. || kp > range.kp_max) {
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 的 kp=%.1f 超出模组范围 [0, %.1f]，已夹紧", name.c_str(),
+                                  kp, range.kp_max);
+                kp = std::min(std::max(kp, 0.0), range.kp_max);
+            }
+            double kd = cmd.kd[j];
+            if (kd < 0. || kd > range.kd_max) {
+                ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 的 kd=%.2f 超出模组范围 [0, %.1f]，已夹紧", name.c_str(),
+                                  kd, range.kd_max);
+                kd = std::min(std::max(kd, 0.0), range.kd_max);
+            }
+
+            PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), pos, vel, kp, kd, tau, range);
+            any = true;
+        }
+        if (!any) continue;
+
+        std::string err;
+        if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
+            ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 发送运行帧失败: %s", bus.name.c_str(), err.c_str());
+        }
+    }
+    return true;
+}
+
+// ---------------------------------------------------------------- 发布
 void HardwareBridge::PublishState() {
     sensor_msgs::JointState js;
     js.header.stamp = ros::Time::now();
@@ -110,50 +440,40 @@ void HardwareBridge::PublishState() {
     imu_pub_.publish(imu);
 }
 
-bool HardwareBridge::ReadHardware() {
-    // TODO(硬件)：SocketCAN 读回关节与 IMU，换算到 policy 约定后写入
-    //   joint_position_ / joint_velocity_ / joint_effort_ / imu_*
-    // 读不到就 return false，保留上一帧值。
-    return false;
-}
-
-bool HardwareBridge::WriteHardware() {
-    invicta_msgs::JointCommandArray cmd;
-    {
-        std::lock_guard<std::mutex> lock(cmd_mutex_);
-        if (!has_cmd_) {
-            if (!warned_no_command_) {
-                warned_no_command_ = true;
-                ROS_WARN("[invicta_hardware] 还没收到 %s，不下发任何指令", command_topic_.c_str());
-            }
-            return false;
-        }
-        cmd = latest_cmd_;
+void HardwareBridge::PublishDiagnostics() {
+    invicta_msgs::JointDiagnostics msg;
+    msg.header.stamp = ros::Time::now();
+    for (size_t i = 0; i < joint_names_.size(); ++i) {
+        if (!has_feedback_[i]) continue;  // 没反馈的关节不报，避免下游把 0 ℃ 当有效值
+        msg.name.push_back(joint_names_[i]);
+        msg.temperature.push_back(temperature_[i]);
+        msg.status.push_back(driver_status_[i]);
+        msg.voltage.push_back(voltage_[i]);
     }
-
-    // 上游 M20 的启停只看控制字，这里也先做长度与有限性检查，避免把坏指令写进电机
-    const size_t n = joint_names_.size();
-    if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n ||
-        cmd.kd.size() != n) {
-        ROS_WARN_THROTTLE(5.0, "[invicta_hardware] 指令数组长度与 %zu 个关节不匹配，丢弃该帧", n);
-        return false;
-    }
-
-    // TODO(硬件)：raw = 反换算(零点, 方向, 单位) 后按控制字下发。
-    return false;
+    if (msg.name.empty()) return;
+    diagnostics_pub_.publish(msg);
 }
 
 void HardwareBridge::Run() {
     running_ = true;
     const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
         std::chrono::duration<double>(1.0 / publish_rate_hz_));
+    const auto diag_period = std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::duration<double>(1.0 / diagnostics_rate_hz_));
     auto next = std::chrono::steady_clock::now();
+    auto next_diag = next;
 
     while (ros::ok() && running_) {
         next += period;
         ReadHardware();
         PublishState();
         WriteHardware();
+
+        const auto now = std::chrono::steady_clock::now();
+        if (now >= next_diag) {
+            next_diag = now + diag_period;
+            PublishDiagnostics();
+        }
         std::this_thread::sleep_until(next);
     }
 }
@@ -178,13 +498,15 @@ int main(int argc, char** argv) {
     std::cout << invicta_hardware::Config().Summary() << std::flush;
 
     invicta_hardware::HardwareBridge bridge(pnh);
-    std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);
+    if (!bridge.Ready()) {
+        ROS_FATAL("[invicta_hardware] CAN 初始化失败，退出（无硬件调试可设 require_can:=false）");
+        return 1;
+    }
 
+    std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);
     ros::spin();
 
     bridge.Stop();
-    if (loop_thread.joinable()) {
-        loop_thread.join();
-    }
+    if (loop_thread.joinable()) loop_thread.join();
     return 0;
 }
diff --git a/src/invicta_msgs/CMakeLists.txt b/src/invicta_msgs/CMakeLists.txt
index 2983654..1eb4489 100644
--- a/src/invicta_msgs/CMakeLists.txt
+++ b/src/invicta_msgs/CMakeLists.txt
@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
   FILES
   JointCommandArray.msg
+  JointDiagnostics.msg
   MotionCommand.msg
   MotionState.msg
 )
diff --git a/src/invicta_msgs/README.md b/src/invicta_msgs/README.md
index 3f39ce2..4f44066 100644
--- a/src/invicta_msgs/README.md
+++ b/src/invicta_msgs/README.md
@@ -9,12 +9,14 @@
 | `/joint_states` | `sensor_msgs/JointState` | **标准** | 关节状态：`name` / `position[rad]` / `velocity[rad/s]` / `effort[Nm]` |
 | `/imu/data` | `sensor_msgs/Imu` | **标准** | IMU：`orientation`（四元数，rad）、`angular_velocity[rad/s]`、`linear_acceleration[m/s²]` |
 | `/joint_commands` | `invicta_msgs/JointCommandArray` | **自定义** | 关节指令：目标角/角速度/前馈力矩/`kp`/`kd`/`control_word` |
+| `/joint_diagnostics` | `invicta_msgs/JointDiagnostics` | **自定义** | 低频诊断：电机温度/驱动状态码/母线电压（默认 10 Hz） |
 | `/motion_command` | `invicta_msgs/MotionCommand` | **自定义** | 协议节点 → RL 节点：运动状态/步态/使用模式/轴指令 |
 | `/motion_state` | `invicta_msgs/MotionState` | **自定义** | RL 节点 → 协议节点：实际运动状态/步态等 |
 
-自定义消息只有 3 条，原因：
+自定义消息只有 4 条，原因：
 
 - `JointCommandArray`：标准消息无法表达 `kp`/`kd`/前馈力矩/控制字（`sensor_msgs/JointState` 只有 position/velocity/effort，`trajectory_msgs/JointTrajectory` 也没有增益）。
+- `JointDiagnostics`：`JointState` 没有温度和状态字字段；诊断量也不该塞进 200 Hz 控制路径，所以单独一条低频话题。
 - `MotionCommand` / `MotionState`：是节点间的指令/状态接口，字段与厂家协议一一对应，不适合塞进 `std_msgs`。
 
 ## 共享值域：`include/invicta_msgs/motion_defs.h`
@@ -36,20 +38,22 @@
 
 | M20 | invicta | 变化 |
 |---|---|---|
-| `/JOINTS_DATA` `drdds/JointsData` | `/joint_states` `sensor_msgs/JointState` | 换标准消息；丢掉 `motion_temp`/`driver_temp`/`status_word`（见下） |
+| `/JOINTS_DATA` `drdds/JointsData` | `/joint_states` `sensor_msgs/JointState` | 换标准消息；`motion_temp`/`driver_temp`/`status_word` 移到 `/joint_diagnostics`（见下） |
 | `/IMU_DATA` `drdds/ImuData` | `/imu/data` `sensor_msgs/Imu` | 换标准消息；`rpy` 由**度**改为四元数 + 弧度 |
 | `/JOINTS_CMD` `drdds/JointsDataCmd` | `/joint_commands` `JointCommandArray` | 控制字重新定义（平台无关） |
 | `/BATTERY_DATA` `drdds/BatteryData` | 无 | RL 层不需要；若做协议状态上报，由 `invicta_hardware` 用标准消息提供 |
 | `drdds/GamepadData` | 无 | 不需要：手柄走 UDP，在 `invicta_remote` 里 |
 | — | `/motion_command` / `/motion_state` | 新增：协议节点与 RL 节点之间的接口 |
 
-## 被删掉的字段与影响
+## 温度与驱动状态字：`/joint_diagnostics`
 
-`sensor_msgs/JointState` 放不下关节温度与驱动器状态字，相对 M20 少了两条保护：
+`sensor_msgs/JointState` 放不下关节温度与驱动器状态字，相对 M20 会少两条保护：
 
-| M20 字段 | 用途 | 现在的行为 |
+| M20 字段 | 用途 | 现在的实现 |
 |---|---|---|
-| `motion_temp` | 电机过温 >100 ℃ 告警 | 温度接口返回常量，检查恒不触发 |
-| `status_word` | 驱动器报错判断（`!=1` 即报错） | 状态字接口返回全 1，检查恒不触发 |
+| `motion_temp` | 电机过温 >100 ℃ 告警 | `JointDiagnostics.temperature`（默认 10 Hz），协议来源是 CAN 反馈帧 Byte8 |
+| `status_word` | 驱动器报错判断（`!=1` 即报错） | `JointDiagnostics.status`（Byte9 原样），invicta_deploy 侧尚未接 |
+| — | 母线电压 | `JointDiagnostics.voltage`（Byte10） |
 
-要恢复就加一条**独立低频诊断话题**（例如 1 Hz 的 `JointDiagnostics`），不要塞回 200 Hz 控制路径。协议里的设备状态上报（`1002/5`）需要的温度/电池数据，也应走 `invicta_hardware` → 协议节点这条线。
+设计上一律走**独立低频诊断话题**，不塞回 200 Hz 控制路径。协议里的设备状态上报（`1002/5`）需要的
+电机温度，也走 `invicta_hardware` → `/joint_diagnostics` → `invicta_remote` 这条线。
diff --git a/src/invicta_msgs/msg/JointDiagnostics.msg b/src/invicta_msgs/msg/JointDiagnostics.msg
new file mode 100644
index 0000000..b6fb784
--- /dev/null
+++ b/src/invicta_msgs/msg/JointDiagnostics.msg
@@ -0,0 +1,19 @@
+# 关节低频诊断量：温度 / 驱动状态码 / 母线电压。
+#
+# 为什么不放进 /joint_states：sensor_msgs/JointState 没有温度和状态字字段，
+# 而 200 Hz 的控制路径也不该承载诊断量。本话题由 invicta_hardware 以
+# [hardware].diagnostics_rate_hz（默认 10 Hz）发布。
+#
+# 字段来源：智身关节模组 CANFD 12 字节反馈帧（docs/zhishen_joint_module_canfd.md §3.2）
+#   Byte8  电机温度：raw 0~255 对应 −40~215 ℃
+#   Byte9  状态码：bit0 使能，bit1~6 故障（过压/过流/过温/过速/编码器/欠压）
+#   Byte10 母线电压：raw 0~255 直接对应 0~255 V
+#
+# 数组顺序与 /joint_states 的 name 一致；没有反馈的关节不出现在本消息里。
+
+std_msgs/Header header
+
+string[]  name          # 关节名，与 /joint_states 顺序一致
+float64[] temperature   # 电机温度 [℃]
+uint8[]   status        # 驱动状态码（按协议原样上报，位定义见文档 §6）
+float64[] voltage       # 母线电压 [V]
diff --git a/src/invicta_remote/README.md b/src/invicta_remote/README.md
index c590150..8da59eb 100644
--- a/src/invicta_remote/README.md
+++ b/src/invicta_remote/README.md
@@ -36,11 +36,24 @@
 | 1101 | 2 | 照明灯 | ❌ `0xE00A`（本平台无灯硬件） |
 | 1101 | 6/7 | 休眠设置/查询 | ❌ `0xE00A` |
 | 1002 | 6 | 基础状态上报 | ✅ 2 Hz（MotionState/Gait/Charge/HES/ControlUsageMode/Sleep/Version） |
-| 1002 | 4 | 运控状态上报 | ✅ 10 Hz（姿态/OmegaZ + 16 关节角/轮速） |
-| 1002 | 5 | 设备状态上报 | ⚠️ 2 Hz，结构完整但电池/温度/CPU/GPS 为占位 0（等硬件数据） |
+| 1002 | 4 | 运控状态上报 | ✅ 10 Hz（姿态/OmegaZ + 16 槽关节，8 个实关节折叠进 M20 布局，见下） |
+| 1002 | 5 | 设备状态上报 | ⚠️ 2 Hz，电机温度来自 `/joint_diagnostics`；电池/CPU/GPS 仍为占位 0 |
 | 1002 | 3 | 异常上报 | ✅ 2 Hz，当前恒为空列表（暂无错误源） |
 | 1003/1004/1007/2101 | — | 导航任务（Pro） | ❌ 未实现 |
 
+### 1002/4 的关节：8 → 16 槽
+
+协议（`proto.md` §3.2）固定 16 个关节，每腿 `HipX / HipY / Knee / Wheel`，腿序 前左、前右、后左、后右。本机每腿只有 swing + wheel 两个自由度，因此：
+
+| M20 槽 | 本机来源 |
+|---|---|
+| `*HipX`（侧摆） | 不存在，恒 **0**（机构无侧摆） |
+| `*HipY`（髋俯仰） | `*_swing` 关节角 [rad] |
+| `*Knee`（膝） | 不存在，恒 **0**（单连杆，腿笔直） |
+| `*Wheel` | `*_wheel` 轮转速 [rad/s] |
+
+`MotorStatus.Joint` 数组与 16 个具名字段用同一套值（非轮为角度、轮为转速）。保留 16 槽而不裁成 8 槽，是为了让同一套 App 不必区分机型；两个恒 0 槽不是缺数据，而是当前机构的正确取值，客户端姿态显示不会异常。映射优先按 `/joint_states` 的关节名解析（`fl_swing`/`fl_wheel` …），无 name 时退回「每腿两个、先 swing 后 wheel」的下标顺序。
+
 未实现项默认回 `0xE00A "Unsupported function"`；若某个 App 把非 0 当致命错误，把参数 `unsupported_as_success` 设为 `true`（只影响这些未实现项，校验类错误码照旧）。
 
 错误码采用厂家定义（`0xE001`～`0xE00B`），见 `include/invicta_remote/protocol_defs.h`。
@@ -50,7 +63,7 @@
 | TOML 段 | 内容 |
 |---|---|
 | `[protocol]` | `udp_port`、`heartbeat_timeout_s`、`unsupported_as_success` |
-| `[topics]` | `motion_command` / `motion_state` / `joint_states` / `imu` 话题名 |
+| `[topics]` | `motion_command` / `motion_state` / `joint_states` / `imu` / `joint_diagnostics` 话题名 |
 | `[telemetry]` | 四路上报的开关与频率（`enabled`、`require_subscription`、`basic_hz`/`motion_hz`/`device_hz`/`error_hz`） |
 
 launch 只传 `config_file`（默认 `$(find invicta_remote)/config/invicta_remote.toml`）。
diff --git a/src/invicta_remote/config/invicta_remote.toml b/src/invicta_remote/config/invicta_remote.toml
index 26a2945..68e3f1c 100644
--- a/src/invicta_remote/config/invicta_remote.toml
+++ b/src/invicta_remote/config/invicta_remote.toml
@@ -17,6 +17,7 @@ motion_command  = "motion_command"   # 发布 invicta_msgs/MotionCommand（invic
 motion_state    = "motion_state"     # 订阅 invicta_msgs/MotionState（判前提 + 状态上报）
 joint_states    = "joint_states"     # 订阅（1002/4 运控状态上报用）
 imu             = "imu/data"         # 订阅（状态上报用）
+joint_diagnostics = "joint_diagnostics"  # 订阅（1002/5 设备状态的电机温度用）
 
 # ------------------------------------------------------------------ 状态上报（协议 1002/*）
 [telemetry]
diff --git a/src/invicta_remote/include/invicta_remote/remote_config.h b/src/invicta_remote/include/invicta_remote/remote_config.h
index 16bcf31..be750d3 100644
--- a/src/invicta_remote/include/invicta_remote/remote_config.h
+++ b/src/invicta_remote/include/invicta_remote/remote_config.h
@@ -24,6 +24,7 @@ public:
     std::string topic_motion_state = "motion_state";
     std::string topic_joint_states = "joint_states";
     std::string topic_imu = "imu/data";
+    std::string topic_joint_diagnostics = "joint_diagnostics";
 
     // ---- 状态上报（1002/*）----
     bool telemetry_enabled = true;
@@ -86,6 +87,7 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
     detail::GetStr(topics, "motion_state", topic_motion_state);
     detail::GetStr(topics, "joint_states", topic_joint_states);
     detail::GetStr(topics, "imu", topic_imu);
+    detail::GetStr(topics, "joint_diagnostics", topic_joint_diagnostics);
 
     const toml::table* tlm = detail::Section(tbl, "telemetry");
     detail::GetBool(tlm, "enabled", telemetry_enabled);
diff --git a/src/invicta_remote/include/invicta_remote/remote_node.h b/src/invicta_remote/include/invicta_remote/remote_node.h
index b5c3085..31685cd 100644
--- a/src/invicta_remote/include/invicta_remote/remote_node.h
+++ b/src/invicta_remote/include/invicta_remote/remote_node.h
@@ -35,6 +35,8 @@
 #include <vector>
 
 #include "heartbeat.h"
+#include "invicta_msgs/JointCommandArray.h"
+#include "invicta_msgs/JointDiagnostics.h"
 #include "invicta_msgs/MotionCommand.h"
 #include "invicta_msgs/MotionState.h"
 #include "remote_config.h"
@@ -56,6 +58,7 @@ public:
         motion_state_topic_ = cfg_.topic_motion_state;
         joint_states_topic_ = cfg_.topic_joint_states;
         imu_topic_ = cfg_.topic_imu;
+        joint_diagnostics_topic_ = cfg_.topic_joint_diagnostics;
         heartbeat_timeout_s_ = cfg_.heartbeat_timeout_s;
         unsupported_as_success_ = cfg_.unsupported_as_success;
 
@@ -71,6 +74,8 @@ public:
         motion_state_sub_ = nh_.subscribe(motion_state_topic_, 10, &RemoteNode::MotionStateCallback, this);
         joint_state_sub_ = nh_.subscribe(joint_states_topic_, 10, &RemoteNode::JointStateCallback, this);
         imu_sub_ = nh_.subscribe(imu_topic_, 10, &RemoteNode::ImuCallback, this);
+        diagnostics_sub_ =
+            nh_.subscribe(joint_diagnostics_topic_, 10, &RemoteNode::JointDiagnosticsCallback, this);
 
         RegisterHandlers();
     }
@@ -211,9 +216,18 @@ private:
 
     void JointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) {
         std::lock_guard<std::mutex> lock(state_mutex_);
+        joint_names_ = msg->name;
         joint_pos_.assign(msg->position.begin(), msg->position.end());
         joint_vel_.assign(msg->velocity.begin(), msg->velocity.end());
-        have_joints_ = joint_pos_.size() == 16;
+        // 本机 8 关节（每腿 swing + wheel），不是 M20 的 16；上报时由 FillM20Joints 折叠到 16 槽
+        have_joints_ = !joint_pos_.empty();
+    }
+
+    void JointDiagnosticsCallback(const invicta_msgs::JointDiagnostics::ConstPtr& msg) {
+        std::lock_guard<std::mutex> lock(state_mutex_);
+        diag_names_ = msg->name;
+        diag_temperature_.assign(msg->temperature.begin(), msg->temperature.end());
+        diag_status_.assign(msg->status.begin(), msg->status.end());
     }
 
     void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
@@ -230,6 +244,77 @@ private:
         have_imu_ = true;
     }
 
+    // ---------------------------------------------------------------- M20 16 关节布局
+    // 协议 §3.2 固定 16 槽：每腿 [HipX, HipY, Knee, Wheel]，腿序 前左/前右/后左/后右。
+    // 本机每腿只有 swing + wheel：HipY ← 摆臂角，Wheel ← 轮转速。
+    // HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，恒为 0——这不是缺数据，而是当前机构
+    // 「无侧摆、腿笔直」的正确取值，客户端按 16 槽解析时姿态不会异常。
+    // 保留 16 槽而不是裁成 8 槽，是为了让同一套 App 不必区分机型。
+    static constexpr int kM20Joints = 16;
+
+    static const char* M20JointName(int slot) {
+        static const char* kNames[kM20Joints] = {
+            "LeftFrontHipX", "LeftFrontHipY", "LeftFrontKnee", "LeftFrontWheel",
+            "RightFrontHipX", "RightFrontHipY", "RightFrontKnee", "RightFrontWheel",
+            "LeftBackHipX", "LeftBackHipY", "LeftBackKnee", "LeftBackWheel",
+            "RightBackHipX", "RightBackHipY", "RightBackKnee", "RightBackWheel"};
+        return kNames[slot];
+    }
+
+    /// 关节名 → 腿序号（0 前左 fl、1 前右 fr、2 后左 hl、3 后右 hr）；不认识返回 -1
+    static int LegIndexFromJointName(const std::string& n) {
+        if (n.compare(0, 2, "fl") == 0) return 0;
+        if (n.compare(0, 2, "fr") == 0) return 1;
+        if (n.compare(0, 2, "hl") == 0) return 2;
+        if (n.compare(0, 2, "hr") == 0) return 3;
+        return -1;
+    }
+
+    /**
+     * @brief 一组关节名 → M20 的 16 槽
+     *
+     * @param names       关节名（/joint_states 或 /joint_diagnostics 的 name）
+     * @param value_count 该消息实际带的数据条数（无 name 时按它做下标回退）
+     * @param slot_index  输出：槽 → names 下标，-1 表示本机没有这个自由度
+     *
+     * HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，永远 -1；swing → HipY，wheel → Wheel。
+     * 优先按 name 解析（fl_swing / fl_wheel …），无 name 时退回「每腿两个、先 swing 后 wheel」。
+     */
+    static void BuildSlotToIndex(const std::vector<std::string>& names, size_t value_count,
+                                 int slot_index[kM20Joints]) {
+        for (int i = 0; i < kM20Joints; ++i) slot_index[i] = -1;
+        const bool by_name = !names.empty() && names.size() == value_count;
+        const size_t n = by_name ? names.size() : std::min(value_count, size_t(kM20Joints / 2));
+        for (size_t k = 0; k < n; ++k) {
+            int leg = -1;
+            bool is_wheel = false;
+            if (by_name) {
+                leg = LegIndexFromJointName(names[k]);
+                if (leg < 0) continue;
+                const bool wheel = names[k].find("wheel") != std::string::npos;
+                const bool swing = names[k].find("swing") != std::string::npos;
+                if (!wheel && !swing) continue;
+                is_wheel = wheel;
+            } else {
+                leg = int(k / 2);
+                is_wheel = (k % 2 == 1);
+            }
+            slot_index[leg * 4 + (is_wheel ? 3 : 1)] = int(k);
+        }
+    }
+
+    /// 16 槽取值：协议里 Joint 数组与具名字段的混合量（角度，轮为 rad/s）
+    void FillM20Joints(double value[kM20Joints]) const {
+        for (int i = 0; i < kM20Joints; ++i) value[i] = 0.0;
+        int slot_index[kM20Joints];
+        BuildSlotToIndex(joint_names_, std::min(joint_pos_.size(), joint_vel_.size()), slot_index);
+        for (int slot = 0; slot < kM20Joints; ++slot) {
+            const int k = slot_index[slot];
+            if (k < 0) continue;
+            value[slot] = (slot % 4 == 3) ? joint_vel_[size_t(k)] : joint_pos_[size_t(k)];
+        }
+    }
+
     // ---------------------------------------------------------------- 状态上报
     json BasicDoc(int type, int command) {
         std::lock_guard<std::mutex> lock(state_mutex_);
@@ -268,19 +353,14 @@ private:
         ms["RemainMile"] = 0.0; // 待电池/里程
 
         auto& motor = items["MotorStatus"];
-        std::vector<double> joint = joint_pos_;
-        std::vector<double> vel = joint_vel_;
-        joint.resize(16, 0.0);
-        vel.resize(16, 0.0);
-        motor["Joint"] = joint;
-        static const char* kNames[16] = {
-            "LeftFrontHipX", "LeftFrontHipY", "LeftFrontKnee", "LeftFrontWheel",
-            "RightFrontHipX", "RightFrontHipY", "RightFrontKnee", "RightFrontWheel",
-            "LeftBackHipX", "LeftBackHipY", "LeftBackKnee", "LeftBackWheel",
-            "RightBackHipX", "RightBackHipY", "RightBackKnee", "RightBackWheel"};
-        for (int i = 0; i < 16; ++i) {
-            motor[kNames[i]] = (i % 4 == 3) ? vel[i] : joint[i];  // 轮关节报转速，其余报角度
+        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 上报",
+                              joint_states_topic_.c_str());
         }
+        motor["Joint"] = std::vector<double>(value, value + kM20Joints);
+        for (int i = 0; i < kM20Joints; ++i) motor[M20JointName(i)] = value[i];
         doc["PatrolDevice"]["Type"] = type;
         doc["PatrolDevice"]["Command"] = command;
         doc["PatrolDevice"]["Time"] = CurrentDateTime();
@@ -288,9 +368,10 @@ private:
     }
 
     json DeviceDoc(int type, int command) {
+        std::lock_guard<std::mutex> lock(state_mutex_);
         json doc;
         json& items = doc["PatrolDevice"]["Items"];
-        // 电池 / 温度 / CPU / GPS 需要硬件桥接提供数据，当前用 0 占位（Version 除外）
+        // 电池 / CPU / GPS 还没有数据源，用 0 占位；电机温度来自 /joint_diagnostics
         json battery = {{"Voltage", 0.0}, {"BatteryLevel", 0.0}, {"battery_temperature", 0.0},
                         {"charge", false}, {"serial", ""}};
         items["BatteryList"] = json::array({battery, battery});
@@ -298,9 +379,24 @@ private:
                                   {"BatteryLevelLeft", 0.0}, {"BatteryLevelRight", 0.0},
                                   {"battery_temperatureLeft", 0.0}, {"battery_temperatureRight", 0.0},
                                   {"chargeLeft", false}, {"chargeRight", false}};
-        json zeros16 = json::array();
-        for (int i = 0; i < 16; ++i) zeros16.push_back(0.0);
-        items["DeviceTemperature"] = {{"Motor", zeros16}, {"Driver", zeros16}};
+        // 温度按 M20 布局展开：本机 8 个电机落在对应槽（swing → HipY、wheel → Wheel），
+        // 其余槽没有对应电机填 0；驱动器温度本协议不返回，恒 0。
+        int slot_index[kM20Joints];
+        BuildSlotToIndex(diag_names_, diag_temperature_.size(), slot_index);
+        json motor_temp = json::array();
+        json driver_temp = json::array();
+        for (int slot = 0; slot < kM20Joints; ++slot) {
+            const int k = slot_index[slot];
+            motor_temp.push_back((k >= 0 && size_t(k) < diag_temperature_.size()) ? diag_temperature_[size_t(k)] : 0.0);
+            driver_temp.push_back(0.0);
+        }
+        items["DeviceTemperature"] = {{"Motor", motor_temp}, {"Driver", driver_temp}};
+        auto& temps = items["DeviceTemperature"];
+        for (int slot = 0; slot < kM20Joints; ++slot) {
+            const std::string base = M20JointName(slot);
+            temps[base + "Motor"] = motor_temp[slot];
+            temps[base + "Driver"] = 0.0;
+        }
         items["Led"] = {{"Fill", {{"Front", 0}, {"Back", 0}}}};
         items["GPS"] = {{"Latitude", 0.0}, {"Longitude", 0.0}, {"Speed", 0.0}, {"Course", 0.0},
                         {"FixQuality", 0}, {"NumSatellites", 0}, {"Altitude", 0.0},
@@ -569,6 +665,7 @@ private:
     ros::Subscriber motion_state_sub_;
     ros::Subscriber joint_state_sub_;
     ros::Subscriber imu_sub_;
+    ros::Subscriber diagnostics_sub_;
 
     std::unique_ptr<Heartbeat> heartbeat_;
     std::unique_ptr<UdpServer> udp_server_;
@@ -582,6 +679,7 @@ private:
     std::string motion_state_topic_ = "motion_state";
     std::string joint_states_topic_ = "joint_states";
     std::string imu_topic_ = "imu/data";
+    std::string joint_diagnostics_topic_ = "joint_diagnostics";
     double heartbeat_timeout_s_ = 3.0;
     bool unsupported_as_success_ = false;
 
@@ -612,8 +710,14 @@ private:
     uint8_t actual_charge_ = 0, actual_hes_ = 0, actual_sleep_ = 0;
     std::string actual_version_ = "INVICTA";
     bool have_state_ = false;
+    std::vector<std::string> joint_names_;
     std::vector<double> joint_pos_, joint_vel_;
     bool have_joints_ = false;
+    /// 启动后 2 s 内不报「还没收到关节状态」：节点刚起来时上游本来就还没发
+    const std::chrono::steady_clock::time_point start_time_ = std::chrono::steady_clock::now();
+    std::vector<std::string> diag_names_;
+    std::vector<double> diag_temperature_;
+    std::vector<uint8_t> diag_status_;
     double rpy_[3] = {0., 0., 0.};
     double omega_z_ = 0.;
     bool have_imu_ = false;