🤖 roboto_origin_03 Wiki
首页 / 部署 / 启动机器人与推理节点

本文档面向初次接触本部署框架的开发者,完整介绍如何从编译到运行,将推理节点与机器人硬件打通。内容覆盖一键启动脚本的工作流程、推理节点的三线程实时架构、多策略配置切换方法,以及首次控制机器人的安全操作顺序。阅读前请确保已完成 环境配置与依赖安装硬件连接与 CAN/IMU 映射电机零点标定

Sources: README_CN.md

启动前检查清单

在运行启动脚本前,请逐项确认以下事项,任何遗漏都可能导致启动失败或硬件异常。首先,机器人必须完成零点标定,且 src/inference/config/robot.yaml 中的 motor_zero_offset 与实际标定方式严格对应:若腰部 yaw 是通过转至限位块标定的,保留 2.093;若使用打印件固定标定,则改为 0.0。其次,CAN 接口与 IMU 串口应已通过 udev 规则自动绑定,执行 ip a 应能看到 can0can3 处于 UP 状态。再次,确保已安装 ros-humble-joyscreenccache 等依赖,并且当前用户具备实时优先级权限(ulimit -r 返回 98)。最后,机器人应放置在安全区域,四周无易碎物品,操作人员做好随时切断电源的准备。

Sources: README_CN.md, robot.yaml

一键启动脚本详解

项目提供 tools/start_robot.sh 作为官方推荐的一键启动入口,它将编译、环境检查、节点启动和 DDS 验证整合为一条自动化流水线。脚本首先设置 Fast DDS 相关的三个环境变量,指向 assets/rt_fastdds_profile.xml,该配置文件启用了共享内存与本地 UDP 混合传输,并将日志级别降至 ERROR 以减少控制台噪声。随后脚本检查 ROS2 环境是否已 source,若未检测到 AMENT_PREFIX_PATH 则自动加载 /opt/ros/humble/setup.bash。接着验证 colconros2screen 三个关键工具是否就绪,并执行 colcon build --symlink-install 编译整个工作空间。编译成功后,脚本清理可能残留的同名 screen 会话,然后依次启动 inference_sessionjoy_session 两个后台会话,分别对应推理节点和手柄节点。每个会话启动后,脚本都会通过 ros2 node list 进行存活检测,若节点未在超时时间内出现,则自动清理会话并退出。最后,脚本调用 verify_dds_effectiveness 函数,从环境变量注入、XML 语法、进程级环境变量、共享内存活跃度以及 DDS 发现延迟五个维度验证通信配置是否真正生效。

Sources: start_robot.sh

flowchart TD
    A[执行 ./tools/start_robot.sh] --> B[设置 Fast DDS 环境变量]
    B --> C{AMENT_PREFIX_PATH 存在?}
    C -->|否| D[source /opt/ros/humble/setup.bash]
    C -->|是| E[检查 colcon / ros2 / screen]
    D --> E
    E --> F[colcon build --symlink-install]
    F --> G[清理旧 screen 会话]
    G --> H[启动 inference_session]
    H --> I{ros2 node list 包含 inference_node?}
    I -->|否| J[清理会话并退出]
    I -->|是| K[启动 joy_session]
    K --> L{ros2 node list 包含 joy_node?}
    L -->|否| J
    L -->|是| M[验证 DDS 配置]
    M --> N[打印启动成功信息与手柄说明]

脚本在启动节点时,通过 bash -c 将会话内部的环境变量显式导出,确保 screen 子进程能够继承 Fast DDS 配置。这一点至关重要,因为 screen 默认不会继承父 shell 中通过 export 设置的部分环境变量,若跳过显式导出步骤,推理节点可能退回到默认 DDS 实现,导致通信延迟飙升。启动成功后,终端会输出两个后台会话的名称及连接方式,使用 screen -r inference_session 即可实时查看推理日志,按 Ctrl+A 再按 D 可 detach 回主终端。

Sources: start_robot.sh, start_robot.sh

推理节点三线程架构

推理节点 inference_node 是整个机器人的大脑,它在初始化阶段会完成 ONNX 模型加载、观测布局解析、硬件抽象层构造以及 ROS 接口注册。节点内部运行三条具备不同实时优先级的线程,分别承担 ROS 事件处理、策略推理和电机控制三大职责。

Sources: inference_node.cpp, inference_node.hpp

主线程(main) 负责运行 MultiThreadedExecutor,调度 ROS2 的订阅、发布和服务回调。它在 main 函数中被设置为 SCHED_FIFO 优先级 50,并调用 mlockall(MCL_CURRENT | MCL_FUTURE) 将进程内存锁定,防止运行时发生页错误导致的延迟抖动。主线程同时也是用户交互的入口,手柄按键和外部服务调用都在此被解析为状态转换指令。

推理线程(inference)SCHED_FIFO 优先级 70 运行,循环周期由 dt * decimation 决定。在默认配置 dt=0.004decimation=5 下,推理周期为 20ms,即每秒执行 50 次策略前向传播。每次循环中,线程从 RobotInterface 读取 IMU 和关节状态,按 obs_layout 定义的顺序组装观测向量,通过 frame_stack 历史帧堆叠后送入 ONNX Runtime 会话,最终将网络输出映射为目标关节位置并发布到 /action 话题。若某次循环耗时超过周期,节点会打印 Inference loop overran! 警告,提示系统负载过高或实时性不足。

控制线程(control) 同样以 SCHED_FIFO 优先级 70 运行,周期固定为 dt(默认 4ms)。它的职责相对单一:以 act_alpha 平滑后的目标动作调用 RobotInterface::apply_action(),将策略输出转换为 MIT 控制指令下发到电机。控制线程与推理线程解耦的设计,确保了即使推理偶尔超时,电机控制仍然以固定频率执行,维持机器人姿态稳定。

Sources: inference_node.cpp, inference_node.cpp, inference_node.hpp

flowchart LR
    subgraph 主线程 ["主线程 (SCHED_FIFO 50)"]
        A1[ROS2 Executor]
        A2[手柄/服务回调]
    end
    subgraph 推理线程 ["推理线程 (SCHED_FIFO 70, 20ms)"]
        B1[读取传感器]
        B2[组装观测]
        B3[ONNX 推理]
        B4[发布 /action]
    end
    subgraph 控制线程 ["控制线程 (SCHED_FIFO 70, 4ms)"]
        C1[平滑动作]
        C2[MIT 指令下发]
    end
    A2 -->|is_running| B1
    B4 -->|act_| C1
    C2 -->|CAN| D[电机驱动]

配置文件与参数速查

推理节点的行为由两个层级的 YAML 文件共同决定:推理参数文件(通过 launch 文件加载)定义策略模型、观测布局和动作缩放;机器人参数文件 robot.yaml 定义 IMU、电机总线和控制增益。初学者最常修改的是 inference.yaml 中的以下参数。

参数名 类型 默认值 说明
model_names string[] ["policy.onnx"] 要加载的 ONNX 策略文件列表,支持多策略
obs_layouts string[] "ang_vel:3, gravity_b:3..." 每条策略的观测向量字段定义
frame_stacks int[] [10] 历史帧堆叠数量,1 表示不堆叠
obs_stack_orders string[] ["frame_major"] 堆叠维度顺序,frame_majorobs_major
decimation int 5 推理周期相对于 dt 的倍数
dt float 0.004 控制周期(秒),与 decimation 共同决定推理频率
act_alpha float 1.0 动作平滑系数,1.0 表示不平滑
action_scale float 0.25 网络输出到关节偏移的缩放系数
joint_default_angle double[] 23 维向量 各关节默认姿态,复位与观测归一化均依赖此值
clip_cmd double[] 6 维向量 [x_min, x_max, y_min, y_max, yaw_min, yaw_max]
gravity_z_upper float -0.5 机体倾倒检测阈值,重力向量 z 分量超过此值则自动关机

robot.yaml 中的 kpkd 数组直接决定 MIT 控制律的刚度与阻尼。对于闭链关节(如踝关节),RobotInterface 会在 apply_action 内部自动完成解耦与力控转换,用户无需在配置中区分普通关节与闭链关节的控制模式。

Sources: inference.yaml, robot.yaml, robot_interface.cpp

多策略配置切换

项目内置了多种预训练策略配置,分别存放在 src/inference/config/ 目录下。切换策略不需要修改任何源码,只需编辑 src/inference/launch/inference.launch.pyconfigs 列表加载的 YAML 文件名,然后重新运行 ./tools/start_robot.sh 即可。

Sources: inference.launch.py

配置文件 策略数量 特殊功能 适用场景
inference.yaml 1 基础行走策略,10 帧观测堆叠 默认行走,适合大多数入门场景
inference_amp.yaml 1 无观测堆叠,AMP 风格训练 需要更高响应速度的行走
inference_getup.yaml 2 基础策略 + 起身策略,带 motion 跌倒后自动起身
inference_beyondmimic.yaml 4 基础 + wave/dance/punch 运动序列 演示多动作切换
inference_interrupt.yaml 1 支持外部 interrupt 观测源 需要外部模块介入控制
inference_attn_enc.yaml 1 注意力编码器策略 使用注意力机制的实验策略

inference_beyondmimic.yaml 为例,它同时加载了四个 ONNX 模型,其中第一个为基础行走策略,后三个分别绑定 wave.npzdance.npzpunch.npz 运动序列。运行时,LB 键用于在基础策略与运动策略之间切换,RB 键用于在运动策略内部循环选择 wave、dance 或 punch。运动序列通过 motion_posmotion_vel 观测字段注入到策略输入中,实现动作模仿与网络推理的混合控制。

Sources: inference_beyondmimic.yaml, ros_interface.cpp

实时性与 DDS 优化

推理节点对通信和调度的实时性有严格要求。启动脚本通过以下机制保障低延迟通信:首先,RMW_IMPLEMENTATION 被固定为 rmw_fastrtps_cpp,强制使用 Fast DDS 而非默认实现;其次,FASTRTPS_DEFAULT_PROFILES_FILE 指向 assets/rt_fastdds_profile.xml,该配置禁用了内置传输,显式启用共享内存(SHM)与本地回环 UDP 双通道,并将发布/订阅的可靠性策略设为 BEST_EFFORT、持久性设为 VOLATILE,避免历史数据重传带来的延迟堆积;最后,内存分配策略采用 PREALLOCATED_WITH_REALLOC,在初始化阶段预分配缓冲区,减少运行时的动态内存分配。

Sources: rt_fastdds_profile.xml, start_robot.sh

若启动脚本末尾的 DDS 验证报告显示发现延迟超过 1000ms,请检查以下可能原因:XML 文件路径错误或被其他 shell 配置覆盖;screen 会话未正确继承环境变量;存在多个 DDS 实现冲突(可通过 dpkg -l | grep rmw 排查)。推理节点内部的三条线程均通过 pthread_setschedparam 申请 SCHED_FIFO 实时调度,若出现 Failed to set realtime priority 错误,说明当前用户未在 /etc/security/limits.conf 中配置 rtprio,请参考 环境配置与依赖安装 重新设置。

Sources: start_robot.sh, inference_node.cpp

首次控制入门

启动成功后,机器人并不会立即运动,这是出于安全考虑的设计。你需要按顺序执行以下操作:第一步,按手柄 X 键 初始化电机,此时电机会上电并进入力控待机状态;第二步,按 A 键 将所有关节复位到 joint_default_angle 定义的默认姿态;第三步,按 B 键 启动推理,机器人开始根据策略模型和手柄指令运动。若需暂停,再次按 B 键 即可,电机会保持当前姿态但不再接收新的策略输出。按 X 键 可以去初始化电机,使其完全失能,机器人会自然下垂。

Sources: ros_interface.cpp

按键 功能 前提条件
X 键 使能 / 失能电机
A 键 复位关节到默认姿态 电机已初始化
B 键 开始 / 暂停推理 电机已初始化
Y 键 切换手柄控制 / cmd_vel 指令控制
LB 键 切换策略模式(interrupt / motion policy) 当前配置支持多策略
RB 键 切换运动序列 当前配置包含 motion policy
右摇杆 前后左右平移速度指令 推理运行中
LT / RT 左右旋转速度指令 推理运行中

当切换到 cmd_vel 控制模式后,你可以通过发布 geometry_msgs/msg/Twist 消息到 /cmd_vel 话题来控制机器人移动,这在自动化测试或上位机远程控制时非常有用。手柄与指令控制的切换是互斥的,通过 is_joy_control_ 原子变量保证线程安全。

Sources: ros_interface.cpp, inference_node.hpp

ROS2 服务接口速览

推理节点在启动时注册了十个 std_srvs/srv/Trigger 类型的服务,允许外部节点或命令行在不停机的情况下执行硬件操作。所有服务都遵循统一的安全检查逻辑:例如 set_zeros 要求推理已停止且电机已初始化,reset_joints 要求推理已停止,start_inference 要求推理尚未运行。这种设计避免了在机器人运动过程中意外执行危险操作。

Sources: ros_interface.cpp

服务名 功能 安全约束
/init_motors 初始化所有电机 电机未初始化
/deinit_motors 去初始化所有电机 电机已初始化
/start_inference 启动推理循环 推理未运行
/stop_inference 停止推理循环 推理运行中
/reset_joints 关节复位到默认姿态 推理停止、电机已初始化
/set_zeros 将当前姿态写入电机零点 推理停止、电机已初始化
/clear_errors 清除电机错误标志
/refresh_joints 刷新关节状态缓存 电机已初始化
/read_joints 读取并发布关节状态 电机已初始化
/read_imu 读取并发布 IMU 状态 IMU 已初始化

通过命令行调用服务的标准格式为 ros2 service call /service_name std_srvs/srv/Trigger,例如 ros2 service call /start_inference std_srvs/srv/Trigger。更多服务调用示例和进阶用法请参考 ROS2 服务接口调用

Sources: inference_node.hpp

后台会话管理

start_robot.sh 使用 screen 作为后台进程管理器,相比 nohup 或直接后台运行,screen 的优势在于可以随时 attach 到会话查看实时日志,并在 detach 后保持进程继续运行。以下是日常调试中最常用的几条命令。

命令 作用
screen -r inference_session attach 到推理节点会话查看日志
screen -r joy_session attach 到手柄节点会话查看日志
Ctrl+A 然后按 D 从当前 screen 会话 detach,进程不中断
screen -S inference_session -X quit 强制终止推理节点会话
screen -S joy_session -X quit 强制终止手柄节点会话
screen -ls 列出所有活跃的 screen 会话

如果你习惯使用 tmux,也可以手动启动节点,只需确保在新会话中先 source ROS2 和工作空间环境,并导出三个 Fast DDS 环境变量即可。

Sources: start_robot.sh, start_robot.sh

常见问题排查

以下是初学者在启动阶段最常遇到的问题及对应排查方向。

现象 可能原因 排查方法
start_robot.shcolcon 未安装 ROS2 开发工具未安装 执行 sudo apt install python3-colcon-common-extensions
Inference paused 后机器人无响应 电机未初始化 检查手柄 X 键是否已按下,或调用 /init_motors
ros2 node listinference_node 编译失败或节点崩溃 attach 到 inference_session 查看具体错误日志
DDS 发现延迟 > 1000ms 环境变量未传递或 XML 路径错误 检查 echo $FASTRTPS_DEFAULT_PROFILES_FILE 输出
Failed to set realtime priority 用户无实时权限 检查 ulimit -r,确认 limits.conf 已配置
Robot fell down! Shutting down... 机器人倾倒,重力向量 z 分量超限 扶正机器人后重新启动,或调整 gravity_z_upper
Joint X out of limit! 关节超出软限位 检查 joint_limits 配置与实际机械限位是否匹配
推理线程打印 overran 系统负载过高或实时内核未生效 检查是否安装了实时内核,关闭不必要的后台程序
手柄无响应 joy 包未安装或设备权限不足 检查 ros-humble-joy 是否安装,/dev/input/js0 是否存在

若上述排查无法解决问题,建议先 attach 到 inference_session 获取完整的 FATAL/ERROR 日志,然后结合 常见问题排查与调试技巧 中的系统级调试方法进一步定位。

Sources: start_robot.sh, obs_manager.cpp, inference_node.cpp

下一步

完成启动并确认机器人可以正常行走后,你可以继续阅读 手柄控制说明 深入了解手柄映射细节,或者阅读 ROS2 服务接口调用 学习如何通过脚本和外部节点控制机器人。若想理解推理节点内部的线程同步、观测堆叠和模型加载机制,可深入阅读 推理节点设计与线程模型ONNX 模型加载与实时推理