refactor(config) + feat(model): 每节点一份 TOML 取代 ROS yaml；8 关节两自由度腿与单连杆 IK

配置（节点自治，跨包不共享）： - 新增各节点自己的 config/<节点>.toml（deploy / remote / hardware），只放本节点参数；删除三份 *.yaml - 各节点自带配置结构与加载器（deploy_config.h / remote_config.h / hardware_config.h），删除 state_machine/parameters/ 与 ControlParameters - TOML 解析库 toml++ 改为系统安装：find_package(tomlplusplus REQUIRED)，以 header-only 方式编译，库源码不进仓库、目标机不需部署运行时 .so - launch 只传 config_file；改 TOML 不需重编，改配置头自动触发该节点重编机器人模型（按本机构型改）： - 16 关节三关节腿 → 8 关节（每腿摆臂 + 轮） - IK 改为单连杆反解 θ = splay_sign·acos(h/L)；趴下 h=0 → 摆臂水平（±90°） - 步态倒向：趴下时前腿向前、后腿向后（splay_sign 前 +1 / 后 −1），与 M20 相反 - 起立/趴下用样条 + 速度前馈；轮 kp 恒 0；限位改为逐关节查表 - 精简死参数：机身长宽、髋偏置、轮半径、力矩限制、通用策略参数，以及 auto_calibrate 与 ±360° 回绕标定 - 硬件侧参数入库：电机量程（P65MAXS/W190S）、CAN 通道与仲裁 ID、使能帧数/间隔、每腿 bus/canid、台架增益保留（未改动 HEAD 已有功能）：多步态模型预加载与运行中切换、站立后自动进 RL 与就绪自检、遥测四路上报、阻尼态单调时钟计时、策略输出 NaN 保护验证（ROS Noetic 容器，干净构建）： - 四个包 catkin_make rc=0 - /joint_states 200 Hz（8 关节名与配置一致）、/motion_state 20 Hz - IK 目标角：站立 ±16.3°、收腿 ±66.4°、趴下 ±90.0°；趴下指令实测峰值 1.5708 rad - 协议 MotionParam=1/4 触发 idle→standup→liedown；自动进 RL 自检在零反馈下正确拦住待实测：摆臂长度/站趴高度、各关节 dir/offset_deg；invicta_hardware 的 CAN 读写仍为 TODO
2026-10-21 21:39:49 +08:00
parent cf70b1a1d7
commit a173383756
39 changed files with 1715 additions and 1088 deletions
@@ -13,20 +13,36 @@ find_package(catkin REQUIRED COMPONENTS
  sensor_msgs
  geometry_msgs
  invicta_msgs
+  roslib
 )

 catkin_package(
  INCLUDE_DIRS include
-  CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs invicta_msgs
+  CATKIN_DEPENDS roscpp roslib sensor_msgs geometry_msgs invicta_msgs
 )

+# TOML 解析库：系统安装（见 README「依赖」），CMake 里体现为 required
+find_package(tomlplusplus REQUIRED)
+get_target_property(TOMLPLUSPLUS_INCLUDE_DIR tomlplusplus::tomlplusplus INTERFACE_INCLUDE_DIRECTORIES)
+include_directories(${TOMLPLUSPLUS_INCLUDE_DIR})
+# 以 header-only 方式使用：实现编进本节点，避免引入运行时动态库依赖（目标机无需部署 .so）
+add_definitions(-DTOML_HEADER_ONLY=1)
+
 include_directories(
  include
  ${catkin_INCLUDE_DIRS}
 )

+# 把本节点自己的配置头列进源列表：改该头会触发重编（改 TOML 不用重编）
+set(NODE_CONFIG_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/include/invicta_hardware/hardware_config.h")
+if(NOT EXISTS "${NODE_CONFIG_HEADER}")
+  message(WARNING "invicta_hardware_node: 找不到本节点配置头")
+  set(NODE_CONFIG_HEADER "")
+endif()
+
 add_executable(invicta_hardware_node
  src/invicta_hardware_node.cpp
+  ${NODE_CONFIG_HEADER}
 )

 target_link_libraries(invicta_hardware_node ${catkin_LIBRARIES})
@@ -34,18 +34,21 @@
 - [ ] `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` 完全一致（默认 16 个 `fl/fr/hl/hr × hipx/hipy/knee/wheel`）。
+- [ ] 关节顺序与 `invicta_deploy` 共用同一份配置里的 `joint_names`，不会不一致。

-## 参数（私有命名空间，`config/invicta_hardware.yaml`）
+## 参数（本节点自己的 `config/invicta_hardware.toml`）

-| 参数 | 默认 | 说明 |
-|---|---|---|
-| `joint_states_topic` / `imu_topic` / `joint_commands_topic` | `joint_states` / `imu/data` / `joint_commands` | 话题名 |
-| `imu_frame_id` | `base_link` | IMU 坐标系 |
-| `publish_rate_hz` | 200.0 | 状态发布频率（需 ≥ deploy 的 200 Hz 控制周期） |
-| `command_queue` | 1 | 指令只取最新一帧 |
-| `joint_names` | 16 个 | 关节顺序 |
+| 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 |

+启动时打印关节表、每条腿的总线/CANID 与电机量程，便于接线核对。
 ## 运行

 ```bash
@@ -0,0 +1,104 @@
+# =============================================================================
+# invicta_hardware（关节/IMU 数据源 + 指令执行端）自己的配置
+# 只放本节点需要的参数：关节表与硬件标定、话题、发布频率、电机与 CAN
+# 运动学/策略/步态等属于 invicta_deploy，见 invicta_deploy/config/invicta_deploy.toml
+# =============================================================================
+
+[robot]
+# 关节顺序：必须与 /joint_states、/joint_commands 里的 name 对应
+joint_names = [
+  "fl_swing", "fl_wheel",
+  "fr_swing", "fr_wheel",
+  "hl_swing", "hl_wheel",
+  "hr_swing", "hr_wheel",
+]
+
+# ------------------------------------------------------------------ 关节限位
+# 顺序与 joint_names 一致；轮关节的 lower/upper 填 ±100 表示不限（用 vel_limit 约束）
+# 本节点实现 I/O 时用它做下发前的自检
+[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]
+
+# ------------------------------------------------------------------ 每腿的硬件标定与总线
+# 指令换算：q_motor = (q_model − offset_deg) · dir；反馈反向。
+#   dir       ：电机正方向与模型正方向的关系（±1，左右镜像、前后镜像都在这里）
+#   offset_deg：机械零点偏置 [deg]（模型零位 = 摆臂竖直向下时读到的电机角）
+#   bus/canid ：该关节挂在哪条 CANFD 总线、CANID 是多少
+# 注意：TOML 里数组表之后的裸键会归到最后一个元素，本节必须放在 motor 段之前
+[[legs]]
+name        = "fl"
+swing_joint = "fl_swing"
+wheel_joint = "fl_wheel"
+swing = { dir =  1.0, offset_deg = 0.0, bus = "can0", canid = 1 }
+wheel = { dir =  1.0, offset_deg = 0.0, bus = "can0", canid = 3 }
+
+[[legs]]
+name        = "fr"
+swing_joint = "fr_swing"
+wheel_joint = "fr_wheel"
+swing = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 1 }
+wheel = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 3 }
+
+[[legs]]
+name        = "hl"
+swing_joint = "hl_swing"
+wheel_joint = "hl_wheel"
+swing = { dir =  1.0, offset_deg = 0.0, bus = "can0", canid = 2 }
+wheel = { dir =  1.0, offset_deg = 0.0, bus = "can0", canid = 4 }
+
+[[legs]]
+name        = "hr"
+swing_joint = "hr_swing"
+wheel_joint = "hr_wheel"
+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 时按此驱动
+[motor.joint]                # 摆臂关节电机
+model  = "P65MAXS"
+p_max  = 12.5                # [rad]
+p_min  = -12.5
+v_max  = 65.0                # [rad/s]
+v_min  = -65.0
+t_max  = 100.0               # [Nm]
+t_min  = -100.0
+kp_max = 500.0
+kd_max = 5.0
+
+[motor.wheel]                # 轮毂电机
+model  = "W190S"
+p_max  = 12.5
+p_min  = -12.5
+v_max  = 10.0                # [rad/s]
+v_min  = -10.0
+t_max  = 50.0
+t_min  = -50.0
+kp_max = 500.0
+kd_max = 5.0
+
+[motor.can]
+channels          = ["can0", "can1"]
+bitrate           = 1000000
+data_bitrate      = 5000000
+arbitration_id    = 0        # 控制报文仲裁 ID（台架脚本用 0x000）
+enable_frames     = 5        # 上电使能帧数
+enable_interval_s = 0.02
+
+[motor.gains]                # 台架脚本当前增益；接 RL 时以 [leg.gains]/策略里的 kp/kd 为准
+bench_joint_kp = 150.0
+bench_joint_kd = 2.0
+bench_wheel_kd = 0.5
+
+# ------------------------------------------------------------------ 本节点的话题与节奏
+[topics]
+joint_states   = "joint_states"      # 发布 sensor_msgs/JointState
+imu            = "imu/data"          # 发布 sensor_msgs/Imu
+joint_commands = "joint_commands"    # 订阅 invicta_msgs/JointCommandArray
+
+[hardware]
+publish_rate_hz = 200.0              # 关节/IMU 状态发布频率（RL 侧控制环 200 Hz）
+imu_frame_id    = "base_link"
+command_queue   = 1                  # 指令只取最新一帧
@@ -1,17 +0,0 @@
-# invicta_hardware 参数（私有命名空间 ~）
-
-# ---- 话题与频率 ----
-joint_states_topic: joint_states       # sensor_msgs/JointState
-imu_topic: imu/data                    # sensor_msgs/Imu
-joint_commands_topic: joint_commands    # invicta_msgs/JointCommandArray
-imu_frame_id: base_link
-
-publish_rate_hz: 200.0                 # 状态发布频率，需 >= RL 侧 200 Hz 控制周期
-command_queue: 1                       # 指令只取最新一帧
-
-# ---- 关节顺序（必须与 invicta_deploy 的 joint_names 完全一致）----
-joint_names:
-  [fl_hipx_joint, fl_hipy_joint, fl_knee_joint, fl_wheel_joint,
-   fr_hipx_joint, fr_hipy_joint, fr_knee_joint, fr_wheel_joint,
-   hl_hipx_joint, hl_hipy_joint, hl_knee_joint, hl_wheel_joint,
-   hr_hipx_joint, hr_hipy_joint, hr_knee_joint, hr_wheel_joint]
@@ -56,7 +56,7 @@ private:
    /**
     * @brief 从电机读回状态
     *
-     * TODO(硬件)：接 SocketCAN 读回 16 个关节的
+     * TODO(硬件)：接 SocketCAN 读回各关节（关节表见 invicta.toml）的
     *   position[rad] / velocity[rad/s] / effort[Nm]
     * 并做 raw → policy 约定的换算（方向、零点、单位）。
     * 同时读回 IMU：姿态四元数（弧度）、角速度[rad/s]（机体系）、线加速度[m/s²]（机体系）。
@@ -0,0 +1,258 @@
+/**
+ * @file hardware_config.h
+ * @brief invicta_hardware 自己的配置（TOML）：config/invicta_hardware.toml
+ *
+ * 只包含本节点需要的参数：关节表与硬件标定、每条腿挂的总线/CANID、电机与 CAN 参数、
+ * 本节点发布/订阅的话题与频率。
+ */
+#pragma once
+
+#include <toml++/toml.hpp>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+namespace invicta_hardware {
+
+/// 单个关节：限位与硬件标定
+struct JointSpec {
+    std::string name;
+    bool is_wheel = false;
+    int leg = -1;
+    double lower = 0., upper = 0.;  ///< 位置限位 [rad]（下发前自检用）
+    double vel_limit = 0.;          ///< 速度限位 [rad/s]
+    double dir = 1.;                ///< 硬件方向 ±1
+    double offset_deg = 0.;         ///< 硬件零点偏置 [deg]
+};
+
+/// 一条腿在总线上的位置
+struct LegSpec {
+    std::string name;
+    std::string swing_joint, wheel_joint;
+    std::string swing_bus, wheel_bus;
+    int swing_canid = 0, wheel_canid = 0;
+    int swing_index = -1, wheel_index = -1;
+};
+
+/// 电机量程（MIT 类协议的 p/v/t/kp/kd 上限）
+struct MotorSpec {
+    std::string model;
+    double p_max = 12.5, p_min = -12.5;
+    double v_max = 0., v_min = 0.;
+    double t_max = 0., t_min = 0.;
+    double kp_max = 0., kd_max = 0.;
+};
+
+struct CanSpec {
+    std::vector<std::string> channels;
+    double bitrate = 1000000, data_bitrate = 5000000;
+    int arbitration_id = 0;
+    int enable_frames = 5;
+    double enable_interval_s = 0.02;
+};
+
+class HardwareConfig {
+public:
+    // ---- 关节表与标定 ----
+    std::vector<std::string> joint_names;
+    std::vector<JointSpec> joints;  ///< 顺序同 joint_names
+    std::vector<LegSpec> legs;
+    int dof = 0;
+
+    // ---- 电机与总线 ----
+    MotorSpec motor_joint, motor_wheel;
+    CanSpec can;
+    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_commands = "joint_commands";
+    double publish_rate_hz = 200.0;
+    std::string imu_frame_id = "base_link";
+    int command_queue = 1;
+
+    bool Load(const std::string& path, std::string* error = nullptr);
+
+    int IndexOf(const std::string& name) const {
+        auto it = std::find(joint_names.begin(), joint_names.end(), name);
+        return it == joint_names.end() ? -1 : int(it - joint_names.begin());
+    }
+    const JointSpec& Joint(int i) const { return joints[size_t(i)]; }
+    bool IsWheel(int i) const { return joints[size_t(i)].is_wheel; }
+
+    std::string Summary() const;
+};
+
+inline HardwareConfig& Config() {
+    static HardwareConfig cfg;
+    return cfg;
+}
+
+namespace detail {
+
+inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }
+
+template <typename T>
+inline void Get(const toml::table* sect, const char* key, T& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<T>()) out = *v;
+}
+inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<std::string>()) out = *v;
+}
+inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector<double>& out) {
+    if (!sect) return;
+    if (const auto* arr = (*sect)[key].as_array()) {
+        out.clear();
+        for (const auto& e : *arr)
+            if (auto v = e.value<double>()) out.push_back(*v);
+    }
+}
+inline void GetStrArr(const toml::table* sect, const char* key, std::vector<std::string>& out) {
+    if (!sect) return;
+    if (const auto* arr = (*sect)[key].as_array()) {
+        out.clear();
+        for (const auto& e : *arr)
+            if (auto v = e.value<std::string>()) out.push_back(*v);
+    }
+}
+
+}  // namespace detail
+
+inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
+    const auto fail = [&](const std::string& msg) {
+        if (error) *error = msg;
+        return false;
+    };
+
+    toml::table tbl;
+    try {
+        tbl = toml::parse_file(path);
+    } catch (const toml::parse_error& e) {
+        return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
+    }
+
+    const toml::table* robot = detail::Section(tbl, "robot");
+    if (!robot) return fail("缺少 [robot] 段");
+    detail::GetStrArr(robot, "joint_names", joint_names);
+    if (joint_names.empty()) return fail("[robot].joint_names 为空");
+    dof = int(joint_names.size());
+
+    std::vector<double> lower, upper, vel_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 长度必须等于关节数量");
+    }
+    joints.assign(size_t(dof), JointSpec{});
+    for (int i = 0; i < dof; ++i) {
+        joints[size_t(i)].name = joint_names[size_t(i)];
+        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)];
+    }
+
+    legs.clear();
+    if (const auto* leg_arr = tbl["legs"].as_array()) {
+        for (const auto& elem : *leg_arr) {
+            const toml::table* lt = elem.as_table();
+            if (!lt) continue;
+            LegSpec leg;
+            detail::GetStr(lt, "name", leg.name);
+            detail::GetStr(lt, "swing_joint", leg.swing_joint);
+            detail::GetStr(lt, "wheel_joint", leg.wheel_joint);
+            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 里");
+            }
+            const toml::table* sw = detail::Section(*lt, "swing");
+            const toml::table* wh = detail::Section(*lt, "wheel");
+            detail::Get(sw, "dir", joints[size_t(leg.swing_index)].dir);
+            detail::Get(sw, "offset_deg", joints[size_t(leg.swing_index)].offset_deg);
+            detail::GetStr(sw, "bus", leg.swing_bus);
+            detail::Get(sw, "canid", leg.swing_canid);
+            detail::Get(wh, "dir", joints[size_t(leg.wheel_index)].dir);
+            detail::Get(wh, "offset_deg", joints[size_t(leg.wheel_index)].offset_deg);
+            detail::GetStr(wh, "bus", leg.wheel_bus);
+            detail::Get(wh, "canid", leg.wheel_canid);
+            joints[size_t(leg.swing_index)].leg = int(legs.size());
+            joints[size_t(leg.wheel_index)].leg = int(legs.size());
+            joints[size_t(leg.wheel_index)].is_wheel = true;
+            legs.push_back(leg);
+        }
+    }
+    if (legs.empty()) return fail("缺少 [[legs]] 段");
+
+    if (const toml::table* mj = detail::Section(tbl, "motor")) {
+        const auto load_motor = [](const toml::table* m, MotorSpec& out) {
+            detail::GetStr(m, "model", out.model);
+            detail::Get(m, "p_max", out.p_max);
+            detail::Get(m, "p_min", out.p_min);
+            detail::Get(m, "v_max", out.v_max);
+            detail::Get(m, "v_min", out.v_min);
+            detail::Get(m, "t_max", out.t_max);
+            detail::Get(m, "t_min", out.t_min);
+            detail::Get(m, "kp_max", out.kp_max);
+            detail::Get(m, "kd_max", out.kd_max);
+        };
+        load_motor(detail::Section(*mj, "joint"), motor_joint);
+        load_motor(detail::Section(*mj, "wheel"), motor_wheel);
+
+        if (const toml::table* c = detail::Section(*mj, "can")) {
+            detail::GetStrArr(c, "channels", can.channels);
+            detail::Get(c, "bitrate", can.bitrate);
+            detail::Get(c, "data_bitrate", can.data_bitrate);
+            detail::Get(c, "arbitration_id", can.arbitration_id);
+            detail::Get(c, "enable_frames", can.enable_frames);
+            detail::Get(c, "enable_interval_s", can.enable_interval_s);
+        }
+        const toml::table* mg = detail::Section(*mj, "gains");
+        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);
+    }
+
+    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);
+
+    const toml::table* hw = detail::Section(tbl, "hardware");
+    detail::Get(hw, "publish_rate_hz", publish_rate_hz);
+    detail::GetStr(hw, "imu_frame_id", imu_frame_id);
+    detail::Get(hw, "command_queue", command_queue);
+    if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz 必须 > 0");
+
+    if (error) error->clear();
+    return true;
+}
+
+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());
+    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(),
+                 leg.swing_joint.c_str(), leg.swing_bus.c_str(), leg.swing_canid, leg.wheel_joint.c_str(),
+                 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",
+             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",
+             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;
+    return s;
+}
+
+}  // namespace invicta_hardware
@@ -1,6 +1,6 @@
 <launch>
-  <!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数在节点的私有命名空间下 -->
+  <!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数来自本节点自己的 config/invicta_hardware.toml -->
  <node pkg="invicta_hardware" type="invicta_hardware_node" name="invicta_hardware" output="screen">
-    <rosparam command="load" file="$(find invicta_hardware)/config/invicta_hardware.yaml"/>
+    <param name="config_file" value="$(find invicta_hardware)/config/invicta_hardware.toml"/>
  </node>
 </launch>
@@ -12,6 +12,7 @@
  <buildtool_depend>catkin</buildtool_depend>

  <depend>roscpp</depend>
+  <depend>roslib</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>invicta_msgs</depend>
@@ -2,10 +2,12 @@
 * @file invicta_hardware_node.cpp
 * @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口）
 */
+#include <ros/package.h>
 #include <ros/ros.h>

 #include <thread>

+#include "invicta_hardware/hardware_config.h"
 #include "invicta_hardware/hardware_bridge.h"
 #include "invicta_msgs/control_word.h"

@@ -28,18 +30,16 @@ HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
 }

 void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
-    pnh.param<std::string>("joint_states_topic", joint_state_topic_, "joint_states");
-    pnh.param<std::string>("imu_topic", imu_topic_, "imu/data");
-    pnh.param<std::string>("joint_commands_topic", command_topic_, "joint_commands");
-    pnh.param<std::string>("imu_frame_id", imu_frame_id_, "base_link");
-    pnh.param<double>("publish_rate_hz", publish_rate_hz_, 200.0);
-    pnh.param<int>("command_queue", command_queue_, 1);
+    // 参数来自本节点自己的 config/invicta_hardware.toml
+    const auto& cfg = invicta_hardware::Config();
+    joint_state_topic_ = cfg.topic_joint_states;
+    imu_topic_ = cfg.topic_imu;
+    command_topic_ = cfg.topic_joint_commands;
+    imu_frame_id_ = cfg.imu_frame_id;
+    publish_rate_hz_ = cfg.publish_rate_hz;
+    command_queue_ = cfg.command_queue;

-    joint_names_ = {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
-                    "fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
-                    "hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
-                    "hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
-    pnh.getParam("joint_names", joint_names_);
+    joint_names_ = cfg.joint_names;  // 顺序必须与 invicta_deploy 一致（同一份配置，天然一致）

    if (publish_rate_hz_ <= 0.) {
        ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0，回退到 200 Hz");
@@ -167,6 +167,16 @@ int main(int argc, char** argv) {
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

+    std::string config_path;
+    pnh.param<std::string>("config_file", config_path, "");
+    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());
+        return 1;
+    }
+    std::cout << invicta_hardware::Config().Summary() << std::flush;
+
    invicta_hardware::HardwareBridge bridge(pnh);
    std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);