caowangrenbo
cf70b1a1d7
Source: DeepRoboticsLab/sdk_deploy (BSD-3-Clause), M20 sim2real part only. Packages: - invicta_msgs: JointCommandArray, MotionCommand, MotionState; standard messages (sensor_msgs/JointState, sensor_msgs/Imu) elsewhere; shared motion value defs. - invicta_deploy: state machine (idle/standup/rl/joint-damping/lie-down), ONNX policy with per-gait models, auto RL entry after standup with readiness checks, safety loop, gait->model table; no protocol/network code. - invicta_remote: vendor basic_server protocol (UDP + JSON, proto.md V1.2.0), header/MsgId, 0xE0xx error codes, motion/usage-mode/gait translation, telemetry 1002/3,4,5,6 (2/10/2/2 Hz); publishes /motion_command, subscribes /motion_state. - invicta_hardware: joint/IMU bridge skeleton (SocketCAN I/O is TODO), publishes sensor_msgs/JointState + sensor_msgs/Imu, consumes /joint_commands. Verified on ROS Noetic (Ubuntu 20.04) in docker: clean catkin_make, 200 Hz joint state / 50 Hz RL commands / 20 Hz state report, protocol conformance (MotionParam, gait preconditions, error codes), telemetry push, and end-to-end switching of two policy models with a real remote controller.
invicta_msgs
消息与共享值域。原则：能复用标准消息就复用，只自定义标准消息表达不了的内容。
消息清单
| 话题（默认） | 消息类型 | 来源 | 说明 |
|---|---|---|---|
/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 |
/motion_command |
invicta_msgs/MotionCommand |
自定义 | 协议节点 → RL 节点：运动状态/步态/使用模式/轴指令 |
/motion_state |
invicta_msgs/MotionState |
自定义 | RL 节点 → 协议节点：实际运动状态/步态等 |
自定义消息只有 3 条，原因：
JointCommandArray：标准消息无法表达kp/kd/前馈力矩/控制字（sensor_msgs/JointState只有 position/velocity/effort，trajectory_msgs/JointTrajectory也没有增益）。MotionCommand/MotionState：是节点间的指令/状态接口，字段与厂家协议一一对应，不适合塞进std_msgs。
共享值域：include/invicta_msgs/motion_defs.h
MotionParam、GaitParam、UsageMode 两个节点都要用，统一放在这里（纯头文件，随包导出）：
| MotionParam | 含义 | GaitParam | 含义 | UsageMode | 含义 |
|---|---|---|---|---|---|
| 0 | 空闲 | 0x1001 |
基础(平地) | 0 | 常规 |
| 1 | 站立 | 0x1003 |
楼梯 | 1 | 导航 |
| 2 | 关节阻尼/软急停 | 0x3002 |
平地(敏捷) | 2 | 辅助 |
| 3 | 开机阻尼 | 0x3003 |
楼梯(敏捷) | ||
| 4 | 趴下 | ||||
| 17 | RL 控制 |
control_word.h 同样是导出的纯头文件，定义 JointCommandArray::control_word 的取值（1 关闭使能、2 使能、3 正常控制、4 清错、5 请求状态）。
对照上游 M20（ROS2 drdds）
| M20 | invicta | 变化 |
|---|---|---|
/JOINTS_DATA drdds/JointsData |
/joint_states sensor_msgs/JointState |
换标准消息；丢掉 motion_temp/driver_temp/status_word（见下） |
/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 节点之间的接口 |
被删掉的字段与影响
sensor_msgs/JointState 放不下关节温度与驱动器状态字，相对 M20 少了两条保护：
| M20 字段 | 用途 | 现在的行为 |
|---|---|---|
motion_temp |
电机过温 >100 ℃ 告警 | 温度接口返回常量，检查恒不触发 |
status_word |
驱动器报错判断（!=1 即报错） |
状态字接口返回全 1，检查恒不触发 |
要恢复就加一条独立低频诊断话题（例如 1 Hz 的 JointDiagnostics），不要塞回 200 Hz 控制路径。协议里的设备状态上报（1002/5）需要的温度/电池数据，也应走 invicta_hardware → 协议节点这条线。