ros2_control 6 自由度机械臂（上）+https://developer.aliyun.com/article/1585408
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 中看到机器人做圆周运动的教程。
