ros2 自定义一个控制器

在 ROS 2 中，自定义一个控制器并进行使用涉及几个步骤，包括编写控制器代码、配置控制器参数、编写控制器启动文件以及在运行时加载和使用控制器。以下是一个详细的步骤指南：

1. 编写控制器代码

首先，创建一个控制器类，该类继承自 controller_interface::ControllerInterface。下面是一个简单的示例控制器代码：

#include <controller_interface/controller_interface.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <string>
#include <vector>
#include <exception>

class CustomController : public controller_interface::ControllerInterface {
public:
  CustomController() = default;

  controller_interface::return_type init(const std::string & controller_name) override {
    auto ret = ControllerInterface::init(controller_name);
    if (ret != controller_interface::return_type::OK) {
      return ret;
    }
    try {
      auto_declare<std::string>("arm_id", "fr3");
    } catch (const std::exception & e) {
      RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s", e.what());
      return controller_interface::return_type::ERROR;
    }
    return controller_interface::return_type::OK;
  }

  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override {
    arm_id_ = get_node()->get_parameter("arm_id").as_string();
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override {
    // Activation logic here
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override {
    // Deactivation logic here
    return CallbackReturn::SUCCESS;
  }

  controller_interface::InterfaceConfiguration command_interface_configuration() const override {
    controller_interface::InterfaceConfiguration config;
    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
    for (int i = 1; i <= num_joints_; ++i) {
      config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort");
    }
    return config;
  }

  controller_interface::InterfaceConfiguration state_interface_configuration() const override {
    return {};
  }

private:
  std::string arm_id_;
  int num_joints_ = 6;
};

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(CustomController, controller_interface::ControllerInterface)

确保在控制器的 init 或 on_configure 方法中声明和获取所需的参数。上面的示例中，我们在 init 方法中使用 auto_declare 声明参数 arm_id，并在 on_configure 方法中获取它。

3. 创建启动文件

为了运行控制器，需要创建一个 ROS 2 启动文件。启动文件通常使用 Python 编写，以下是一个示例：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='controller_manager',
            executable='ros2_control_node',
            parameters=[{
                'robot_description': '<robot_urdf>',
                'arm_id': 'fr3'
            }],
            output='screen'
        ),
        Node(
            package='controller_manager',
            executable='spawner.py',
            arguments=['custom_controller'],
            output='screen'
        )
    ])

4. 编写控制器配置文件

在控制器配置文件中指定控制器类型和相关参数，例如：

controller_manager:
  ros__parameters:
    update_rate: 100
    custom_controller:
      type: "package_name/CustomController"
      arm_id: "fr3"

5. 加载和使用控制器

启动 ROS 2 启动文件：

ros2 launch your_package your_launch_file.launch.py

通过 ros2 control 命令可以加载、配置和启动控制器。例如：

ros2 control load_controller custom_controller
ros2 control configure_controller custom_controller
ros2 control switch_controller --start custom_controller

总结

通过以上步骤，你可以自定义一个 ROS 2 控制器并进行使用。从编写控制器代码、配置参数，到创建启动文件和配置文件，最后加载和启动控制器，这些步骤覆盖了自定义控制器的完整流程。