Skip to main content

RL控制框架ROS接口文档

本文档整理了RL控制框架中提供的所有ROS服务接口和监控话题，包括控制器管理、控制器状态查询、以及特定控制器的功能服务和监控调试话题。

目录​

  1. 控制器管理服务（RLControllerManager） — 行走列表/循环/倒地状态及多舞蹈接口
  2. 控制器基础服务（RLControllerBase） - 5个服务
  3. 倒地起身控制器服务（FallStandController） - 1个服务
  4. 主控制器服务（humanoidController） - 1个服务
  5. 腰部控制器接口（WaistController） - 2个话题
  6. 监控与调试话题 - 5个话题

1. 控制器管理服务（RLControllerManager）​

这些服务由 RLControllerManager 提供，用于管理多个RL控制器的切换、查询和状态管理。所有服务位于 /humanoid_controller 命名空间下。

1.1 /humanoid_controller/switch_controller​

服务类型: kuavo_msgs/switchController

功能: 切换到指定的控制器

请求参数:

  • controller_name (string): 要切换到的控制器名称
    • "mpc" 或空字符串：切回MPC基础控制器
    • 其他名称（如 "amp_controller"、"fall_stand_controller" 等）：切换到对应的RL控制器

响应参数:

  • success (bool): 切换是否成功
  • message (string): 返回消息，包含成功或失败的原因

使用说明:

  • 只能切换到已加载且启用的控制器
  • 控制器必须在 walk_controllers_ 列表中（不包含舞蹈控制器；舞蹈请使用 1.5 节 switch_to_dance_controller）
  • 从RL切换到MPC时，如果RL控制器不在stance状态，切换会被阻止
  • 从MPC切换到RL时，如果MPC不在stance状态，切换会被阻止（倒地起身控制器除外）

示例:

# 切换到MPC控制器
rosservice call /humanoid_controller/switch_controller "controller_name: 'mpc'"

# 切换到AMP行走控制器
rosservice call /humanoid_controller/switch_controller "controller_name: 'amp_controller'"

1.2 /humanoid_controller/get_controller_list​

服务类型: kuavo_msgs/getControllerList

功能: 获取当前可用的控制器列表和当前激活的控制器信息

请求参数: 无

响应参数:

  • controller_names (string[]): 可用控制器名称列表（包含 "mpc"）
  • count (int32): 控制器数量
  • current_index (int32): 当前控制器索引（-1表示未找到）
  • current_controller (string): 当前控制器名称（"mpc"表示MPC控制器）
  • success (bool): 获取是否成功
  • message (string): 返回消息

使用说明:

  • MPC控制器始终在索引0
  • 返回的列表只包含行走控制器（walkcontrollers），不包括其他类型的控制器

示例:

rosservice call /humanoid_controller/get_controller_list

1.3 /humanoid_controller/switch_to_next_controller​

服务类型: kuavo_msgs/switchToNextController

功能: 在控制器列表中循环切换到下一个控制器

请求参数: 无

响应参数:

  • success (bool): 切换是否成功
  • message (string): 返回消息
  • current_controller (string): 切换前的控制器名称
  • next_controller (string): 切换后的控制器名称
  • current_index (int32): 切换前的控制器索引
  • next_index (int32): 切换后的控制器索引

使用说明:

  • 按顺序循环切换：mpc → 第一个RL → ... → 最后一个RL → mpc
  • 适合作为手柄或键盘的"一键切换模式"接口
  • 切换逻辑与 switch_controller 相同，包含相同的保护机制
  • PICO（kuavo_pico_gmr / pico_comm_minimal）：RG + A（右手握把边键 + A 上升沿）触发本服务；详见 kuavo_pico_gmr/launch/README.md

示例:

rosservice call /humanoid_controller/switch_to_next_controller

1.3.1 /humanoid_controller/switch_to_previous_controller​

服务类型: kuavo_msgs/switchToNextController

功能: 在控制器列表中循环切换到上一个控制器

请求参数: 无

响应参数: 与 switch_to_next_controller 相同（success、message、current_controller、next_controller、current_index、next_index）

使用说明:

  • 按顺序反向循环：mpc ← 最后一个RL ← … ← 第一个RL ← mpc
  • 保护机制与 switch_to_next_controller 相同
  • PICO（pico_comm_minimal）：RG + B（右手握把边键 + B 上升沿，且未按 RT）触发本服务

示例:

rosservice call /humanoid_controller/switch_to_previous_controller

1.4 /humanoid_controller/set_fall_down_state​

服务类型: std_srvs/SetBool

功能: 设置机器人的倒地状态，并自动切换到倒地起身控制器

请求参数:

  • data (bool):
    • true: 设置为倒地状态（FALL_DOWN）
    • false: 设置为站立状态（STANDING）

响应参数:

  • success (bool): 设置是否成功
  • message (string): 返回消息，包含状态设置和控制器切换的结果

使用说明:

  • 当设置为倒地状态（true）时：
    1. 通过回调函数更新 humanoidController 的 fall_down_state_ 成员变量
    2. 如果存在 FALL_STAND_CONTROLLER，会自动切换到倒地起身控制器
    3. 如果切换失败或控制器不存在，会在响应消息中说明
  • 当设置为站立状态（false）时：
    • 仅更新 fall_down_state_ 为 STANDING，不进行控制器切换
  • 用于外部系统（如状态估计模块）通知主控制器机器人已倒地

示例:

# 设置为倒地状态（会自动切换到倒地起身控制器）
rosservice call /humanoid_controller/set_fall_down_state "data: true"

# 设置为站立状态
rosservice call /humanoid_controller/set_fall_down_state "data: false"

1.5 /humanoid_controller/switch_to_dance_controller​

服务类型: kuavo_msgs/SetString

功能: 切换到指定舞蹈 RL 控制器实例。逻辑与 RLControllerManager::switchDanceControllerByStringCallback 一致（多支舞在 rl_controllers.yaml 中配置多条 type: DANCE_CONTROLLER）。

请求参数:

  • data (string)，语义如下：
    • 空字符串 ""：切换到舞蹈列表中的第一项（与旧版仅支持单舞时的默认行为一致）
    • # + 非负整数（如 #0、#1）：按 get_dance_controller_list 返回的 data[] 下标切换
    • 其他字符串：按已注册的控制器名称切换（须为 get_dance_controller_list 中的一项，例如 dance_controller）

响应参数:

  • success (bool): 是否切换成功
  • message (string): 说明信息或失败原因

使用说明:

  • 需在对应版本 rl_controllers.yaml 中启用至少一条 DANCE_CONTROLLER
  • 从 MPC 切到舞蹈时，仍受「MPC 须在 stance」等与 switchController 相同的保护；不同舞蹈实例之间允许直接切换（由各自 resume() 重置轨迹）
  • 从舞蹈切回 MPC/行走时，若舞蹈侧 requestToExit() 为 false，可能被 mimic 保护拦截（与倒地起身类似逻辑，详见框架说明文档）

示例:

# 切换到 yaml 中第一个 DANCE_CONTROLLER
rosservice call /humanoid_controller/switch_to_dance_controller "data: ''"

# 按列表下标（第二个舞蹈）
rosservice call /humanoid_controller/switch_to_dance_controller "data: '#1'"

# 按控制器名称
rosservice call /humanoid_controller/switch_to_dance_controller "data: 'dance_controller'"

1.6 /humanoid_controller/get_dance_controller_list​

服务类型: kuavo_msgs/GetStringList

功能: 返回当前已加载、且类型为 DANCE_CONTROLLER 的控制器 name 列表，顺序与 rl_controllers.yaml 中声明顺序一致（内部为 dance_controllers_）。

请求参数: 无（GetStringList 请求体为空）

响应参数:

  • data (string[]): 舞蹈控制器名称列表
  • success (bool): 查询是否成功
  • message (string): 简要说明（如舞蹈数量）

使用说明:

  • 与 get_controller_list 互补：后者只返回行走列表（含 mpc），本服务只列舞蹈项
  • 可与 1.5 配合：先 get_dance_controller_list 再按名或 #索引 调用 switch_to_dance_controller

示例:

rosservice call /humanoid_controller/get_dance_controller_list

2. 控制器基础服务（RLControllerBase）​

这些服务由所有 RL 控制器（如 AmpWalkController、FallStandController、DanceController 等）继承提供。服务命名空间为 /humanoid_controllers/{controller_name}，其中 {controller_name} 是控制器的名称（如 amp_controller、fall_stand_controller、dance_controller 等）。

2.1 /humanoid_controllers/{controller_name}/reload​

服务类型: std_srvs/Trigger

功能: 重新加载控制器的配置文件

请求参数: 无

响应参数:

  • success (bool): 重新加载是否成功
  • message (string): 返回消息

使用说明:

  • 只有在控制器处于非运行状态（PAUSED或STOPPED）时才能重新加载
  • 如果控制器正在运行，会返回失败并提示先停止或暂停控制器

示例:

# 重新加载amp_walk控制器的配置
rosservice call /humanoid_controllers/amp_controller/reload

# 重新加载fall_stand控制器的配置
rosservice call /humanoid_controllers/fall_stand_controller/reload

2.2 /humanoid_controllers/{controller_name}/isActive​

服务类型: std_srvs/Trigger

功能: 查询控制器是否处于激活状态

请求参数: 无

响应参数:

  • success (bool): 控制器是否激活（true表示激活，false表示未激活）
  • message (string): 返回消息（"Controller is active" 或 "Controller is not active"）

使用说明:

  • 控制器处于 RUNNING 状态时返回 true
  • 控制器处于 PAUSED、STOPPED 或 INITIALIZING 状态时返回 false

示例:

rosservice call /humanoid_controllers/amp_controller/isActive

2.3 /humanoid_controllers/{controller_name}/getState​

服务类型: std_srvs/Trigger

功能: 获取控制器的当前状态

请求参数: 无

响应参数:

  • success (bool): 查询是否成功（始终为true）
  • message (string): 状态码（整数字符串）
    • 0: INITIALIZING（初始化中）
    • 1: RUNNING（运行中）
    • 2: PAUSED（已暂停）
    • 3: STOPPED（已停止）

使用说明:

  • 状态码以字符串形式返回，需要解析为整数

示例:

rosservice call /humanoid_controllers/amp_controller/getState

2.4 /humanoid_controllers/{controller_name}/getType​

服务类型: std_srvs/Trigger

功能: 获取控制器的类型

请求参数: 无

响应参数:

  • success (bool): 查询是否成功（始终为true）
  • message (string): 控制器类型码（整数字符串）
    • 0: MPC（基础控制器）
    • 1: FALL_STAND_CONTROLLER（倒地起身控制器）
    • 2: AMP_CONTROLLER（AMP行走控制器）
    • 其他: 未来可能扩展的类型

使用说明:

  • 类型码以字符串形式返回，需要解析为整数

示例:

rosservice call /humanoid_controllers/amp_controller/getType

2.5 /humanoid_controllers/{controller_name}/reset​

服务类型: std_srvs/Trigger

功能: 重置控制器的内部状态

请求参数: 无

响应参数:

  • success (bool): 重置是否成功
  • message (string): 返回消息

使用说明:

  • 只有在控制器处于非运行状态（PAUSED或STOPPED）时才能重置
  • 如果控制器正在运行，会返回失败并提示先停止或暂停控制器
  • 重置会清除控制器的内部状态（如相位、动作历史等）

示例:

rosservice call /humanoid_controllers/amp_controller/reset

3. 倒地起身控制器服务（FallStandController）​

这些服务由 FallStandController 提供，专门用于倒地起身功能。

3.1 /humanoid_controller/fall_stand_command​

服务类型: kuavo_msgs/FallStandCommand

功能: 显式控制倒地起身状态机

请求参数:

  • command (uint8):
    • 1 = PREPARE: 插值到起身初始姿态 (FALL_DOWN→INTERPOLATING→READY)
    • 2 = STAND_UP: 执行 RL 起身轨迹 (READY→STAND_UP)
    • 3 = RESET: 复位状态机

状态机流程:

  • FALL_DOWN(0) → INTERPOLATING(1) → READY(2) → STAND_UP(3) → STANDING(4)
  • 阶段状态通过 /humanoid_controller/FallStandController/fall_stand_state_ 话题发布

使用说明:

  • PREPARE 触发后控制器自动插值，完成后状态变为 READY
  • STAND_UP 仅在 READY 状态有效
  • 正常操作流程：先 PREPARE，等 fallstand_state 变为 2(READY)，再 STAND_UP

示例:

rosservice call /humanoid_controller/fall_stand_command "command: 1"  # PREPARE
rosservice call /humanoid_controller/fall_stand_command "command: 2"  # STAND_UP

---

## 4. 主控制器服务（humanoidController）

这些服务由 `humanoidController` 直接提供，用于控制搬运模式等全局状态。

### 4.1 `/humanoid_controller/transport_mode_command`

**服务类型**: `kuavo_msgs/TransportModeCommand`

**功能**: 搬运模式控制

**请求参数 - command (uint8)**:
| 值 | 名称 | 说明 |
|----|------|------|
| 1 | TRANSPORT_ENTER | 进入搬运，躯干插值到搬运姿态 |
| 2 | TRANSPORT_LOCK | 关节锁死进入可搬运状态 |
| 3 | TRANSPORT_EXIT | 完全退出搬运（HANDING_OVER → INACTIVE） |
| 4 | TRANSPORT_FALL_DOWN | 瘫软倒地，切倒地起身控制器 |
| 5 | TRANSPORT_HAND_OVER | 移交管理权，开始退出搬运 |

**状态机流程**:

INACTIVE →(ENTER)→ INTERPOLATING(~1s躯干插值) →(自动)→ READY(姿态就绪) →(LOCK)→ ACTIVE(关节锁死) →(HAND_OVER)→ 恢复原控制器 →(EXIT)→ INACTIVE ACTIVE →(FALL_DOWN)→ 倒地起身 → 恢复站立


**使用说明**:
- ENTER：需 MPC stance 或 RL 模式，进入后躯干自动插值到搬运姿态
- READY 阶段：姿态已到位，关节未锁，可 LOCK 或 HAND_OVER
- ACTIVE 阶段：关节 CSP 锁死（腿+腰+臂+头全覆盖），暂停 MPC/WBC/RL 计算，可安全搬运
- HAND_OVER：移交管理权，控制器恢复原模式
- FALL_DOWN：仅 ACTIVE 可用，瘫软后走倒地起身流程恢复
- 搬运期间拉起保护暂停

**示例**:
```bash
rosservice call /humanoid_controller/transport_mode_command "command: 1"  # ENTER
rosservice call /humanoid_controller/transport_mode_command "command: 2"  # LOCK→ACTIVE
rosservice call /humanoid_controller/transport_mode_command "command: 5"  # HAND_OVER
rosservice call /humanoid_controller/transport_mode_command "command: 4"  # FALL_DOWN

5. 腰部控制器接口（WaistController）​

WaistController 是集成在RL控制器中的腰部控制模块，提供外部控制腰部关节的功能。支持两种控制模式：模式1（RL控制）和模式2（外部控制）。

话题接口:

  • /humanoid_controller/enable_waist_control (std_msgs/Bool): 启用/禁用腰部外部控制
    • true: 切换到模式2（外部控制）
    • false: 切换回模式1（RL控制），使用低通滤波器平滑过渡到默认位置
  • /robot_waist_motion_data (kuavo_msgs/robotWaistControl): 发送外部腰部控制指令（仅在模式2时生效）
    • data.data[]: 腰部关节目标角度（度），超出范围的值会被自动限制

控制模式:

  • 模式1（RL控制）: 默认模式，由RL控制器完全控制。从模式2切换回时，如果误差大于阈值（0.02 rad），会使用低通滤波器平滑过渡到默认位置
  • 模式2（外部控制）: 通过 /robot_waist_motion_data 接收外部指令，经过低通滤波处理。仿真环境会计算PD前馈扭矩，实物环境不计算

配置参数 (waistControllerParam):

  • mode2CutoffFreq: 低通滤波器截止频率（Hz，默认0.8）
  • kp: PD控制位置增益（默认10.0）
  • kd: PD控制速度增益（默认2.0）

启用条件: 需在配置文件中设置 use_external_waist_controller = true，且机器人有腰部关节（waist_dof_ > 0）

6. 监控与调试话题​

这些话题由 humanoidController 通过 TopicLogger 实时发布，用于监控控制器状态和调试MPC↔RL模式切换过程。

6.1 /humanoid_controller/is_rl_controller_​

话题类型: std_msgs/Float64

发布频率: 与控制循环频率相同（通常为100Hz或更高）

功能: 实时发布当前是否处于RL控制模式

消息内容:

  • data (float64):
    • 1.0: 当前处于RL控制模式
    • 0.0: 当前处于MPC控制模式

使用说明:

  • 状态由 !controller_manager_->isBaseControllerActive() 决定
  • 便于监控MPC↔RL模式切换
  • 可用于外部系统（如可视化工具、日志记录）判断当前控制模式

订阅示例:

# 使用rostopic查看
rostopic echo /humanoid_controller/is_rl_controller_

# 使用rqt_plot可视化
rqt_plot /humanoid_controller/is_rl_controller_/data

6.2 /humanoid_controller/resetting_mpc_state_​

话题类型: std_msgs/Float64

发布频率: 与控制循环频率相同（通常为100Hz或更高）

功能: 实时发布MPC重置状态，用于监控从RL切换到MPC时的重置过程

消息内容:

  • data (float64): MPC重置状态码
    • 0 (NORMAL): 正常状态，MPC正常运行
    • 1 (RESET_INITIAL_POLICY): 重置MPC状态1，等待初始策略
    • 2 (RESET_BASE): 重置MPC状态2，更新躯干位置（插值阶段）

状态转换流程:

  • 当从RL切回MPC时，状态会依次经历：
    • RESET_INITIAL_POLICY (1) → RESET_BASE (2) → NORMAL (0)
  • 便于监控MPC重置进度和调试切换过程

使用说明:

  • 在RL→MPC切换过程中，可以通过此话题监控重置进度
  • 当状态为 NORMAL (0) 时，表示MPC已完全重置并正常运行
  • 可用于外部系统判断MPC是否已完成重置，避免在重置过程中执行其他操作

订阅示例:

# 使用rostopic查看
rostopic echo /humanoid_controller/resetting_mpc_state_

# 使用rqt_plot可视化
rqt_plot /humanoid_controller/resetting_mpc_state_/data

6.3 /humanoid_controller/transport_mode_state_​

话题类型: std_msgs/Float64

功能: 实时发布搬运模式状态

  • 0 = INACTIVE, 1 = INTERPOLATING, 2 = READY, 3 = ACTIVE, 4 = HANDING_OVER

6.4 /humanoid_controller/is_stance_mode_​

话题类型: std_msgs/Float64

功能: 实时发布 MPC stance 状态 (0.0/1.0)

6.5 /humanoid_controller/FallStandController/fall_stand_state_​

话题类型: std_msgs/Float64

功能: 实时发布 FallStand 阶段 (0=FALL_DOWN 1=INTERPOLATING 2=READY 3=STAND_UP 4=STANDING)

服务调用示例​

完整的控制器切换流程​

# 1. 查询可用的控制器列表
rosservice call /humanoid_controller/get_controller_list

# 2. 切换到AMP行走控制器
rosservice call /humanoid_controller/switch_controller "controller_name: 'amp_controller'"

# 3. 查询控制器状态
rosservice call /humanoid_controllers/amp_controller/getState
rosservice call /humanoid_controllers/amp_controller/isActive

# 4. 切回MPC控制器
rosservice call /humanoid_controller/switch_controller "controller_name: 'mpc'"

多舞蹈切换流程​

# 1. 查看已加载的舞蹈控制器名称（顺序与 rl_controllers.yaml 一致）
rosservice call /humanoid_controller/get_dance_controller_list

# 2. 进入第一个舞蹈（空 data）
rosservice call /humanoid_controller/switch_to_dance_controller "data: ''"

# 3. 切换到列表中的第二个舞蹈（若存在）
rosservice call /humanoid_controller/switch_to_dance_controller "data: '#1'"

# 4. 从舞蹈回到行走：须先满足 stance 等条件，再通过 switch_controller 切 amp 等
rosservice call /humanoid_controller/switch_controller "controller_name: 'amp_controller'"

倒地起身流程（推荐使用 fall_stand_command 接口）​

# 1. 设置倒地状态（会自动切换到倒地起身控制器）
rosservice call /humanoid_controller/set_fall_down_state "data: true"

# 2. 第一次触发：PREPARE（插值到起身初始姿态）
rosservice call /humanoid_controller/fall_stand_command "command: 1"

# 3. 等待插值完成（监控 fall_stand_state_ 从 1→2）
rostopic echo /humanoid_controller/FallStandController/fall_stand_state_

# 4. 第二次触发：STAND_UP（执行 RL 起身）
rosservice call /humanoid_controller/fall_stand_command "command: 2"

# 5. 等待起身完成后，设置站立状态
rosservice call /humanoid_controller/set_fall_down_state "data: false"

注意事项​

  1. 控制器切换保护机制:

    • 从RL切换到MPC时，RL控制器必须在stance状态
    • 从MPC切换到RL时，MPC必须在stance状态（倒地起身控制器除外）
    • 倒地起身控制器在未完成起身任务前，不允许切换到其他控制器
    • 舞蹈控制器：switch_controller 的行走列表不包含舞蹈；请使用 switch_to_dance_controller（SetString）。舞蹈 A → 舞蹈 B 允许直接切换；舞蹈 → MPC/行走 仍受上述 stance / requestToExit 等限制

  2. 控制器状态:

    • INITIALIZING: 控制器正在初始化，不能执行操作
    • RUNNING: 控制器正在运行，可以执行控制
    • PAUSED: 控制器已暂停，推理线程继续运行但不执行控制
    • STOPPED: 控制器已停止，推理线程已退出

  3. 服务命名空间:

    • 控制器管理服务：/humanoid_controller/*（包括行走切换、行走列表、舞蹈切换/舞蹈列表、倒地状态设置等）
    • 控制器基础服务：/humanoid_controllers/{controller_name}/*（每个RL控制器的独立服务）
    • 倒地起身服务：/humanoid_controller/fall_stand_command
浏览量 --