手柄控制与服务接口

本文档聚焦 Roboto Atom01 在 ROS2 部署阶段的人机交互层，系统阐述推理节点（inference_node）如何通过标准 ROS2 sensor_msgs/Joy 消息接收手柄输入，以及如何通过 std_srvs/Trigger 服务接口向外部暴露电机生命周期与推理状态的控制能力。理解这一层的设计，是连接底层实时控制与上层交互决策的关键枢纽。

Sources: inference_node.hpp

系统架构与数据流

手柄控制与服务接口并非孤立模块，而是嵌入在推理节点内部的一组 ROS2 订阅端与服务端。系统启动时，start_robot.sh 会同时拉起 joy_node（ROS2 官方 joy 包）与 roboto_inference_node。前者通过 Linux input/event 子系统读取物理手柄信号并发布到 /joy；后者订阅该话题，将摇杆与按键映射为速度指令或状态切换事件，同时对外暴露 10 个标准触发服务。

以下 Mermaid 图展示了关键节点、话题与服务的拓扑关系：

graph LR
    A[joy_node<br/>ROS2 joy 包] -->|/joy<br/>sensor_msgs/Joy| B[roboto_inference_node]
    C[外部遥控/导航栈] -->|/cmd_vel<br/>geometry_msgs/Twist| B
    B -->|/action<br/>sensor_msgs/JointState| D[RobotInterface]
    B -->|/imu<br/>sensor_msgs/Imu| E[上位机/可视化]
    B -->|/joint_states<br/>sensor_msgs/JointState| E
    D -->|CAN/Serial| F[电机驱动器]
    D -->|Serial/CAN| G[IMU]
    B -.->|std_srvs/Trigger| H[服务调用方<br/>CLI/SDK]

图中 B 节点是本文的核心对象。它的订阅端采用 KeepLast(1) + Best Effort + Volatile 的 QoS 策略，确保在实时控制场景下总是消费最新的手柄或速度指令，避免因历史消息堆积造成控制延迟。服务端则基于 std_srvs/srv/Trigger，这是一种无参请求-响应模式，适合“执行一次、反馈结果”的离散控制语义。

Sources: inference_node.hpp

手柄控制映射

推理节点在 subs_joy_callback 中完成手柄信号的语义解析。默认情况下，is_joy_control_ 为 true，表示手柄拥有最高控制权；通过 Y 键可随时切至 /cmd_vel 外部指令控制。

摇杆与扳机轴映射

手柄的模拟输入通过 sensor_msgs::msg::Joy 的 axes 数组读取，并经过 clip_cmd 参数限幅后写入共享的 cmd_vel_ 缓冲区。clip_cmd 是一个六元数组 [vx_min, vx_max, vy_min, vy_max, ω_min, ω_max]，在默认配置中对应 [-0.4, 0.6, -0.4, 0.4, -0.8, 0.8]。

物理输入	Joy 索引	语义	限幅区间
右摇杆左右（X 轴）	`axes[4]`	前后线速度 `vx`	`[clip_cmd[0], clip_cmd[1]]`
右摇杆上下（Y 轴）	`axes[3]`	左右线速度 `vy`	`[clip_cmd[2], clip_cmd[3]]`
LT（左扳机）	`axes[2]`	左转角速度 `ω`	`[clip_cmd[4], 0]`
RT（右扳机）	`axes[5]`	右转角速度 `ω`	`[0, clip_cmd[5]]`

当 LT 与 RT 同时未按下时，角速度归零；若同时按下，以 LT 逻辑优先（代码中先判断 axes[2] < 0）。

按键映射

按键通过 buttons 数组读取，所有按键均在上升沿触发（当前帧为 1 且上一帧为 0 时生效），避免长按导致重复执行。

按键	Joy 索引	功能	适用模式
X	`buttons[2]`	电机使能 / 失能切换	全局
A	`buttons[0]`	关节复位到默认角度	全局（需电机已初始化）
B	`buttons[1]`	推理启停切换	全局
Y	`buttons[3]`	切换手柄控制 ↔ `/cmd_vel` 控制	全局
LB	`buttons[4]`	切换策略模式（中断 / 运动策略）	BeyondMimic / Interrupt 模式
RB	`buttons[5]`	切换运动序列	BeyondMimic 模式

当按下 X 键时，节点会暂停推理（若正在运行），然后调用 RobotInterface::init_motors() 或 deinit_motors()，实现安全的电机上下电流程。B 键则直接对 is_running_ 原子变量取反，控制 ONNX 推理循环的启停。

Sources: ros_interface.cpp

双模式控制机制

推理节点支持两种互斥的指令输入通道，由 is_joy_control_ 原子标志仲裁：

手柄模式（Joy）：is_joy_control_ == true 时，仅 /joy 话题的回调能够写入 cmd_vel_ 缓冲区，/cmd_vel 订阅被忽略。
外部指令模式（cmd_vel）：is_joy_control_ == false 时，subs_cmd_callback 接收 geometry_msgs::msg::Twist 消息，提取 linear.x、linear.y、angular.z 并限幅写入同一缓冲区。

两种模式共享相同的 cmd_mutex_ 锁与 clip_cmd 限幅逻辑，因此切换时不会引发数据竞争。该设计使得机器人可以在手柄遥控与自主导航栈之间无缝切换——例如，在调试阶段用手柄介入，在运行阶段交由导航算法接管。

Sources: ros_interface.cpp

服务接口详解

推理节点在构造函数中注册了 10 个 std_srvs/srv/Trigger 服务，覆盖电机生命周期管理、状态读取与推理控制。每个服务均在执行前进行严格的前置状态校验，防止在推理运行或电机未初始化时执行危险操作。

服务名	功能描述	前置条件	执行效果
`init_motors`	初始化电机	电机未初始化	调用 `RobotInterface::init_motors()`，电机上电
`deinit_motors`	反初始化电机	电机已初始化	调用 `RobotInterface::deinit_motors()`，电机下电
`reset_joints`	复位关节	推理已暂停、电机已初始化	关节回到 `joint_default_angle`
`set_zeros`	标定零位	推理已暂停、电机已初始化	设置当前关节位置为零点
`clear_errors`	清除驱动错误	RobotInterface 已实例化	清除底层电机错误标志
`refresh_joints`	刷新电机状态	电机已初始化	刷新内部关节状态缓存
`read_joints`	读取并发布关节状态	电机已初始化	发布 `/joint_states`
`read_imu`	读取并发布 IMU 状态	IMU 已初始化	发布 `/imu`
`start_inference`	启动 ONNX 推理	推理未运行	`is_running_ = true`
`stop_inference`	暂停 ONNX 推理	推理正在运行	`is_running_ = false`

所有服务均采用 try-catch 包裹底层调用，并将异常信息通过 response->message 返回给调用方。例如，reset_joints 在推理运行时立即拒绝请求，返回 "Inference is running, cannot reset joints."，这种**失效安全（fail-safe）**设计是实机部署的关键保障。

Sources: ros_interface.cpp

启动流程与运行环境

start_robot.sh 是生产环境下的统一入口脚本，它负责完成编译、DDS 配置、会话管理与节点健康检查。其启动顺序如下：

sequenceDiagram
    participant U as 用户
    participant S as start_robot.sh
    participant I as inference_session<br/>(screen)
    participant J as joy_session<br/>(screen)
    participant DDS as Fast DDS<br/>共享内存

    U->>S: 执行脚本
    S->>S: 检查并 source ROS2 环境
    S->>S: 导出 Fast DDS XML 配置
    S->>S: colcon build 编译推理包
    S->>I: 启动 inference_node
    S->>S: ros2 node list 检查
    S->>J: 启动 joy_node
    S->>S: ros2 node list 检查
    S->>S: verify_dds_effectiveness
    S->>U: 输出 screen 会话查看命令

脚本通过 screen 将两个节点分别隔离在独立会话中：inference_session 运行推理主节点，joy_session 运行手柄驱动节点。两者均继承脚本中设置的 RMW_IMPLEMENTATION=rmw_fastrtps_cpp 与 FASTRTPS_DEFAULT_PROFILES_FILE 环境变量，确保共享内存传输生效。用户可通过 screen -r inference_session 或 screen -r joy_session 实时查看日志，按 Ctrl+A 再按 D 即可 detach 而不中断进程。

Sources: start_robot.sh

实时线程与安全机制

推理节点内部包含三条具有不同实时优先级的线程：

main 线程：SCHED_FIFO，优先级 50，负责 ROS2 executor spin 与服务/订阅回调处理，手柄按键响应即在此线程中完成。
inference 线程：SCHED_FIFO，优先级 70，负责 ONNX 模型推理与观测栈更新，周期为 dt * decimation（默认 20 ms）。
control 线程：SCHED_FIFO，优先级 70，负责将推理输出通过 RobotInterface::apply_action() 下发到电机，周期为 dt（默认 4 ms）。

由于手柄回调运行于 main 线程，而 cmd_vel_ 同时被 inference 线程读取，节点使用 cmd_mutex_ 保护速度指令缓冲区。同理，act_mutex_、mode_mutex_、perception_mutex_ 等锁分别隔离了动作输出、策略模式切换与感知观测的并发访问，避免在推理循环中途被手柄事件打断而产生不一致状态。

Sources: inference_node.cpp

配置要点与扩展建议

手柄控制的行为可通过 inference.yaml 中的参数调整，最常用的是 clip_cmd 与 joint_default_angle：

clip_cmd：定义三自由度速度指令的上下界，直接影响摇杆灵敏度与最大运动速度。
joint_default_angle：定义 A 键复位或初始姿态时的关节目标值，需与机械零位和 URDF 保持一致。
decimation：控制推理步频相对于控制步频的降采样倍数，影响手柄响应的延迟感。

若需集成自定义手柄（如 PS5 或 Switch Pro 手柄），只需确保 joy_node 正确映射到 Linux /dev/input/js0，并根据实际 axes 与 buttons 顺序微调 subs_joy_callback 中的索引即可，无需改动推理循环本身。

Sources: inference.yaml

阅读衔接

本文档聚焦于人机交互与服务暴露层。若需了解推理节点内部 ONNX 运行时的模型加载、观测栈管理与策略切换逻辑，请参阅前序文档推理节点与ONNX策略部署；若希望通过 Python SDK 以编程方式调用上述服务接口或封装新的交互逻辑，请继续阅读 Python SDK与二次开发。