moveit_simple_controller_manager:MoveIt 2 通用简单控制器管理器插件的架构、配置与演进全解
moveit_simple_controller_manager:MoveIt 2 通用简单控制器管理器插件的架构、配置与演进全解
导读
本文以 MoveIt 2 仓库中 moveit_simple_controller_manager 包(变更日志、插件描述)为主线,系统讲解这个"简单但通用"的控制器管理器插件:它如何在 MoveIt 与底层 ROS 2 Action 控制器之间搭建桥梁,支持 FollowJointTrajectory、GripperCommand、ParallelGripperCommand 三种控制器类型,以及 controller_list 中每一个配置参数的含义与默认值。读完本文,你将能独立配置 moveit_controllers.yaml、理解插件内部执行流程,并掌握其从 ROS 1 时代到当前版本的核心演进脉络。
一、插件定位:MoveIt 的"控制器管理器"是什么
在 MoveIt 的架构中,move_group 负责规划,而规划结果(moveit_msgs/msg/RobotTrajectory)最终要交给真实的机器人控制器去执行。这一层"把轨迹交给谁、如何等待完成、如何取消"的抽象,由 moveit_controller_manager::MoveItControllerManager 基类定义,位于 controller_manager.hpp。
moveit_simple_controller_manager 正是该基类的一个 pluginlib 插件实现,仓库根目录的 moveit_plugins/README.md 将其概括为 "A very basic controller manager plugin. Can connect to FollowJointTrajectoryAction and GripperCommandAction servers."。它与 moveit_ros_control_interface(面向 ros_control 节点)相比,不依赖 ros_control 的服务轮询,而是直接通过 ROS 2 Action 客户端向控制器 Action Server 发送目标,因此适用于任何暴露了标准 Action 接口的控制器实现(包括仿真中的 fake controller)。
插件的注册信息位于 moveit_simple_controller_manager_plugin_description.xml:
<library path="moveit_simple_controller_manager">
<class name="moveit_simple_controller_manager/MoveItSimpleControllerManager"
type="moveit_simple_controller_manager::MoveItSimpleControllerManager"
base_class_type="moveit_controller_manager::MoveItControllerManager">
</class>
</library>
在 moveit_simple_controller_manager.cpp 末尾通过 PLUGINLIB_EXPORT_CLASS 完成导出。使用时在 MoveIt 启动文件中把 moveit_controller_manager 参数设为 moveit_simple_controller_manager/MoveItSimpleControllerManager 即可。
二、核心架构:ActionBasedControllerHandle 双层设计
从 CMakeLists.txt 可以看到,该插件由两个源文件编译为共享库 moveit_simple_controller_manager:
src/moveit_simple_controller_manager.cpp—— 管理器主体(参数解析、控制器注册);src/follow_joint_trajectory_controller_handle.cpp—— FollowJointTrajectory 句柄实现。
头文件布局(include/moveit_simple_controller_manager/)清晰地体现了类层次:
action_based_controller_handle.hpp # 模板基类:ActionBasedControllerHandleBase / ActionBasedControllerHandle<T>
follow_joint_trajectory_controller_handle.hpp # 机械臂轨迹
gripper_command_controller_handle.hpp # 单自由度夹爪
parallel_gripper_command_controller_handle.hpp # 并行夹爪(2.13.0 新增)
empty_controller_handle.hpp # 空句柄(链式控制器)
2.1 基类接口
moveit_controller_manager::MoveItControllerHandle(见 controller_manager.hpp)定义了一个控制器句柄必须实现的四个纯虚方法:
sendTrajectory():发送轨迹,不应阻塞,返回 false 表示控制器无法接受;cancelExecution():取消当前执行,无执行时为空操作返回 true;waitForExecution(timeout):阻塞等待执行完成,默认 timeout 为 -1 表示无限等待;getLastExecutionStatus():返回上次执行的ExecutionStatus(UNKNOWN / RUNNING / SUCCEEDED / PREEMPTED / TIMED_OUT / ABORTED / FAILED)。
2.2 模板基类 ActionBasedControllerHandle
action_based_controller_handle.hpp 定义了本包的核心抽象:
ActionBasedControllerHandleBase:非模板基类,注入addJoint()/getJoints()与logger_;ActionBasedControllerHandle<T>:模板基类,构造时通过rclcpp_action::create_client<T>(node, getActionName())创建 Action 客户端,getActionName()将name_与namespace_拼接为name/ns(ns 为空时直接使用name_),对应 0.5.1 版本中"ns 参数改为 action_ns"的演进。
模板基类还实现了通用的 cancelExecution()(调用 async_cancel_goal)与 waitForExecution()(通过 std::promise/std::future 等待结果回调,且支持 use_sim_time 模拟时间——即 2.7.0 版本引入的"Use emulated time in action-based controller")。finishControllerExecution() 将 rclcpp_action::ResultCode 映射为 MoveIt 的 ExecutionStatus(SUCCEEDED/ABORTED/CANCELED/UNKNOWN 分别映射为 SUCCEEDED/ABORTED/PREEMPTED/UNKNOWN,其余为 FAILED)。
三、配置详解:controller_list 与参数命名空间
管理器在 initialize()(moveit_simple_controller_manager.cpp)中完成全部参数解析。所有参数都以 moveit_simple_controller_manager 为顶层命名空间,逐级用 . 拼接,例如 moveit_simple_controller_manager.controller_names。
3.1 必需参数
| 参数 | 类型 | 说明 |
|---|---|---|
moveit_simple_controller_manager.controller_names |
string[] | 控制器名称列表,缺失时直接报错返回;类型非字符串数组时报错 |
<controller_name>.action_ns |
string | 控制器 Action 的命名空间,缺失则跳过该控制器 |
<controller_name>.type |
string | 控制器类型:FollowJointTrajectory / GripperCommand / ParallelGripperCommand,未知类型报错并跳过 |
<controller_name>.joints |
string[] | 控制器控制的关节列表,缺失或为空则跳过 |
3.2 可选参数
| 参数 | 适用类型 | 默认值 | 说明 |
|---|---|---|---|
default |
全部 | false |
标记默认控制器(写入 ControllerState.default_) |
command_joint |
GripperCommand / ParallelGripperCommand | joints[0] |
真正发送给夹爪命令的关节 |
parallel |
GripperCommand | false |
平行夹爪模式,要求恰好 2 个关节,命令取两关节之和 |
allow_failure |
GripperCommand | false |
允许"夹持失败"(见下文) |
max_effort |
GripperCommand / ParallelGripperCommand | 0.0 |
请求轨迹未指定 effort 时使用的最大力(2.5.0 版本引入) |
max_velocity |
ParallelGripperCommand | 0.0 |
并行夹爪最大速度(2.13.0 引入) |
3.3 真实配置示例
仓库中 prbt 测试配置给出了可直接对照的实例。controllers_manipulator.yaml:
controller_list:
- name: "prbt/manipulator_joint_trajectory_controller/"
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints: [prbt_joint_1, prbt_joint_2, prbt_joint_3, prbt_joint_4, prbt_joint_5, prbt_joint_6]
controllers_manipulator_gripper.yaml 则同时配置了机械臂与夹爪两个 FollowJointTrajectory 控制器;而 fake_controllers.yaml 展示了仿真场景下 type: $(arg execution_type) 的用法。
3.4 参数解析的源码细节
从源码看,管理器对每个控制器做了三项校验:必须有 action_ns、必须有 type、必须有非空的 joints,任一缺失都会打印错误并 continue 跳过。default 参数用 get_parameter_or(..., false) 读取;command_joint 缺省取 controller_joints<a href="https://link.gitcode.com/i/01496f8945dfe2e01cfd226c45d6db51" target="_blank">0]([第 170-174 行)。成功创建的控制器会以 controller_name 为键存入 controllers_ 与 controller_states_,并逐个把 joints 注入句柄。
四、三种控制器类型的执行流程
4.1 FollowJointTrajectory:机械臂主通道
FollowJointTrajectoryControllerHandle(头文件、实现)基于 control_msgs/action/FollowJointTrajectory。sendTrajectory() 的要点:
- 校验 Action 客户端存在且已连接(
action_server_is_ready()); - 将整个
RobotTrajectory的joint_trajectory与multi_dof_joint_trajectory都填入 goal(对应 2.3.0 版本 "publish new multi_dof_trajectory field"); - 通过
async_send_goal发送并同步等待 goal 响应(current_goal_future.get()),被拒绝则返回 false; - 发送后将
done_ = false、last_exec_ = RUNNING。
结果回调 controllerDoneCallback() 会把 error_code 翻译成可读文本(SUCCESSFUL、INVALID_GOAL、INVALID_JOINTS、OLD_HEADER_TIMESTAMP、PATH_TOLERANCE_VIOLATED、GOAL_TOLERANCE_VIOLATED),再调用 finishControllerExecution()。源码中还保留着被注释掉的 configure() 与 JointTolerance 解析逻辑(path_tolerance/goal_tolerance/goal_time_tolerance 及按关节的 position/velocity/acceleration 容差),标注 TODO(JafarAbdi): Revise parameter lookup——这是 ROS 1 时代遗留、当前版本尚未重新启用的参数能力。
4.2 GripperCommand:单自由度夹爪
GripperCommandControllerHandle(gripper_command_controller_handle.hpp)基于 control_msgs/action/GripperCommand,关键机制:
- 取最后一个轨迹点:
int tpoint = points.size() - 1;(对应 0.5.5 版本 "allow trajectories with >1 points, use the last point"); - command_joint 机制:
GripperCommand只有一个float64 command字段,因此用command_joints_集合匹配轨迹中的关节名,命中即取其位置;找不到时告警并假定索引 0; - 平行夹爪:
parallel_jaw_gripper_ = true时要求 2 个关节,命令值取两关节位置之和(URDF 中常见"两指各走一半距离"的建模); - max_effort:轨迹点带 effort 时用轨迹值,否则回退到配置的
max_effort_(第 141-148 行); - allow_failure:某些夹爪驱动在夹住物体无法完全闭合时会报 ABORTED,开启后把 ABORTED 视为 SUCCEEDED。
4.3 ParallelGripperCommand:2.13.0 新增的并行夹爪
ParallelGripperCommandControllerHandle(parallel_gripper_command_controller_handle.hpp)在 2.13.0 版本 引入(PR #3246),基于 control_msgs/action/ParallelGripperCommand。与 GripperCommand 不同,它只支持单关节命令:setCommandJoint(joints[0]),配置中给出多个关节时打印告警并假定控制第一个。max_velocity > 0.0 或 max_effort > 0.0 时才会把对应字段写入 goal。
4.4 EmptyControllerHandle:链式控制器占位
empty_controller_handle.hpp 是 2.6.0 版本 支持链式控制器(PR #1482)的产物:像 Admittance 控制器这类"挂在其他控制器下游"的控制器不需要也不支持直接发送轨迹,用空句柄占位(sendTrajectory 报错返回 false,waitForExecution 恒 true,getLastExecutionStatus 恒 FAILED)。
五、执行状态与等待语义
waitForExecution() 在 1.1.0 版本 引入 "wait for done-callback"(PR #1783),并在 2.1.1 版本 用 promise/future 替换了此前的 workaround(PR #323)。当前实现(action_based_controller_handle.hpp 第 127-168 行):
async_get_result注册回调,回调内先执行controllerDoneCallback再置位 promise;- timeout 为负时无限等待;否则按
use_sim_time分支:模拟时间下用 50ms 轮询 +node_->now()差值判断,真实时间下直接wait_for(timeout); - 结果 future 就绪后还要等待 promise,以覆盖"结果已到但回调尚未执行完"的窗口。
超时返回 false,并输出 waitForExecution timed out 警告。取消执行则由 cancelExecution() 调用 async_cancel_goal,并把状态置为 PREEMPTED。
六、关键演进脉络(2013 → 2026)
CHANGELOG.rst 完整记录了从 ROS 1 时代(0.1.0,2013-06-05)到当前(2.15.2,2026-09-16)的全部版本,其中最具技术意义的分水岭如下:
6.1 ROS 1 → ROS 2 移植
- 2.0.0(2020-02-17):正式移植到 ROS 2(PR #158),同步重构了参数查找逻辑并修复插件安装;
- 0.5.1:
ns参数更名为action_ns,去掉默认值——这一命名沿用至今。
6.2 参数能力持续增强
- 2.5.0:为 GripperCommand 增加
max_effort参数(PR #2984,issue #2956),仅在请求轨迹设置了 effort 时生效; - 2.12.0:修复夹爪控制器的参数命名空间问题(PR #3023);
- 2.13.0:并行夹爪控制器(PR #3246),配套新增
max_velocity参数。
6.3 工程化与生态协作
- 2.6.0:支持链式控制器(PR #1482),新增空句柄;CMake 改用标准导出 targets;
- 2.12.0:全部头文件迁移为
.hpp后缀(PR #3113),旧的.h由 create_deprecated_headers.py 自动生成并带弃用告警(见 gripper_controller_handle.h); - 2.15.0:移除
ament_target_dependencies(PR #3726),改为导出并链接命名空间 target(如moveit_core::moveit_core),下游需相应迁移; - 2.15.1:避免使用已废弃的
FindBoost.cmake模块——当前 CMakeLists.txt 通过ConfigExtras.cmake使用find_package(Boost CONFIG REQUIRED thread),并链接Boost::headers; - 2.15.2:修复 clang-tidy 问题。
6.4 健壮性与行为修正
- 1.0.3:
getControllerState()正确处理default参数; - 2.1.1:修复 ActionBasedControllerHandle 超时悬空引用;
- 2.3.1:对缺失或迟到的 ros2_control 节点更加宽容(PR #792);
- 2.7.0:基于模拟时间的等待(PR #899);修正 package.xml 为 SPDX 兼容的 BSD-3-Clause(PR #1796);
- 2.9.0:日志切换为 Node logging(PR #2599)——当前实现使用
moveit::getLogger("moveit.plugins.simple_controller_manager"); - 2.14.1:代码中
and全部改为&&(PR #3510)。
6.5 版本时间线速览
| 版本 | 时间 | 关键事件 |
|---|---|---|
| 0.1.0 | 2013-06 | 初始工作版本,拆分 follow/gripper 头文件 |
| 0.5.5 | 2013-09 | 轨迹取最后一点;夹爪 effort;更好错误报告 |
| 0.5.6 | 2014-03 | 允许忽略 virtual joints |
| 0.9.10 | 2017-12 | 可选无限等待控制器(#695) |
| 1.1.0 | 2020-09 | 等待 done-callback(#1783);不同控制器执行(#1832) |
| 2.0.0 | 2020-02 | 移植 ROS 2(#158) |
| 2.6.0 | 2022-11 | 链式控制器(#1482);标准导出 targets |
| 2.7.0 | 2023-01 | 模拟时间;BSD license |
| 2.12.0 | 2024-11 | hpp 头文件(#3113);参数命名空间修复(#3023) |
| 2.13.0 | 2025-02 | 并行夹爪控制器(#3246) |
| 2.15.0 | 2026-08 | 移除 ament_target_dependencies(#3726) |
| 2.15.2 | 2026-09 | clang-tidy 修复 |
七、与 moveit_ros_control_interface 的协作
在真实 ros_control 场景中,MoveIt 更常用的是 moveit_ros_control_interface 提供的 Ros2ControlManager/Ros2ControlMultiManager:它通过 list_controllers 服务(1Hz 轮询)感知 ros_control 节点上的控制器,而实际执行轨迹的句柄正是本包的 FollowJointTrajectoryControllerHandle(由 JointTrajectoryControllerAllocator 包装,见 joint_trajectory_controller_plugin.cpp)。moveit_simple_controller_manager 的句柄类因此成为 MoveIt 与 ros_control 之间共享的"执行基座",这也是其头文件被 moveit_ros_control_interface/CMakeLists.txt 依赖的原因。
八、构建、安装与使用
该包使用 ament_cmake 构建,从源码编译可在 MoveIt 2 工作空间中执行:
colcon build --packages-select moveit_simple_controller_manager
插件通过 CMakeLists.txt 的 pluginlib_export_plugin_description_file(moveit_core moveit_simple_controller_manager_plugin_description.xml) 导出,安装后头文件位于 include/moveit_simple_controller_manager/。使用时在 MoveIt 配置包的 moveit_controllers.yaml 中维护 controller_list(结构见第三节),并在启动文件中指定 moveit_controller_manager 参数为 moveit_simple_controller_manager/MoveItSimpleControllerManager。
九、工程质量实践
从 CHANGELOG 可以清晰看到该包的持续质量投入:clang-tidy 规则演进(#596、#1706、#2177、#999、#1949、#2177)、clang-format 版本升级(#367、1.0.6)、cppcheck(#1224)、codespell(#934)、CMake format/lint 进入 pre-commit(#2683)、Ubuntu 26.04 CI 修复(#3818)。这些条目说明 moveit_simple_controller_manager 虽定位"simple",但始终遵循 MoveIt 2 统一的代码规范与 CI 流水线,是理解 MoveIt 插件工程规范的理想样本。
结语
moveit_simple_controller_manager 是 MoveIt 2 中"约定优于配置"的典型插件:用一份 controller_list YAML 加三个 Action 类型,就覆盖了机械臂、单自由度夹爪、并行夹爪和链式控制器的主流执行场景。其 CHANGELOG 不只是版本流水账,更是一部浓缩的 ROS 2 移植与工程化演进史——从 ns 到 action_ns、从 ROS 1 参数服务到 ROS 2 参数命名空间、从 boost::bind 到 std::bind、从 ament_target_dependencies 到导出 targets,每一步都对应着 MoveIt 2 整体架构的演进。结合本文所列源码路径与配置示例,你可以直接在自己的 MoveIt 配置包中复现并验证全部行为。