系列文章目录
前言
ros2_control 是一个实时控制框架，专为普通机器人应用而设计。标准的 c++ 接口用于与硬件交互和查询用户定义的控制器命令。这些接口增强了代码的模块化和与机器人无关的设计。具体的应用细节，例如使用什么控制器、机器人有多少个关节以及它们的运动学结构，则通过 YAML 参数配置文件和通用机器人描述文件（URDF）来指定。最后，通过 ROS 2 启动文件部署 ros2_control 框架。
本教程将详细介绍 ros2_control 的各个组成部分，即
- ros2_control 概述
- 编写 URDF
- 编写硬件接口
- 编写控制器
一、ros2_control 概述
ros2_control 引入了状态接口（state_interfaces）和命令接口（command_interfaces）来抽象硬件接口。state_interfaces 是只读数据句柄，通常代表传感器读数，如关节编码器。command_interfaces 是读写数据句柄，代表硬件命令，如设置关节速度参考。command_interfaces 是专用访问接口，也就是说，如果控制器 "认领 "了某个接口，那么在该接口被释放之前，任何其他控制器都不能使用它。这两种接口类型都有唯一的名称和类型。所有可用状态和命令接口的名称和类型都在 YAML 配置文件和 URDF 文件中指定。
ros2_control 提供了控制器接口（ControllerInterface）和硬件接口（HardwareInterface）类，以实现与机器人无关的控制。在初始化过程中，控制器通过 ControllerInterface 申请运行所需的状态接口和命令接口。另一方面，硬件驱动程序通过硬件接口（HardwareInterface）提供状态接口和命令接口。ros2_control 确保在启动控制器之前，所有请求的接口都可用。接口模式允许供应商编写运行时加载的特定硬件驱动程序。
主程序是一个实时读、更新、写循环。在读取调用期间，符合 HardwareInterface 的硬件驱动程序会用从硬件接收到的最新值更新其提供的状态_接口。在更新调用期间，控制器根据更新后的状态接口计算命令，并将其写入命令接口。最后，在写入调用期间，硬件驱动程序从其提供的 command_interfaces 中读取值，并将其发送给硬件。ros2_control 节点通过一个实时线程运行主循环。ros2_control 节点运行第二个非实时线程，与 ROS 发布者、订阅者和服务进行交互。
二、编写 URDF
URDF 文件是一种基于 XML 的标准文件，用于描述机器人的特征。它可以表示任何具有树形结构的机器人，但具有循环结构的机器人除外。每个链接必须只有一个父链接。对于 ros2_control，有三个主要标签：link、joint 和 ros2_control。关节标签定义了机器人的运动学结构，而链接标签则定义了动态属性和 3D 几何结构。ros2_control 则定义硬件和控制器配置。
2.1 几何结构
大多数商用机器人已经定义了机器人描述包（robot_description packages），请参阅通用机器人（Universal Robots）。不过，本教程将详细介绍如何从头开始创建一个机器人描述包。
首先，我们需要一个机器人的 3D 模型。为了便于说明，我们将使用一个通用的 6 DOF 机器人操作器。
机器人的 6 个刚体需要分别处理并导出为各自的 .stl 和 .dae 文件。一般来说，.stl 3D 模型文件是用于快速碰撞检查的粗网格，而 .dae 文件仅用于可视化目的。为简单起见，我们将使用相同的网格。
按照惯例，每个 .stl 文件都在自己的坐标系中表达其顶点的位置。因此，我们需要指定每个刚体之间的线性变换（旋转和平移），以定义机器人的完整几何体。每个刚体的三维模型都应调整为近端关节轴（连接刚体与其父体的轴）位于 Z 轴方向。三维模型的原点也应调整为网格底面与 xy 平面共面。下面的网格说明了这种配置。
每个网格都应在处理后导出到各自的文件中。Blender 是一款开源三维建模软件，可以导入/导出 .stl 和 .dae 文件并操作其顶点。本教程使用 Blender 处理机器人模型。
最后，我们可以计算机器人关节之间的变换，并开始编写 URDF。首先，对其坐标系中的刚体 2 应用负 90 度翻滚。
为了使示例简单明了，我们现在不应用俯仰角。然后，我们施加 90 度的正偏航。
最后，我们在刚体 2 和刚体 1 的坐标系帧之间进行 x 轴-0.1 米和 z 轴 0.18 米的平移。最终结果如下所示。
然后对所有刚体重复上述过程。
2.2 URDF 文件
URDF 文件一般按照以下模板格式化。
<robot name="robot_6_dof">
<!-- create link fixed to the "world" -->
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://robot_6_dof/meshes/visual/link_0.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://robot_6_dof/meshes/collision/link_0.stl"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<!-- additional links ... -->
<link name="world"/>
<link name="tool0"/>
<joint name="base_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- joints - main serial chain -->
<joint name="joint_1" type="revolute">
<parent link="base_link"/>
<child link="link_1"/>
<origin rpy="0 0 0" xyz="0 0 0.061584"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5"/>
</joint>
<!-- additional joints ... -->
<!-- ros2 control tag -->
<ros2_control name="robot_6_dof" type="system">
<hardware>
<plugin>
<!-- {Name_Space}/{Class_Name}-->
</plugin>
</hardware>
<joint name="joint_1">
<command_interface name="position">
<param name="min">{-2*pi}</param>
<param name="max">{2*pi}</param>
</command_interface>
<!-- additional command interfaces ... -->
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<!-- additional state interfaces ... -->
</joint>
<!-- additional joints ...-->
<!-- additional hardware/sensors ...-->
</ros2_control>
</robot>- robot 标签包含 URDF 文件的所有内容。它有一个必须指定的名称属性。
- link 标签定义了机器人的几何形状和惯性属性。它有一个名称属性，将被关节标签引用。
- visual 标记指定视觉网格的旋转和平移。如果网格是按照前面所述的方法处理的，那么原点标签可以全部为零。
- geometry 和 mesh 标签指定三维网格文件相对于指定 ROS 2 软件包的位置。
- collision 标签等同于视觉标签，只是指定的网格在某些应用中用于碰撞检测。
- inertial 标签指定 link 的质量和惯性。origin 标签指定刚体的质心。这些值用于计算正向和反向动力学。由于我们的应用程序不使用动力学，因此使用统一的任意值。
- <!-- additional links ... --> 注释表示将定义多个连续的链接标记，每个链接一个。
- <link name="world"/> 和 <link name="tool0"/> 元素不是必需的。不过，约定俗成的做法是将机器人顶端的 link 设置为 tool0，并定义机器人相对于世界坐标系的 base link。
- joint 标签指定机器人的运动结构。它有两个必备属性：名称和类型。类型指定两个相连刚体之间的可行运动。随后的父刚体 parent 和子刚体 child 则指定关节连接了哪两个链接。
- axis 标签规定了关节的自由度。如果网格是按前述方法处理的，则轴值始终为 "0 0 1"。
- limits 标签指定关节的运动学和动力学限制。
- ros2_control 标签指定机器人的硬件配置。更具体地说，就是可用的状态和命令接口。该标签有两个必填属性：名称和类型。该标签还包含传感器等附加元素。
- hardware 和 plugin 标签指示 ros2_control 框架将符合 HardwareInterface 标准的硬件驱动程序作为插件动态加载。插件被指定为 <{Name_Space}/{Class_Name} 。
- 最后，joint 标记指定了加载的插件将提供的状态和命令接口。关节点用 name 属性指定。command_interface 和 state_interface 标签指定接口类型，通常是位置、速度、加速度或力。
为简化 URDF 文件，使用 xacro 来定义宏，请参阅本教程。本教程中机器人的完整 xacro 文件可在此处获取。在 xacro 生成 URDF 后，可以使用 urdf_to_graphviz 工具验证运动学链。运行
xacro description/urdf/r6bot.urdf.xacro > r6bot.urdf
urdf_to_graphviz r6bot.urdf r6bot生成 r6bot.pdf，显示机器人的运动链。
三、编写硬件接口
在 ros2_control 中，硬件系统组件是通过符合 HardwareInterface 公共接口的用户定义驱动插件集成的。URDF 中指定的硬件插件在初始化过程中使用 pluginlib 接口动态加载。为了运行 ros2_control_node，必须设置名为 robot_description 的参数。这通常在 ros2_control 启动文件中完成。
以下代码块将解释编写新硬件接口的要求。
教程机器人的硬件插件是一个名为 RobotSystem 的类，它继承自 hardware_interface::SystemInterface。SystemInterface 是为完整机器人系统设计的硬件接口之一。例如，UR5 就使用了该接口。机器人系统必须实现五个公共方法。
- 启动（on_init）
- 输出状态接口（export_state_interfaces）
- 导出命令接口（export_command_interfaces）
- 读取（read）
- 写（write）
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
#include "hardware_interface/types/hardware_interface_return_values.hpp"
class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface {
public:
CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
// private members
// ...
}如果在 URDF 中指定了机器人系统，on_init 方法会在 ros2_control 初始化过程中被调用一次。在该方法中，需要设置机器人硬件之间的通信，并分配动态内存。由于教程机器人是仿真的，因此不会建立明确的通信。相反，将初始化代表所有硬件状态的向量，例如描述关节角度的 double 向量等。
CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo &info) {
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
return CallbackReturn::ERROR;
}
// setup communication with robot hardware
// ...
return CallbackReturn::SUCCESS;
}值得注意的是，on_init 的行为预计会因 URDF 文件的不同而不同。SystemInterface::on_init(info) 调用会用 URDF 中的具体信息填充 info 对象。例如，info 对象有 joints、sensors、gpios 等字段。假设传感器字段的名称值为 tcp_force_torque_sensor。那么 on_init 必须尝试与该传感器建立通信。如果失败，则返回错误值。
接下来，将依次调用 export_state_interfaces 和 export_command_interfaces 方法。export_state_interfaces 方法会返回一个 StateInterface 向量，描述每个关节的状态接口。StateInterface 对象是只读数据句柄。它们的构造函数需要一个接口名称、接口类型和一个指向 double 数据值的指针。对于机器人系统来说，数据指针引用的是类成员变量。这样，每个方法都可以访问数据。
std::vector<hardware_interface::StateInterface> RobotSystem::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> state_interfaces;
// add state interfaces to ``state_interfaces`` for each joint, e.g. `info_.joints[0].state_interfaces_`, `info_.joints[1].state_interfaces_`, `info_.joints[2].state_interfaces_` ...
// ...
return state_interfaces;
}export_command_interfaces 方法与前一个方法几乎完全相同。不同之处在于返回的是一个 CommandInterface 向量。该向量包含描述每个关节的命令接口的对象。
std::vector<hardware_interface::CommandInterface> RobotSystem::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> command_interfaces;
// add command interfaces to ``command_interfaces`` for each joint, e.g. `info_.joints[0].command_interfaces_`, `info_.joints[1].command_interfaces_`, `info_.joints[2].command_interfaces_` ...
// ...
return command_interfaces;
}read 方法是 ros2_control 循环的核心方法。在主循环中，ros2_control 会遍历所有硬件组件并调用 read 方法。该方法在实时线程上执行，因此必须遵守实时约束。read 方法负责更新状态接口的数据值。由于数据值指向类成员变量，因此可以用相应的传感器值填充这些值，进而更新每个导出状态接口对象的值。
return_type RobotSystem::read(const rclcpp::Time & time, const rclcpp::Duration &period) {
// read hardware values for state interfaces, e.g joint encoders and sensor readings
// ...
return return_type::OK;
}write 方法是 ros2_control 循环中的另一个核心方法。它在实时循环中更新后被调用。因此，它也必须遵守实时约束。write 方法负责更新命令接口的数据值。与读取方法不同，写入方法访问由导出的命令接口对象指针指向的数据值，并将其发送给相应的硬件。例如，如果硬件支持通过 TCP 协议设置关节速度，那么该方法就会访问相应命令接口的数据并发送包含该值的数据包。
return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) {
// send command interface values to hardware, e.g joint set joint velocity
// ...
return return_type::OK;
}最后，所有 ros2_control 插件都应在文件末尾添加以下两行代码。
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(robot_6_dof_hardware::RobotSystem, hardware_interface::SystemInterface)PLUGINLIB_EXPORT_CLASS 是一个使用 pluginlib 创建插件库的 c++ 宏。
3.1 插件说明文件（hardware）
插件描述文件是一个必需的 XML 文件，用于描述插件的库名、类类型、命名空间、描述和接口类型。该文件允许 ROS 2 自动发现和加载插件。其格式如下
<library path="{Library_Name}">
<class
name="{Namespace}/{Class_Name}"
type="{Namespace}::{Class_Name}"
base_class_type="hardware_interface::SystemInterface">
<description>
{Human readable description}
</description>
</class>
</library>库标记的路径属性指的是用户自定义硬件插件的 cmake 库名称。完整的 XML 文件请参见此处。
3.2 CMake 库（hardware）
在 ros2_control 中制作硬件插件的一般 CMake 模板如下所示。请注意，使用插件源代码创建的库就像其他 cmake 库一样。此外，还需要添加额外的编译定义和 cmake 导出宏 (pluginlib_export_plugin_description_file)。请点击此处查看完整的 CMakeLists.txt 文件。
add_library(
robot_6_dof_hardware
SHARED
src/robot_hardware.cpp
)四、编写控制器
在 ros2_control 中，控制器是作为符合 ControllerInterface 公共接口的插件来实现的。与硬件接口类似，要加载的控制器插件也是通过 ROS 参数指定的。这通常是通过向 ros2_control_node 传递 YAML 参数文件来实现的。与硬件接口不同，控制器有一组有限的状态：
- 未配置（Unconfigured）
- 未激活（Inactive）
- 激活（Active）
- 最终完成（Finalized）
在这些状态之间转换时，会调用某些接口方法。在主控制循环期间，控制器处于激活状态。
以下代码块将解释编写新控制器的要求。
教程机器人的控制器插件是一个名为 RobotController 的类，它继承自 controller_interface::ControllerInterface。机器人控制器必须实现九个公共方法。最后六个是管理节点转换回调。
- 命令接口配置（command_interface_configuration）
- 状态界面配置（state_interface_configuration）
- 更新（update）
- 配置（on_configure）
- 激活（on_activate）
- 停用时（on_deactivate）
- 清除（on_cleanup）
- 出错时（on_error）
- 关闭（on_shutdown）
class RobotController : public controller_interface::ControllerInterface {
public:
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
controller_interface::CallbackReturn on_init() override;
controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) override;
controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) override;
controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &previous_state) override;
// private members
// ...
}控制器插件动态加载后，会立即调用 on_init 方法。该方法在控制器的生命周期内只被调用一次，因此应分配控制器生命周期内存在的内存。此外，应声明并访问关节（joints）、命令接口（command_interfaces）和状态接口（state_interfaces）的参数值。接下来的两个方法都需要这些参数值。
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
controller_interface::CallbackReturn on_init(){
// declare and get parameters needed for controller initialization
// allocate memory that will exist for the life of the controller
// ...
return CallbackReturn::SUCCESS;
}控制器设置为非激活状态后，on_configure 方法会立即被调用。这种状态不仅会在控制器首次启动时出现，也会在重新启动时出现。应在此方法中读取可重新配置的参数。此外，还应创建发布者和订阅者。
controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state){
// declare and get parameters needed for controller operations
// setup realtime buffers, ROS publishers, and ROS subscribers
// ...
return CallbackReturn::SUCCESS;
}命令接口配置（command_interface_configuration）方法在 on_configure 之后调用。该方法返回一个 InterfaceConfiguration 对象列表，用于指明控制器需要运行哪些命令接口。命令接口由其名称和接口类型唯一标识。如果已加载的硬件接口不提供所请求的接口，那么控制器就会失败。
controller_interface::InterfaceConfiguration command_interface_configuration(){
controller_interface::InterfaceConfiguration conf;
// add required command interface to `conf` by specifying their names and interface types.
// ..
return conf
}然后调用 state_interface_configuration 方法，该方法与上一个方法类似。不同的是，将返回一个 InterfaceConfiguration 对象列表，该列表代表运行所需的状态接口。
controller_interface::InterfaceConfiguration state_interface_configuration() {
controller_interface::InterfaceConfiguration conf;
// add required state interface to `conf` by specifying their names and interface types.
// ..
return conf
}控制器启动时，on_activate 会被调用一次。该方法应处理控制器重启，例如将重置引用设置为安全值。它还应执行控制器特定的安全检查。激活控制器时，还会再次调用 command_interface_configuration 和 state_interface_configuration 方法。
controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state){
// Handle controller restarts and dynamic parameter updating
// ...
return CallbackReturn::SUCCESS;
}更新（update）方法是主控制环的一部分。由于该方法是实时控制环的一部分，因此必须执行实时约束。控制器应从其状态接口读取数据，读取参考值并计算控制输出。通常情况下，参考点是通过 ROS 2 用户访问的。由于订阅器运行在非实时线程上，因此需要使用实时缓冲区将消息传递给实时线程。实时缓冲区最终是一个指向 ROS 消息的指针，它带有一个互斥器，可以保证线程安全，实时线程永远不会被阻塞。然后，计算出的控制输出将写入命令接口，进而控制硬件。
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period){
// Read controller inputs values from state interfaces
// Calculate controller output values and write them to command interfaces
// ...
return controller_interface::return_type::OK;
}控制器停止运行时会调用 on_deactivate。在此方法中释放已申请的命令接口非常重要，这样其他控制器就可以在需要时使用它们。这可以通过 release_interfaces 函数来实现。
controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state){
release_interfaces();
// The controller should be properly shutdown during this
// ...
return CallbackReturn::SUCCESS;
}当控制器的生命周期节点过渡到关闭时，会调用 on_cleanup 和 on_shutdown。释放已分配的内存和一般清理工作应在这些方法中完成。
controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state){
// Callback function for cleanup transition
// ...
return CallbackReturn::SUCCESS;
}controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &previous_state){
// Callback function for shutdown transition
// ...
return CallbackReturn::SUCCESS;
}如果托管节点的状态转换失败，则会调用 on_error 方法。这种情况一般不会发生。
controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state){
// Callback function for erroneous transition
// ...
return CallbackReturn::SUCCESS;
}4.1 插件说明文件（控制器）
控制器也需要插件说明文件，因为它是作为库导出的。控制器插件说明文件格式如下。完整的 XML 文件请参见此处。
<library path="{Library_Name}">
<class
name="{Namespace}/{Class_Name}"
type="{Namespace}::{Class_Name}"
base_class_type="controller_interface::ControllerInterface">
<description>
{Human readable description}
</description>
</class>
</library>4.2 CMake 库（控制器）
必须在构建控制器插件的 CMake 文件中指定该插件。完整的 CMakeLists.txt 文件请参见此处。
add_library(
r6bot_controller
SHARED
src/robot_controller.cpp
)五、启动示例
首先构建工作区，即可运行完整的教程示例。
git clone -b iron https://github.com/ros-controls/ros2_control_demos.git
cd ros2_control_demos
colcon build --symlink-install
source install/setup.bash要查看机器人，请打开终端并启动 ros2_control_demo_example_7 软件包中的 view_r6bot.launch.py 文件。
ros2 launch ros2_control_demo_example_7 view_r6bot.launch.py现在，您可以使用 joint_state_publisher_gui 改变每个关节的位置。
接下来，杀死启动文件中的进程，开始仿真受控机器人。打开终端，启动 ros2_control_demo_example_7 软件包中的 r6bot_controller.launch.py 文件。
ros2 launch ros2_control_demo_example_7 r6bot_controller.launch.py最后，打开一个新的终端，运行以下命令。
ros2 launch ros2_control_demo_example_7 send_trajectory.launch.py您应该能在 RViz 中看到机器人做圆周运动的教程。
1456
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
