Controller Manager（控制器管理器）

Controller Manager 是 ros2_control 框架中的核心组件。它负责管理控制器的生命周期、硬件接口的访问，并向 ROS 世界提供服务。

---

确定性（Determinism）

为了在控制硬件时获得最佳性能，您希望 Controller Manager 的主控制循环尽可能减少抖动（jitter）。

无论是否安装了实时内核，Controller Manager 的主线程都会尝试将调度策略设置为 SCHED_FIFO，优先级设为 50。有关调度策略的更多信息，请参阅 Linux 调度策略文档。

对于实时任务，优先级范围为 0 到 99，数值越大优先级越高。默认情况下，普通用户没有权限设置如此高的优先级。要授予用户此类权限，请执行以下步骤：

sudo addgroup realtime
sudo usermod -a -G realtime $(whoami)

然后，在 /etc/security/limits.conf 中为 realtime 组添加以下限制：

@realtime soft rtprio 99
@realtime soft priority 99
@realtime soft memlock unlimited
@realtime hard rtprio 99
@realtime hard priority 99
@realtime hard memlock unlimited

注意：这些限制在您注销并重新登录后才会生效。

在 Docker 容器中运行带实时要求的 ros2_control

若要在 Docker 容器中运行具备实时能力的 ros2_control，请添加以下能力选项，以允许容器设置线程优先级并锁定内存：

docker run -it \
    --cap-add=sys_nice \
    --ulimit rtprio=99 \
    --ulimit memlock=-1 \
    --rm --net host <IMAGE>

更多详情请参考 Docker 引擎文档：资源限制与 Linux 能力（capabilities）。

内核选择建议

标准 Linux 内核针对计算吞吐量进行了优化，不适合硬件控制。建议考虑以下替代方案：

• Real-time Ubuntu（适用于 Ubuntu，包括 Raspberry Pi）
• Debian 上的 linux-image-rt-amd64 或 linux-image-rt-arm64（适用于 64 位 PC）
• lowlatency 内核（在 Ubuntu 上通过 sudo apt install linux-lowlatency 安装）

虽然安装实时内核（realtime kernel）能获得最低抖动的最佳效果，但使用 lowlatency 内核也能显著改善性能，且安装非常简单。

---

发布者（Publishers）

• ~/activity
  类型：controller_manager_msgs::msg::ControllerManagerActivity
  每当 Controller Manager 管理的控制器或硬件组件状态发生变化时，都会发布此话题。消息包含所有被管理的控制器和硬件组件及其生命周期状态。
  QoS 设置：使用 transient local，因此订阅者也必须使用 transient local QoS。

---

订阅者（Subscribers）

• robot_description
  类型：std_msgs::msg::String
  包含 URDF XML 字符串（例如来自 robot_state_publisher）。
  注意：目前不支持重新加载 URDF。所有在 <ros2_control> 标签中定义的关节都必须存在于 URDF 中。

---

参数（Parameters）

控制器定义

• <controller_name>.type
  使用 pluginlib 导出的控制器插件名称。Controller Manager 将基于此类创建名为 controller_name 的实例。

• <controller_name>.params_file
  控制器参数 YAML 文件的绝对路径。文件应使用标准 ROS 2 YAML 格式。

• <controller_name>.fallback_controllers
  当主控制器在更新周期中返回 return_type::ERROR 时，将激活此列表中的备用控制器。建议将所有支持备用策略所需的控制器（包括链式控制器）全部列入。

⚠️ 警告：备用控制器的激活依赖于当时状态接口和命令接口的可用性。强烈建议在仿真环境中测试备用策略后再部署到真实机器人上。

---

全局参数

• update_rate (int)
  Controller Manager 实时更新循环的频率（Hz）。该循环从硬件读取状态、更新控制器并向硬件写入命令。
• 只读：是

• 默认值：100

• enforce_command_limits (bool)
  若为 true，Controller Manager 会强制执行 URDF 中定义的命令限制；若命令超出限制，则根据关节类型进行钳位（clamp）。

• 只读：是
• 默认值：true

---

硬件组件初始状态管理

• hardware_components_initial_state（映射结构）

hardware_components_initial_state:
  unconfigured:
    - "arm1"
    - "arm2"
  inactive:
    - "base3"

• unconfigured（string_array）
  启动时仅加载但不配置的硬件组件，需手动或通过 hardware_spawner 配置和激活。

  • 默认：{}
  • 约束：无重复项

• inactive（string_array）
  启动时自动配置但不激活的硬件组件，需手动或通过 hardware_spawner 激活。

  • 默认：{}
  • 约束：无重复项

• shutdown_on_initial_state_failure（bool）
  若设置初始状态失败，Controller Manager 是否应关闭。

  • 只读：是
  • 默认值：false

---

默认行为参数

• defaults.switch_controller.strictness（string）
  默认的控制器切换策略。若服务调用未指定策略，则使用此值。
• 默认值："strict"

• 约束：必须为 "strict" 或 "best_effort"

• defaults.allow_controller_activation_with_inactive_hardware（bool）
  若为 true，允许控制器从未激活的硬件组件申请资源。

• 只读：是

• 默认值：false

  ⚠️ 出于安全考虑，不建议设为 true（仅用于向后兼容）。

• defaults.deactivate_controllers_on_hardware_self_deactivate（bool）
  若硬件组件在写入周期返回 DEACTIVATE，是否自动停用使用其接口的控制器。

• 只读：是
• 默认值：true

  ⚠️ 不建议设为 false，未来版本将移除此参数并强制启用此行为。

---

诊断阈值（Diagnostics Thresholds）

用于监控 Controller Manager、控制器和硬件组件的周期性和执行时间性能。

Controller Manager 周期性诊断

参数	默认值	说明
diagnostics.threshold.controller_manager.periodicity.mean_error.warn	5.0 Hz	均值误差警告阈值
diagnostics.threshold.controller_manager.periodicity.mean_error.error	10.0 Hz	均值误差错误阈值
diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn	5.0 Hz	标准差警告阈值
diagnostics.threshold.controller_manager.periodicity.standard_deviation.error	10.0 Hz	标准差错误阈值

控制器诊断（适用于异步控制器或更新率不同的同步控制器）

• 周期性（单位：Hz）
• mean_error.warn / error: 5.0 / 10.0

• standard_deviation.warn / error: 5.0 / 10.0

• 执行时间（单位：微秒）

• mean_error.warn / error: 1000.0 / 2000.0
• standard_deviation.warn / error: 100.0 / 200.0

硬件组件诊断（适用于异步硬件或读写率不同的同步硬件）

• 周期性（单位：Hz）
• mean_error.warn / error: 5.0 / 10.0

• standard_deviation.warn / error: 5.0 / 10.0

• 执行时间（单位：微秒）

• mean_error.warn / error: 1000.0 / 2000.0
• standard_deviation.warn / error: 100.0 / 200.0

---

其他参数

• overruns.print_warnings（bool）
  若为 true，当实时循环（读-更新-写）发生超限时，Controller Manager 会打印警告。
• 默认值：true（除非 use_sim_time=true）

---

示例参数文件

controller_manager:
  ros__parameters:
    defaults:
      allow_controller_activation_with_inactive_hardware: false
      deactivate_controllers_on_hardware_self_deactivate: true
      switch_controller:
        strictness: strict
    diagnostics:
      threshold:
        controller_manager:
          periodicity:
            mean_error:
              error: 10.0
              warn: 5.0
            standard_deviation:
              error: 10.0
              warn: 5.0
        controllers:
          execution_time:
            mean_error:
              error: 2000.0
              warn: 1000.0
            standard_deviation:
              error: 200.0
              warn: 100.0
          periodicity:
            mean_error:
              error: 10.0
              warn: 5.0
            standard_deviation:
              error: 10.0
              warn: 5.0
        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
              warn: 5.0
            standard_deviation:
              error: 10.0
              warn: 5.0
    enforce_command_limits: true
    hardware_components_initial_state:
      inactive: '{}'
      shutdown_on_initial_state_failure: false
      unconfigured: '{}'
    overruns:
      print_warnings: ''
    update_rate: 100.0

---

多 Controller Manager 管理

处理多个 Controller Manager 时，有两种方式管理不同的机器人描述：

1. 使用命名空间（Namespaces）

将 robot_state_publisher 和 controller_manager 放入同一命名空间：

control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[robot_controllers],
    output="both",
    namespace="rrbot",
)
robot_state_pub_node = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    output="both",
    parameters=[robot_description],
    namespace="rrbot",
)

2. 使用话题重映射（Remappings）

通过重映射指定不同 robot_description 话题：

control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[robot_controllers],
    output="both",
    remappings=[('robot_description', '/rrbot/robot_description')]
)
robot_state_pub_node = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    output="both",
    parameters=[robot_description],
    namespace="rrbot",
)

---

辅助脚本（Helper Scripts）

spawner

加载、配置并启动控制器。

ros2 run controller_manager spawner -h

常用选项： - -c：指定 Controller Manager 节点名 - -p：加载控制器参数文件 - --load-only：仅加载，不配置 - --inactive：加载并配置，但不激活 - -u：程序中断时自动卸载控制器 - --activate-as-group：批量激活控制器（适用于链式控制器）

支持多种参数文件格式（含通配符和命名空间）：

/**:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController

/rrbot_1/position_trajectory_controller:
  ros__parameters:
    type: ...

unspawner

停止并卸载控制器。

ros2 run controller_manager unspawner controller_name

hardware_spawner

激活或配置硬件组件。

ros2 run controller_manager hardware_spawner --activate hardware_name

---

GUI 工具：rqt_controller_manager

提供图形界面管理控制器和硬件组件的生命周期状态。

ros2 run rqt_controller_manager rqt_controller_manager

• 双击：查看详细信息
• 右键：生命周期管理菜单（激活、停用等）

---

在进程中实例化 ControllerManager

ControllerManager 可作为类在进程中实例化，但需注意：

• 它继承自 rclcpp::Node
• 若进程中有多个 Node，全局重映射规则可能导致节点名冲突 解决方案：在 NodeOptions 中显式指定重映射规则：

  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);
  

---

使用 ros2_control_node 启动 ControllerManager

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

ros2_control_node 特有参数

参数	类型	默认值	说明
lock_memory	bool	非实时内核: false 实时内核: true	锁定内存防止换页
cpu_affinity	int 或 int[]	-	绑定到指定 CPU 核心
thread_priority	int	50	线程优先级（0–99）
use_sim_time	bool	false	启用仿真时间
overruns.manage	bool	true	启用超限检测（仿真时间下自动禁用）

---

核心概念

重启所有控制器

使用 switch_controllers 服务或 CLI，将所有控制器加入停止和启动列表（注意：广播器等无需重启）。

重启硬件

硬件重启后，需重新执行其生命周期（配置 → 激活）以重新导出接口。

硬件与控制器错误处理

• 硬件 read/write 返回 ERROR → 停用所有使用其接口的控制器
• 控制器 update 返回 ERROR → 停用该控制器（或整个链），并尝试激活备用控制器

---

影响确定性的因素

即使配置了实时环境，以下操作仍可能引入抖动：

• 控制器在实时循环中激活失败 → 调用 prepare_command_mode_switch 等方法
• 控制器更新失败 → 触发接口切换逻辑

---

异步更新支持（Asynchronous Updates）

适用于更新耗时较长的控制器（例如 15ms > 10ms @100Hz）。

在控制器参数中设置：

example_async_controller:
  ros__parameters:
    type: example_controller/ExampleAsyncController
    is_async: true
    update_rate: 20  # Hz
    async_parameters:
      cpu_affinity: [2, 4]
      thread_priority: 50
      wait_until_initial_trigger: false

• Controller Manager 每 10ms 触发一次（100Hz）
• 异步控制器每 50ms 执行一次（20Hz）
• 若更新未完成，将复用上次结果，并打印警告：

[WARN] The controller missed xx update cycles out of yy total triggers.

• 若异步线程抛出异常，Controller Manager 会停用该控制器并报错。

---

监控与调优

• 使用 ControllerUpdateStats 结构监控更新率和丢帧数
• 数据发布至 /diagnostics 话题，可用于优化控制器频率

---

Controller Manager 使用的时钟

• 非仿真环境：
• RCL_STEADY_TIME：用于实时循环（读/更新/写），单调递增
• RCL_ROS_TIME：用于非实时操作（如服务调用）
• 仿真环境：全部使用 RCL_ROS_TIME

硬件接口的 read/write 接收 RCL_STEADY_TIME（避免系统时间跳变影响）
控制器 update 接收 RCL_ROS_TIME（便于与其它节点时间同步）

Controller Manager 会检测因系统时间变化或执行超时导致的更新丢失，并打印警告。

---

颜色输出处理

辅助脚本（spawner、hardware_spawner）支持环境感知的颜色输出：

• RCUTILS_COLORIZED_OUTPUT=0：禁用颜色
• RCUTILS_COLORIZED_OUTPUT=1：强制启用颜色
• 未设置：自动检测 TTY，仅在交互式终端启用颜色