ros2_control知识详解

原创已于 2025-06-29 00:42:18 修改 · 2.8k 阅读

30 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#c++ #机器人

于 2025-06-19 23:32:20 首次发布

ROS系列专栏收录该内容

2 篇文章

订阅专栏

ros2 control知识整理

因工作原因，不定时更新，希望对学习的各位有所帮助

1.核心框架

1.1基础概念

1.1.1控制器管理器（Controller Manager）

功能：管理控制器的加载、激活、停用和卸载，匹配控制器所需接口与硬件提供的接口，避免访问冲突。
控制循环：通过 update() 方法执行控制循环，读取硬件数据、更新控制器输出并写入硬件。

1.1.2 资源管理器（Resource Manager）

资源管理器（RM）为 ros2_control 框架抽象物理硬件及其驱动，通过 pluginlib 库加载硬件组件，管理其生命周期和接口（状态接口、命令接口）。

硬件通信：在控制循环中，通过 read() 和 write() 方法处理与硬件的通信。

1.1.3控制器（Controllers）

ros2_control 框架中的控制器，继承自 ControllerInterface，并通过 pluginlib 作为插件导出。

生命周期：基于 LifecycleNode 实现状态机（初始化、配置、激活等）。
控制循环：update() 方法可访问最新硬件状态并写入命令接口。

1.1.4用户接口（User Interfaces）

服务定义可在 controller_manager_msgs 包的 srv 文件夹中查看。此外，框架提供了与 ros2 cli 集成的命令行接口（CLI），基础命令为 ros2 control。

1.1.5硬件组件（Hardware Components）

硬件组件实现与物理硬件的通信，并在框架中抽象为三类：

系统（System）：复杂多自由度机器人硬件（如工业机器人），具备读写能力。
传感器（Sensor）：仅具备读取能力。
执行器（Actuator）：如电机、舵机等，用于执行控制指令，具备读写能力。

1.2系统架构图

请添加图片描述

1.3 URDF 中的硬件描述

ros2_control 框架通过机器人 URDF 文件中的 <ros2_control> 标签描述硬件配置，支持嵌套 xacro 宏。以下是一个示例（RRBot 机器人）：

<!-- 位置控制的 2 自由度系统 -->
<ros2_control name="RRBotSystemPositionOnly" type="system">
  <hardware>
    <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
    <param name="example_param_write_for_sec">2</param>
    <param name="example_param_read_for_sec">2</param>
  </hardware>
  <joint name="joint1">
    <command_interface name="position">
      <param name="min">-1</param>
      <param name="max">1</param>
    </command_interface>
    <state_interface name="position"/>
  </joint>
  <!-- joint2 配置类似 -->
</ros2_control>

<!-- 1 自由度力扭矩传感器 -->
<ros2_control name="RRBotForceTorqueSensor1D" type="sensor">
  <hardware>
    <plugin>ros2_control_demo_hardware/ForceTorqueSensor1DHardware</plugin>
  </hardware>
  <sensor name="tcp_fts_sensor">
    <state_interface name="force"/>
  </sensor>
</ros2_control>

<!-- 1 自由度夹持器执行器 -->
<ros2_control name="RRBotGripper" type="actuator">
  <hardware>
    <plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
  </hardware>
  <joint name="gripper_joint">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

2.功能组成

2.1 控制器管理器（Controller Manager）

ros2_control 框架的核心组件，负责管理控制器的生命周期、硬件接口的访问，并向 ROS 系统提供服务。

2.1.1配置参数

参考yaml文件

controller_manager:
  ros__parameters:
    defaults:
      switch_controller:
        strictness: best_effort  # 切换控制器的默认策略（"strict"严格模式/"best_effort"尽力模式），当服务调用未指定策略时生效
    diagnostics:
      threshold:
        controller_manager:
          periodicity:
            mean_error:
              error: 10.0  # 控制器管理器周期平均误差的错误阈值（Hz），超过时发布错误诊断
              warn: 5.0   # 控制器管理器周期平均误差的警告阈值（Hz），超过时发布警告诊断
            standard_deviation:
              error: 10.0  # 控制器管理器周期标准差的错误阈值（Hz），反映周期稳定性
              warn: 5.0   # 控制器管理器周期标准差的警告阈值（Hz）
        controllers:
          execution_time:
            mean_error:
              error: 2000.0  # 控制器更新执行时间平均误差的错误阈值（微秒），异步控制器对比目标周期，同步控制器对比0
              warn: 1000.0  # 控制器更新执行时间平均误差的警告阈值（微秒）
            standard_deviation:
              error: 200.0   # 控制器执行时间标准差的错误阈值（微秒），反映执行时间波动
              warn: 100.0    # 控制器执行时间标准差的警告阈值（微秒）
          periodicity:
            mean_error:
              error: 10.0    # 控制器更新周期平均误差的错误阈值（Hz），异步控制器专用
              warn: 5.0     # 控制器更新周期平均误差的警告阈值（Hz）
            standard_deviation:
              error: 10.0    # 控制器更新周期标准差的错误阈值（Hz）
              warn: 5.0     # 控制器更新周期标准差的警告阈值（Hz）
        hardware_components:
          execution_time:
            mean_error:
              error: 2000.0  # 硬件组件读写执行时间平均误差的错误阈值（微秒）
              warn: 1000.0  # 硬件组件读写执行时间平均误差的警告阈值（微秒）
            standard_deviation:
              error: 200.0   # 硬件组件执行时间标准差的错误阈值（微秒）
              warn: 100.0    # 硬件组件执行时间标准差的警告阈值（微秒）
          periodicity:
            mean_error:
              error: 10.0    # 硬件组件读写周期平均误差的错误阈值（Hz），异步硬件专用
              warn: 5.0     # 硬件组件读写周期平均误差的警告阈值（Hz）
            standard_deviation:
              error: 10.0    # 硬件组件读写周期标准差的错误阈值（Hz）
              warn: 5.0     # 硬件组件读写周期标准差的警告阈值（Hz）
    enforce_command_limits: true  # 是否强制应用URDF中定义的命令限制（如关节位置上下限），超出时自动截断命令
    hardware_components_initial_state:
      inactive: '{}'              # 启动时已配置但未激活的硬件组件列表，需手动激活（默认空列表）
      shutdown_on_initial_state_failure: false  # 启动时硬件状态设置失败是否关闭控制器管理器（默认true）
      unconfigured: '{}'          # 启动时仅加载未配置的硬件组件列表，需手动配置（默认空列表）
    update_rate: 100.0            # 控制器管理器主循环频率（Hz），决定硬件读写和控制器更新的周期

2.1.2启动方式

2.1.2.1 通过launch启动

control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[robot_controllers],
    output="both",
)

具体配置参数参考官方文档

2.1.2.2 在进程中使用 Controller Manager

auto options = controller_manager::get_cm_node_options();
  options.arguments({
    "--ros-args",
    "--remap", "_target_node_name:__node:=dst_node_name",
    "--log-level", "info"});

  auto cm = std::make_shared<controller_manager::ControllerManager>(
    executor, "_target_node_name", "some_optional_namespace", options);

2.1.3异步更新

在一个周期内，推测Controller Manager会轮流执行所有的控制器，如果某个控制器的执行时间过长，会影响整体的执行频率。
例如，如果控制器管理器的执行频率为 100Hz，则所有控制器的调用和硬件组件和调用的执行时间之和必须小于 10ms。如果一个控制器需要 15ms 的执行时间，则无法在不影响整体系统更新率的情况下同步执行。这时候可以使用异步更新的方式

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    ...

example_async_controller:
  ros__parameters:
    type: example_controller/ExampleAsyncController
    is_async: true
    update_rate: 20  # Hz
    ...

设置为控制器管理器充当调度程序，在控制循环期间触发异步控制器的更新。如果更新需要很长时间，并且在上一个更新仍在运行时触发另一个更新，则将使用上一个更新的结果。发生这种情况时，终端会提示用户降低 controller 的频率，因为计算花费的时间比最初估计的要长，类似如下：

[ros2_control_node-1] [WARN] [1741626670.311533972] [example_async_controller]: The controller missed xx update cycles out of yy total triggers.

2.1.4 控制器链式连接 / 级联控制

个人认为这部分比较华而不实，会增加系统的复杂性和项目的工作量，真有需要，直接看官网文档。这里只留一张应用场景的例子：

请添加图片描述

2.3 硬件组件（Hardware Components）

2.31 硬件组件类型

执行器（Actuator）：如电机、舵机等，用于执行控制指令。
传感器（Sensor）：如编码器、力传感器等，用于采集硬件状态数据。
系统（System）：代表整体硬件系统，可能包含多个执行器和传感器的组合。

2.32 硬件组件生命周期与状态转换

硬件组件通过生命周期管理（Lifecycle Manager）其状态：

未配置状态（UNCONFIGURED）
- 触发方法：on_init()（初始化）、on_cleanup()（清理）。
- 特征：仅完成初始化，未启动通信，未向资源管理器（ResourceManager）导入接口。
非活跃状态（INACTIVE）
- 触发方法：on_configure()（配置）、on_deactivate()（停用）。
- 特征：已建立硬件通信并完成配置，可读取状态，系统和执行器的命令接口可用（当前需组件自行决定是否处理命令）。
最终状态（FINALIZED）
- 触发方法：on_shutdown()（关闭）。
- 特征：硬件接口准备卸载或销毁，已释放分配的内存。
活跃状态（ACTIVE）
- 触发方法：on_activate()（激活）。
- 特征：可读取状态

2.33 返回值类型

所有生命周期方法返回值类型有以下几种
返回值类型：rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

CallbaetckRurn::SUCCESS：方法执行成功。
CallbackReturn::FAILURE：方法执行失败，生命周期转换未成功。
CallbackReturn::ERROR：发生严重错误，需在 on_error 方法中处理。

2.34 硬件接口类型

注意区别于硬件组件，硬件接口属于硬件组件

关节接口（Joints）
传感器接口（Sensors）
IO 接口（GPIO）

接口类型	命令接口	状态接口
Joints	✔️	✔️
Sensors	❌	✔️
GPIO	✔️（可选）	✔️（可选）

2.35示例

更多示例参考官方文档

<ros2_control name="RRBotSystemMutipleGPIOs" type="system">
  <hardware>
    <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
    <param name="example_param_hw_start_duration_sec">2.0</param>
    <param name="example_param_hw_stop_duration_sec">3.0</param>
    <param name="example_param_hw_slowdown">2.0</param>
  </hardware>
  <joint name="joint1">
    <command_interface name="position">
      <param name="min">-1</param>
      <param name="max">1</param>
    </command_interface>
    <state_interface name="position"/>
  </joint>
  <joint name="joint2">
    <command_interface name="position">
      <param name="min">-1</param>
      <param name="max">1</param>
    </command_interface>
    <state_interface name="position"/>
  </joint>
  <gpio name="flange_digital_IOs">
    <command_interface name="digital_output1"/>
    <state_interface name="digital_output1"/>    <!-- Needed to know current state of the output -->
    <command_interface name="digital_output2"/>
    <state_interface name="digital_output2"/>
    <state_interface name="digital_input1"/>
    <state_interface name="digital_input2"/>
  </gpio>
  <gpio name="flange_analog_IOs">
    <command_interface name="analog_output1"/>
    <state_interface name="analog_output1">    <!-- Needed to know current state of the output -->
      <param name="initial_value">3.1</param>  <!-- Optional initial value for mock_hardware -->
    </state_interface>
    <state_interface name="analog_input1"/>
    <state_interface name="analog_input2"/>
  </gpio>
  <gpio name="flange_vacuum">
    <command_interface name="vacuum"/>
    <state_interface name="vacuum"/>    <!-- Needed to know current state of the output -->
  </gpio>
</ros2_control>

3.硬件组件功能包

3.1 创建功能包

ros2 pkg create --build-type ament_cmake mypkg --dependencies hardware_interface pluginlib rclcpp rclcpp_lifecycle

3.2 编写头文件（hpp）

继承硬件接口基类（如 public hardware_interface::ActuatorInterface）。
声明生命周期方法：on_configure、on_activate、on_deactivate 等。
声明接口导出方法：export_state_interfaces（必选）、export_command_interfaces（传感器无）。
声明数据读写方法：read、write。

3.3 实现源文件（cpp）

on_init：初始化成员变量，处理参数（需先调用基类 on_init）。
on_configure：建立硬件通信，完成激活前配置。
on_activate：启用硬件电源（如松开刹车）。
on_deactivate：关闭硬件电源，与 on_activate 互逆。
on_shutdown：优雅关闭硬件，释放资源。

3.3.1 接口导出方法

export_state_interfaces：定义状态接口（格式：joint_name/interface_type）

3.3.2 数据交互方法

read：从硬件读取状态，存入内部变量。
write：根据命令接口变量向硬件发送指令

3.3.3 导出成组件

结尾添加 PLUGINLIB_EXPORT_CLASS 宏，格式为：

PLUGINLIB_EXPORT_CLASS(my_package::RobotHardware,hardware_interface::ActuatorInterface)

3.4配置 pluginlib 导出

3.4.1创建插件描述文件：

在包中创建 <包名>.xml，示例内容:
插件名称格式：包名/类名（如 my_package/RobotHardware）。

<library path="lib/libmy_hardware_interface.so">
  <class type="my_package::RobotHardware"base_class_type="hardware_interface::ActuatorInterface">
	 <description>My Robot Hardware Interface</description>
  </class>
</library>

4. 如何为硬件组件设置不同更新速率

4.1 循环计数法

通过主循环迭代次数的模运算控制硬件更新频率，适用于主循环速率固定的场景。

4.1.1 URDF 参数配置

使用 xacro 宏定义主循环速率和硬件期望速率：

<xacro:macro name="system_interface" params="name main_loop_update_rate desired_hw_update_rate">
  <ros2_control name="${name}" type="system">
    <hardware>
      <plugin>my_system_interface/MySystemHardware</plugin>
      <param name="main_loop_update_rate">${main_loop_update_rate}</param>
      <param name="desired_hw_update_rate">${desired_hw_update_rate}</param>
    </hardware>
  </ros2_control>
</xacro:macro>

4.1.2在 on_init 中获取参数

namespace my_system_interface {
CallbackReturn MySystemHardware::on_init(const HardwareInfo& info) {
  if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
    return CallbackReturn::ERROR;
  
  main_loop_update_rate_ = stoi(info_.hardware_parameters["main_loop_update_rate"]);
  desired_hw_update_rate_ = stoi(info_.hardware_parameters["desired_hw_update_rate"]);
  // 声明变量：unsigned int main_loop_update_rate_, desired_hw_update_rate_ = 100;
  return CallbackReturn::SUCCESS;
}
}

4.1.3在 on_activate 中重置计数器

return_type MySystemHardware::read(const Time&, const Duration&) {
  bool hardware_go = main_loop_update_rate_ == 0 || 
                     desired_hw_update_rate_ == 0 || 
                     ((update_loop_counter_ % desired_hw_update_rate_) == 0);
  if (hardware_go) {
    // 执行硬件通信
  }
  ++update_loop_counter_;
  update_loop_counter_ %= main_loop_update_rate_;
  return return_type::SUCCESS;
}

4.2 时间间隔法

通过记录时间戳并比较间隔控制更新频率，如果主循环每次循环的时间波动比较大，可以使用这个方法

4.2.1 URDF 参数配置

使用 xacro 宏定义主循环速率和硬件期望速率：

<param name="desired_hw_update_period">0.1</param>  <!-- 100ms -->

4.2.2 在 on_activate 中重置计数器

CallbackReturn MySystemHardware::on_activate(const State&) {
  first_read_pass_ = first_write_pass_ = true; // 声明：bool first_read_pass_, first_write_pass_ = true;
  return CallbackReturn::SUCCESS;
}

4.2.3 在 read/write 中实现时间判断

return_type MySystemHardware::read(const Time& time, const Duration&) {
  if (first_read_pass_ || (time - last_read_time_) > desired_hw_update_period_) {
    first_read_pass_ = false;
    last_read_time_ = time; // 声明：Time last_read_time_;
    // 执行硬件通信
  }
  return return_type::SUCCESS;
}

return_type MySystemHardware::write(const Time& time, const Duration&) {
  if (first_write_pass_ || (time - last_write_time_) > desired_hw_update_period_) {
    first_write_pass_ = false;
    last_write_time_ = time; // 声明：Time last_write_time_;
    // 执行硬件通信
  }
  return return_type::SUCCESS;
}