一、ROS2 Control程序架构[1]
通过urdf/xacro建立底盘模型之后需要对底盘的关节进行控制，ROS2 Control提供了控制流程，如下所示：
1.Controller Manager控制器管理器
控制器管理器 (CM) 连接 ros2_control 框架的控制器和硬件抽象端。 它还作为用户通过 ROS 服务的入口点。 CM 实现了一个没有执行程序的节点，因此它可以集成到自定义设置中。 尽管如此，对于标准用户，建议使用 controller_manager 包中的 ros2_control_node 文件中实现的默认节点设置。 本手册假定您使用此默认节点设置。
一方面，CM 管理（例如，加载、激活、停用、卸载）控制器以及它们所需的接口。 另一方面，它可以访问硬件组件（通过资源管理器），即它们的接口。 控制器管理器匹配所需和提供的接口，在激活时为控制器提供对硬件的访问权限，或者在存在访问冲突时报告错误。
控制循环的执行由 CM 的 update() 方法管理。 该方法从硬件组件读取数据，更新所有活动控制器的输出，并将结果写入组件。
2.Resource Manager资源管理器
资源管理器 (RM) 为 ros2_control 框架抽象物理硬件及其驱动程序（称为硬件组件）。 RM 使用 pluginlib-library 加载组件，管理它们的生命周期和组件的状态和命令接口。 RM 提供的这种抽象实现了已实现硬件组件的可重用性，例如机器人和夹持器，无需任何实现和灵活的硬件应用程序即可用于状态和命令接口，例如用于电机控制和编码器读取的单独硬件/通信库。
在控制循环执行中，RM 的 read() 和 write() 方法处理与硬件组件的通信。
3.Controllers控制器
ros2_control 框架中的控制器具有与控制理论中定义的相同的功能。 他们将参考值与测量输出进行比较，并根据此误差计算系统的输入（有关更多详细信息，请访问维基百科）。 控制器是从 ControllerInterface（ros2_control 中的 controller_interface 包）派生的对象，并使用 pluginlib-library 作为插件导出。 例如，ros2_controllers 存储库中的控制器检查 ForwardCommandController 实现。 控制器的生命周期基于实现节点生命周期设计文档中描述的状态机的 LifecycleNode-Class。
执行控制循环时调用 update() 方法。 该方法可以访问最新的硬件状态并使控制器能够编写硬件的命令接口。
4.User Interfaces用户界面
用户使用 Controller Manager 的服务与 ros2_control 框架交互。 有关服务及其定义的列表，请查看 controller_manager_msgs 包中的 srv 文件夹。
虽然可以直接从命令行或通过节点使用服务调用，但存在一个与 ros2 cli 集成的用户友好的命令行界面 (CLI)。 这支持自动完成并具有一系列可用的常用命令。 基本命令是ros2控制。 有关我们的 CLI 功能的描述，请参阅 ros2controlcli 包的 README.md 文件。
二、Hardware Components硬件组件
硬件组件实现与物理硬件的通信，将其抽象表示在ros2_control框架中。 必须使用 pluginlib-library 将组件导出为插件。 资源管理器动态加载这些插件并管理它们的生命周期。
组件分为三种基本类型：
System系统
复杂（多自由度）机器人硬件，如工业机器人。 Actuator 组件之间的主要区别在于可以使用复杂的传动装置，就像人形机器人的手所需要的那样。 该组件具有读写功能。 当只有一个到硬件的逻辑通信通道时使用它（例如，KUKA-RSI）。
Sensor传感器
机器人硬件用于感知其环境。 传感器组件与关节（例如编码器）或连杆（例如力-扭矩传感器）相关。 此组件类型只有读取功能。
Actuator执行器
简单的 (1 DOF) 机器人硬件，如电机、阀门等。 执行器实现仅与一个关节相关。 此组件类型具有读写功能。 如果不可能，则不强制阅读（例如，使用 Arduino 板的直流电机控制）。 如果其硬件支持模块化设计，例如与每个电机独立进行 CAN 通信，则执行器类型也可用于多自由度机器人。
1. Hardware Description in URDF
ros2_control 框架使用机器人 URDF 文件中的 <ros2_control> 标签来描述其组件，即硬件设置。 所选择的结构可以将多个 xacro-macros 一起跟踪为一个，而无需任何更改。 下面的示例显示了一个具有 2-DOF (RRBot)、外部 1-DOF 力扭矩传感器和外部控制的 1-DOF 平行夹持器作为其末端执行器的位置控制机器人。
<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>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
</ros2_control>
<ros2_control name="RRBotForceTorqueSensor1D" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/ForceTorqueSensor1DHardware</plugin>
<param name="example_param_read_for_sec">0.43</param>
</hardware>
<sensor name="tcp_fts_sensor">
<state_interface name="force"/>
<param name="frame_id">rrbot_tcp</param>
<param name="min_force">-100</param>
<param name="max_force">100</param>
</sensor>
</ros2_control>
<ros2_control name="RRBotGripper" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="gripper_joint ">
<command_interface name="position">
<param name="min">0</param>
<param name="max">50</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
2.Running the Framework for Your Robot
要运行 ros2_control 框架，请执行以下操作。 示例文件可以在 ros2_control_demos 存储库中找到。
(1)使用控制器管理器和两个控制器的配置创建一个 YAML 文件。 （RRBot 的示例配置）
(2)使用所需的 <ros2_control> 标签扩展机器人的 URDF 描述。 建议使用宏文件 (xacro) 而不是纯 URDF。 （RRBot 的 URDF 示例）
(3)创建启动文件以使用 Controller Manager 启动节点。 您可以使用默认的 ros2_control 节点（推荐）或将控制器管理器集成到您的软件堆栈中。
三、Controller Manager控制器管理器
Controller Manager是ros2_control框架中的主要组件。 它管理控制器的生命周期、对硬件接口的访问并为 ROS 世界提供服务。
为了在控制硬件时获得最佳性能，您希望控制器管理器在主控制循环中具有尽可能小的抖动。 普通的 linux 内核针对计算吞吐量进行了优化，因此不太适合硬件控制。 两个最简单的内核选项是实时 Ubuntu 22.04 LTS Beta 或 Debian Bullseye 上的 linux-image-rt-amd64。
如果安装了实时内核，控制器管理器的主线程会尝试将 SCHED_FIFO 的优先级配置为 50。默认情况下，用户无权设置如此高的优先级。 要授予用户此类权限，请添加一个名为 realtime 的组并将控制您的机器人的用户添加到该组：
sudo addgroup realtime
sudo usermod -a -G realtime $(whoami)
之后，在 /etc/security/limits.conf 中的实时组中添加以下限制：
@realtime soft rtprio 99
@realtime soft priority 99
@realtime soft memlock 102400
@realtime hard rtprio 99
@realtime hard priority 99
@realtime hard memlock 102400
这些限制将在您注销并再次登录后应用。
1.Parameters参数
(1)activate_components_on_start (optional; list<string>; default: empty)
定义启动控制器管理器时应激活哪些硬件组件。
组件的名称定义为 robot_description 中 <ros2_control> 标签的属性。 所有其他组件将保持UNCONFIGURED。 如果 this 和 configure_components_on_start 为空，则将激活所有可用组件。 如果这个或 configure_components_on_start 不为空，则任何不在任一列表中的组件都将处于未配置状态。
(2)configure_components_on_start (optional; list<string>; default: empty)
定义启动控制器管理器时应配置哪些硬件组件。
组件的名称定义为 robot_description 中 <ros2_control> 标签的属性。 所有其他组件将保持UNCONFIGURED。 如果 this 和 configure_components_on_start 为空，则将激活所有可用组件。 如果这个或 configure_components_on_start 不为空，则任何不在任一列表中的组件都将处于未配置状态。
(3)robot_description (mandatory; string)
以 URDF 字符串作为机器人描述的字符串。 这通常是 xacro 命令解析描述文件的结果。
(4)update_rate (mandatory; integer)
控制器管理器实时更新循环的频率。 此循环从硬件读取状态、更新控制器并将命令写入硬件。
(5)<controller_name>.type
使用控制器的 pluginlib 导出的插件的名称。 这是一个类，从中创建名称为“controller_name”的控制器实例。
2.帮助脚本
有两个脚本可以从启动文件与控制器管理器进行交互：
spawner - 在启动时加载、配置和启动控制器。
unspawner - 停止并卸载控制器。
ros2 run controller_manager spawner -h
usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [--load-only] [--stopped] [-t CONTROLLER_TYPE] [-u]
[--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
controller_name
positional arguments:
controller_name Name of the controller
optional arguments:
-h, --help show this help message and exit
-c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
Name of the controller manager ROS node
-p PARAM_FILE, --param-file PARAM_FILE
Controller param file to be loaded into controller node before configure
--load-only Only load the controller and leave unconfigured.
--stopped Load and configure the controller, however do not start them
-t CONTROLLER_TYPE, --controller-type CONTROLLER_TYPE
If not provided it should exist in the controller manager namespace
-u, --unload-on-kill Wait until this application is interrupted and unload controller
--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
Time to wait for the controller manager
ros2 run controller_manager unspawner -h
usage: unspawner [-h] [-c CONTROLLER_MANAGER] controller_name
positional arguments:
controller_name Name of the controller
optional arguments:
-h, --help show this help message and exit
-c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
Name of the controller manager ROS node
3.概念
(4)Restarting all controllers
重启所有控制器的最简单方法是使用 switch_controllers 服务或 CLI 并将所有控制器添加到启动和停止列表。 请注意，并非所有控制器都必须重新启动，例如广播。
(5)Restarting hardware
如果硬件重新启动，那么您应该再次经历它的生命周期。 这可以通过从接口实现的写入和读取方法返回错误来简单地实现。 尚未实施 - 请暂时手动停止/重新启动所有控制器控制器管理器检测到并停止所有正在命令该硬件的控制器并重新启动正在侦听其状态的广播器。
三、Hardware Components硬件组件
硬件组件表示 ros2_control 框架中物理硬件的抽象。 硬件分为Actuator、Sensor和System三种。 有关每种类型的详细信息，请查看硬件组件说明。
Writing a new hardware interface
在 ros2_control 中，硬件系统组件是库，由控制器管理器使用 pluginlib 接口动态加载。 以下是为新硬件接口创建源文件、基本测试和编译规则的分步指南。
1.Preparing package准备功能包
如果硬件接口的包不存在，则先创建它。 该包应将 ament_cmake 作为构建类型。 最简单的方法是在线搜索最新的手册。 支持此过程的一个有用命令是 ros2 pkg create。 使用 --help 标志获取有关如何使用它的更多信息。 还有一个选项可以创建库源文件和编译规则，以帮助您完成以下步骤。
2.Preparing source files准备源文件
创建包后，您应该至少在其中包含 CMakeLists.txt 和 package.xml 文件。 如果 include/<PACKAGE_NAME>/ 和 src 文件夹不存在，也创建它们。 在 include/<PACKAGE_NAME>/ 文件夹中，在 src 文件夹中添加 <robot_hardware_interface_name>.hpp 和 <robot_hardware_interface_name>.cpp。 可选择添加 visibility_control.h 以及 Windows 导出规则的定义。 您可以从现有的控制器包中复制此文件并将名称前缀更改为 <PACKAGE_NAME>。
3.Adding declarations into header file (.hpp)向头文件中添加声明
1.如果您使用的是“hardware_interface/$interface_type$_interface.hpp”和 visibility_control.h，请包括在内。 $interface_type$ 可以是执行器、传感器或系统，具体取决于您使用的硬件类型。 有关每种类型的更多详细信息，请查看硬件组件说明。
2.为您的 hardware_interface 定义一个唯一的命名空间。 这通常是用 snake_case 写的包名。
3.定义 hardware_interface 的类，扩展 $InterfaceType$Interface，例如，… code:: c++ class HardwareInterfaceName : public hardware_interface::$InterfaceType$Interface
4.添加一个不带参数的构造函数和以下实现LifecycleNodeInterface的公共方法：on_configure、on_cleanup、on_shutdown、on_activate、on_deactivate、on_error； 并覆盖 $InterfaceType$Interface 定义：on_init、export_state_interfaces、export_command_interfaces、prepare_command_mode_switch（可选）、perform_command_mode_switch（可选）、读、写。 有关硬件生命周期的进一步说明，请查看拉取请求并查看方法的确切定义，请查看“hardware_interface/$interface_type$_interface.hpp”标头或执行器、传感器或系统的 doxygen 文档。
4.Adding definitions into source file (.cpp)向源文件中添加定义
1.包括硬件接口的头文件并添加命名空间定义以简化进一步的开发。
2.实现 on_init 方法。 在这里，您应该初始化所有成员变量并处理来自 info 参数的参数。 在第一行中，通常会调用父级 on_init 来处理标准值，例如名称。 这是使用以下方法完成的：hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)。 如果所有必需的参数都已设置且有效并且一切正常，则返回 CallbackReturn::SUCCESS 或返回 CallbackReturn::ERROR 否则。
3.编写 on_configure 方法，您通常在其中设置与硬件的通信并设置所有内容以便激活硬件。
4.实现 on_cleanup 方法，它与 on_configure 相反。
5.实施 export_state_interfaces 和 export_command_interfaces 方法，其中定义了硬件提供的接口。 对于传感器类型的硬件接口，没有 export_command_interfaces 方法。 提醒一下，完整的接口名称具有结构 <joint_name>/<interface_type>。
6.（可选）对于执行器和系统类型的硬件接口，如果您的硬件接受多种控制模式，请实施 prepare_command_mode_switch 和 perform_command_mode_switch。
7.实现启用硬件“电源”的 on_activate 方法。
8.实现 on_deactivate 方法，它与 on_activate 相反。
9.实现硬件正常关闭的 on_shutdown 方法。
10.实现 on_error 方法，处理来自所有状态的不同错误。
11.实现从硬件获取状态并将它们存储到 export_state_interfaces 中定义的内部变量的读取方法。
12.实现 write 方法，该方法根据存储在 export_command_interfaces 中定义的内部变量中的值来命令硬件。
13.重要提示：命名空间关闭后，在文件末尾添加 PLUGINLIB_EXPORT_CLASS 宏。
为此，您需要包含“pluginlib/class_list_macros.hpp”标头。 作为第一个参数，您应该提供确切的硬件接口类，例如 <my_hardware_interface_package>::，作为第二个参数，您应该提供基类，即 hardware_interface::(Actuator|Sensor|System)Interface。
5.Writing export definition for pluginlib为 pluginlib 编写导出定义
1.在包中创建 <my_hardware_interface_package>.xml 文件，并添加必须对 pluginlib 可见的库和硬件接口类的定义。 最简单的方法是检查 hardware_interface mock_components 部分中模拟组件的定义。
2.通常，插件名称由包（命名空间）和类名定义，例如 <my_hardware_interface_package>/。 当资源管理器搜索它时，这个名称定义了硬件接口的类型。 其他两个参数必须对应于 <robot_hardware_interface_name>.cpp 文件底部宏中的定义。
6.编写一个简单的测试来检查是否可以找到并加载控制器
1.在包中创建文件夹 test（如果尚不存在），并添加名为 test_load_<robot_hardware_interface_name>.cpp 的文件。
2.可以复制test_generic_system.cpp包中定义的load_generic_system_2dof内容。
3.更改复制测试的名称，并在指定硬件接口类型的最后一行放置 <my_hardware_interface_package>.xml 文件中定义的名称，例如 <my_hardware_interface_package>/。
7.Add compile directives into CMakeLists.txt file将编译指令添加到 CMakeLists.txt文件中
1.在 find_package(ament_cmake REQUIRED) 行下添加更多依赖项。 至少有：hardware_interface、pluginlib、rclcpp 和 rclcpp_lifecycle。
2.为提供 <robot_hardware_interface_name>.cpp 文件作为源的共享库添加编译指令。
3.为库添加目标包含目录。 这通常只include.
4.添加库需要的 ament 依赖项。 您应该至少添加 1 下列出的那些。
5.使用以下命令导出 pluginlib 描述文件：… code:: cmake
pluginlib_export_plugin_description_file(hardware_interface <my_hardware_interface_package>.xml)
6.为目标和包含目录添加安装指令。
7.在测试部分添加以下依赖项：ament_cmake_gmock、hardware_interface。
8.使用 ament_add_gmock 指令为测试添加编译定义。 有关详细信息，请参阅 ros2_control 包中的模拟硬件是如何完成的。
9.（可选）在 ament_package() 之前将您的硬件接口库添加到 ament_export_libraries 中。
8.Add dependencies into package.xml file将依赖项添加到 package.xml 文件中
至少将以下包添加到 <depend> 标记中：hardware_interface、pluginlib、rclcpp 和 rclcpp_lifecycle。
至少将以下包添加到 <test_depend> 标记中：ament_add_gmock 和 hardware_interface。
9.Compiling and testing the hardware component编译和测试硬件组件
现在一切准备就绪，可以使用 colcon build <my_hardware_interface_package> 命令编译硬件组件。 请记住在执行此命令之前进入工作区的根目录。
如果编译成功，则从安装文件夹中获取 setup.bash 文件并执行 colcon test <my_hardware_interface_package> 以检查是否可以通过 pluginlib 库找到新控制器并由控制器管理器加载。
四、Mock Components模拟组件
模拟组件是硬件组件的简单“模拟”，即系统、传感器和执行器。 它们通过将命令镜像到它们的状态来提供理想的行为。 ros2_control框架的离线测试可以添加相应的硬件接口代替真实硬件。 主要优点是您可以在不访问硬件的情况下测试框架内的所有“管道”。 这意味着您可以测试您的控制器、广播器、启动文件，甚至与 MoveIt 等的集成。 主要目的是减少物理硬件上的调试时间并促进您的开发。
1.Generic System通用系统
该组件实现 hardware_interface::SystemInterface> 支持命令和状态接口。 有关硬件组件的更多信息，请查看详细文档。
2.参数
mock_sensor_commands (optional; boolean; default: false)
使用外部命令创建伪造的命令接口以伪造传感器测量值。 这些接口通常由前向控制器用于提供来自 ROS-world 的访问。
position_state_following_offset (optional; double; default: 0.0)
当镜像到状态时，以下偏移量添加到命令值。
custom_interface_with_following_offset (optional; string; default: “”)
将偏移命令映射到自定义界面。
3.每个关节参数
mimic (optional; string)
要模拟的关节的定义名称。 这是平行夹持器经常使用的概念。 示例：joint1。
multiplier (optional; double; default: 1; used if mimic joint is defined)
模拟参数中定义的模拟关节值的乘数。 示例：-2。
五、ros2_controllers
1.Nomenclature命名法
ros2_control 框架使用命名空间根据控制器使用的命令接口类型对控制器进行排序。 控制器使用通用的硬件接口定义。 控制器的命名空间正在命令以下命令接口类型：
position_controllers: hardware_interface::HW_IF_POSITION
velocity_controller: hardware_interface::HW_IF_VELOCITY
effort_controllers: hardware_interface::HW_IF_ACCELERATION
effort_controllers: hardware_interface::HW_IF_EFFORT
2.Writing a new controller编写新的控制器
(1)Preparing package准备功能包
如果控制器的包不存在，则先创建它。 该包应将 ament_cmake 作为构建类型。 最简单的方法是在线搜索最新的手册。 支持此过程的一个有用命令是 ros2 pkg create。 使用 --help 标志获取有关如何使用它的更多信息。 还有一个选项可以创建库源文件和编译规则，以帮助您完成以下步骤。
(2)Preparing source files准备源文件
创建包后，您应该至少在其中包含 CMakeLists.txt 和 package.xml 文件。 如果 include/<PACKAGE_NAME>/ 和 src 文件夹不存在，也创建它们。 在 include/<PACKAGE_NAME>/ 文件夹中，在 src 文件夹中添加 <controller_name>.hpp 和 <controller_name>.cpp。 可选择添加 visibility_control.h 以及 Windows 导出规则的定义。 您可以从现有的控制器包中复制此文件并将名称前缀更改为 <PACKAGE_NAME>。
(3)Adding declarations into header file (.hpp)
1.如果您使用的是“controller_interface/controller_interface.hpp”和 visibility_control.h，则包括在内。
2.为您的控制器定义一个唯一的命名空间。 这通常是用 snake_case 写的包名。
3.定义控制器的类，扩展 ControllerInterface，例如 … code:: c++ class ControllerName : public controller_interface::ControllerInterface
4.添加一个不带参数的构造函数和以下覆盖 ControllerInterface 定义的公共方法：init、command_interface_configuration、state_interface_configuration、on_configure、on_activate、on_deactivate、update。 有关确切定义，请检查 controller_interface/controller_interface.hpp 标头或 ros2_controllers 中的控制器之一。
5.（可选）通常，控制器接受关节名称和接口名称列表作为参数。 如果是这样，您可以添加两个受保护的字符串向量来存储这些值。
(4)Adding definitions into source file (.cpp)
1.包含控制器的头文件并添加命名空间定义以简化进一步的开发。
2.（可选）如果需要，实现一个构造函数。 在那里，您可以初始化成员变量。 这也可以在 init 方法中完成。
3.实现初始化方法。 第一行通常调用父 init 方法。 这里是初始化变量、预留内存以及最重要的是声明控制器使用的节点参数的最佳位置。 如果一切正常，则返回 controller_interface::return_type::OK 或 controller_interface::return_type::ERROR 否则。
4.编写 on_configure 方法。 通常在这里读取参数，一切准备就绪，以便可以启动控制器。
5.在定义所需接口的地方实施 command_interface_configuration 和 state_interface_configuration。 在 controller_interface/controller_interface.hpp 中定义了接口配置 ALL、INDIVIDUAL 和 NONE 三个选项。ALL 和 NONE 选项将要求访问所有可用接口或不访问任何接口。INDIVIDUAL 配置需要所需接口的详细列表 名称。这些通常作为参数提供。完整的接口名称具有结构 <joint_name>/<interface_type>。
6.实现 on_activate 方法，检查并可能排序接口并分配成员的初始值。 此方法是实时循环的一部分，因此应避免保留任何内存，并且通常应尽可能短。
7.实现 on_deactivate 方法，它与 on_activate 相反。 在许多情况下，此方法是空的。 这种方法也应该尽可能做到实时安全。
8.实现更新方法作为主要入口点。 该方法的实施应考虑到实时约束。 调用此方法时，状态接口具有来自硬件的最新值，并且硬件的新命令应写入命令接口。
重要提示：命名空间关闭后，在文件末尾添加 PLUGINLIB_EXPORT_CLASS 宏。 为此，您需要包含“pluginlib/class_list_macros.hpp”标头。 作为第一个参数，您应该提供确切的控制器类，例如 <controller_name_namespace>::，作为第二个参数，您应该提供基类，即 controller_interface::ControllerInterface。
参考文献
[1].https://control.ros.org/master/doc/getting_started/getting_started.html
注：本文档翻译自参考文献[1]，仅供个人学习参考使用。