ros2_control 6 自由度机械臂

系列文章目录

---

前言

        ros2_control 是一个实时控制框架，专为普通机器人应用而设计。标准的 c++ 接口用于与硬件交互和查询用户定义的控制器命令。这些接口增强了代码的模块化和与机器人无关的设计。具体的应用细节，例如使用什么控制器、机器人有多少个关节以及它们的运动学结构，则通过 YAML 参数配置文件和通用机器人描述文件（URDF）来指定。最后，通过 ROS 2 启动文件部署 ros2_control 框架。

        本教程将详细介绍 ros2_control 的各个组成部分，即

1. ros2_control 概述
2. 编写 URDF
3. 编写硬件接口
4. 编写控制器

---

一、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 DOF 机器人操作器

         机器人的 6 个刚体需要分别处理并导出为各自的 .stl 和 .dae 文件。一般来说，.stl 3D 模型文件是用于快速碰撞检查的粗网格，而 .dae 文件仅用于可视化目的。为简单起见，我们将使用相同的网格。

        按照惯例，每个 .stl 文件都在自己的坐标系中表达其顶点的位置。因此，我们需要指定每个刚体之间的线性变换（旋转和平移），以定义机器人的完整几何体。每个刚体的三维模型都应调整为近端关节轴（连接刚体与其父体的轴）位于 Z 轴方向。三维模型的原点也应调整为网格底面与 xy 平面共面。下面的网格说明了这种配置。

刚体 1

刚体 2 已对齐

        每个网格都应在处理后导出到各自的文件中。Blender 是一款开源三维建模软件，可以导入/导出 .stl 和 .dae 文件并操作其顶点。本教程使用 Blender 处理机器人模型。

        最后，我们可以计算机器人关节之间的变换，并开始编写 URDF。首先，对其坐标系中的刚体 2 应用负 90 度翻滚。

刚体 2 带 -90 度滚动

         为了使示例简单明了，我们现在不应用俯仰角。然后，我们施加 90 度的正偏航。

刚体 2，-90 度滚动和 90 度偏航

        最后，我们在刚体 2 和刚体 1 的坐标系帧之间进行 x 轴-0.1 米和 z 轴 0.18 米的平移。最终结果如下所示。

刚体 2 带有-90 度滚动、90 度偏航和平移功能

        然后对所有刚体重复上述过程。

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 就使用了该接口。机器人系统必须实现五个公共方法。

1. 启动（on_init）
2. 输出状态接口（export_state_interfaces）
3. 导出命令接口（export_command_interfaces）
4. 读取（read）
5. 写（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}"><classname="{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_hardwareSHAREDsrc/robot_hardware.cpp
)

四、编写控制器

        在 ros2_control 中，控制器是作为符合 ControllerInterface 公共接口的插件来实现的。与硬件接口类似，要加载的控制器插件也是通过 ROS 参数指定的。这通常是通过向 ros2_control_node 传递 YAML 参数文件来实现的。与硬件接口不同，控制器有一组有限的状态：

1. 未配置（Unconfigured）
2. 未激活（Inactive）
3. 激活（Active）
4. 最终完成（Finalized）

        在这些状态之间转换时，会调用某些接口方法。在主控制循环期间，控制器处于激活状态。

        以下代码块将解释编写新控制器的要求。

        教程机器人的控制器插件是一个名为 RobotController 的类，它继承自 controller_interface::ControllerInterface。机器人控制器必须实现九个公共方法。最后六个是管理节点转换回调。

1. 命令接口配置（command_interface_configuration）
2. 状态界面配置（state_interface_configuration）
3. 更新（update）
4. 配置（on_configure）
5. 激活（on_activate）
6. 停用时（on_deactivate）
7. 清除（on_cleanup）
8. 出错时（on_error）
9. 关闭（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}"><classname="{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_controllerSHAREDsrc/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 中看到机器人做圆周运动的教程。