ROS2系列教程：ROS2-Control与控制器（原理）

1
/controller_manager（总管家）
2
    ├── 控制器：diff_drive_controller（订阅 /cmd_vel → 计算轮速）
3
    ├── 控制器：joint_trajectory_controller（执行轨迹）
4
    └── 硬件接口：RobotHW（把目标轮速写到电机，读回编码器）
5
            ↓ 串口 / MicroROS / CAN
6
         真实电机 + 编码器

1
读取状态（State Interface：读编码器）
2
    ↓
3
控制器计算（diff_drive：把 /cmd_vel 换算成轮速）
4
    ↓
5
写入命令（Command Interface：写目标轮速）
6
    ↓
7
等待下一个周期（默认 100Hz / 10ms）

1
<!-- robot.urdf.xacro：单关节机器人 + ros2_control 仿真硬件 -->
2
<?xml version="1.0"?>
3
<robot name="single_joint_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
4

5
  <link name="base_link"/>
6
  <link name="joint1_link"/>
7

8
  <joint name="joint1" type="revolute">
9
    <parent link="base_link"/>
10
    <child link="joint1_link"/>
11
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
12
    <axis xyz="0 0 1"/>
13
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1.0"/>
14
  </joint>
15

16
  <!-- ros2_control 硬件声明 -->
17
  <ros2_control name="SingleJointHW" type="system">
18
    <hardware>
19
      <plugin>mock_components/GenericSystem</plugin>
20
      <param name="example_param">1.0</param>
21
    </hardware>
22
    <joint name="joint1">
23
      <command_interface name="position"/>
24
      <command_interface name="velocity"/>
25
      <state_interface name="position"/>
26
      <state_interface name="velocity"/>
27
    </joint>
28
  </ros2_control>
29

30
  <!-- 连接 ros2_control 与 robot_state_publisher -->
31
  <gazebo>
32
    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
33
      <parameters>$(find single_joint_robot)/config/ros2_control.yaml</parameters>
34
    </plugin>
35
  </gazebo>
36
</robot>

1
controller_manager:
2
  ros__parameters:
3
    update_rate: 100          # 控制循环 100Hz
4
    # 声明要加载的控制器
5
    joint_state_broadcaster:
6
      type: joint_state_broadcaster/JointStateBroadcaster
7
    joint_trajectory_controller:
8
      type: joint_trajectory_controller/JointTrajectoryController
9

10
joint_state_broadcaster:
11
  ros__parameters:
12
    interface_names:
13
      - joint1/position
14
      - joint1/velocity
15

16
joint_trajectory_controller:
17
  ros__parameters:
18
    joints:
19
      - joint1
20
    command_interfaces:
21
      - position
22
    state_interfaces:
23
      - position
24
      - velocity
25
    state_publish_rate: 50.0
26
    action_monitor_rate: 20.0

1
# launch/control.launch.py：启动整个 ros2_control 系统
2
from launch import LaunchDescription
3
from launch_ros.actions import Node
4
from launch.actions import ExecuteProcess
5
from launch.substitutions import Command, FindExecutable
6
import os
7

8
def generate_launch_description():
9
    # 解析 URDF（含 ros2_control 声明）
10
    robot_description = Command([
11
        FindExecutable(name='xacro'), ' ',
12
        os.path.join('src/single_joint_robot/urdf/robot.urdf.xacro')
13
    ])
14

15
    return LaunchDescription([
16
        # 1. 发布机器人模型
17
        Node(
18
            package='robot_state_publisher',
19
            executable='robot_state_publisher',
20
            parameters=[{'robot_description': robot_description}],
21
            output='screen',
22
        ),
23
        # 2. 启动 Controller Manager（读取 YAML 配置）
24
        Node(
25
            package='controller_manager',
26
            executable='ros2_control_node',
27
            parameters=['config/ros2_control.yaml'],
28
            output='screen',
29
        ),
30
        # 3. 启动 RViz（可选，可视化）
31
        ExecuteProcess(
32
            cmd=['rviz2', '-d', 'config/view.rviz'],
33
            output='screen',
34
        ),
35
    ])

1
# send_trajectory.py：发送关节轨迹的 Action 客户端
2
import rclpy
3
from rclpy.action import ActionClient
4
from rclpy.node import Node
5
from control_msgs.action import FollowJointTrajectory
6
from trajectory_msgs.msg import JointTrajectoryPoint
7

8
class TrajectoryClient(Node):
9
    def __init__(self):
10
        super().__init__('trajectory_client')
11
        self.client = ActionClient(
12
            self,
13
            FollowJointTrajectory,
14
            '/joint_trajectory_controller/follow_joint_trajectory')
15
        self.client.wait_for_server()
16
        self.get_logger().info('trajectory server connected')
17

18
    def send(self, positions, seconds=2.0):
19
        goal = FollowJointTrajectory.Goal()
20
        point = JointTrajectoryPoint()
21
        point.positions = list(positions)
22
        point.time_from_start.sec = int(seconds)
23
        goal.trajectory.joint_names = ['joint1']
24
        goal.trajectory.points = [point]
25
        future = self.client.send_goal_async(goal)
26
        future.add_done_callback(self._goal_done)
27

28
    def _goal_done(self, future):
29
        goal_handle = future.result()
30
        if goal_handle.accepted:
31
            self.get_logger().info('goal accepted')
32
            goal_handle.get_result_async()
33
        else:
34
            self.get_logger().warn('goal rejected')
35

36
def main():
37
    rclpy.init()
38
    node = TrajectoryClient()
39
    node.send([1.57])      # 转到 90 度
40
    rclpy.spin(node)
41
    node.destroy_node()
42
    rclpy.shutdown()
43

44
if __name__ == '__main__':
45
    main()

1
// send_trajectory.cpp：C++ 发送关节轨迹
2
#include <rclcpp/rclcpp.hpp>
3
#include <rclcpp_action/rclcpp_action.hpp>
4
#include <control_msgs/action/follow_joint_trajectory.hpp>
5
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
6

7
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
8

9
class TrajectoryClient : public rclcpp::Node {
10
public:
11
  using Client = rclcpp_action::Client<FollowJointTrajectory>;
12

13
  TrajectoryClient() : Node("trajectory_client") {
14
    client_ = rclcpp_action::create_client<FollowJointTrajectory>(
15
        this, "/joint_trajectory_controller/follow_joint_trajectory");
16
    while (!client_->wait_for_action_server(std::chrono::seconds(1))) {
17
      RCLCPP_INFO(get_logger(), "waiting for action server...");
18
    }
19
    RCLCPP_INFO(get_logger(), "action server connected");
20
    send(1.57);
21
  }
22

23
  void send(double position, double seconds = 2.0) {
24
    auto goal = FollowJointTrajectory::Goal();
25
    trajectory_msgs::msg::JointTrajectoryPoint point;
26
    point.positions.push_back(position);
27
    point.time_from_start = rclcpp::Duration::from_seconds(seconds);
28
    goal.trajectory.joint_names = {"joint1"};
29
    goal.trajectory.points.push_back(point);
30

31
    auto send_opt = rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
32
    send_opt.goal_response_callback = [this](auto future) {
33
      if (future.get()) RCLCPP_INFO(get_logger(), "goal accepted");
34
      else RCLCPP_WARN(get_logger(), "goal rejected");
35
    };
36
    client_->async_send_goal(goal, send_opt);
37
  }
38

39
private:
40
  std::shared_ptr<Client> client_;
41
};
42

43
int main(int argc, char **argv) {
44
  rclcpp::init(argc, argv);
45
  rclcpp::spin(std::make_shared<TrajectoryClient>());
46
  rclcpp::shutdown();
47
  return 0;
48
}

1
find_package(control_msgs REQUIRED)
2
find_package(trajectory_msgs REQUIRED)
3
find_package(rclcpp_action REQUIRED)
4
add_executable(send_trajectory src/send_trajectory.cpp)
5
ament_target_dependencies(send_trajectory rclcpp rclcpp_action control_msgs trajectory_msgs)

1
<ros2_control name="DiffDriveHW" type="system">
2
  <hardware>
3
    <plugin>mock_components/GenericSystem</plugin>
4
  </hardware>
5
  <joint name="wheel_left_joint">
6
    <command_interface name="velocity"/>
7
    <state_interface name="position"/>
8
    <state_interface name="velocity"/>
9
  </joint>
10
  <joint name="wheel_right_joint">
11
    <command_interface name="velocity"/>
12
    <state_interface name="position"/>
13
    <state_interface name="velocity"/>
14
  </joint>
15
</ros2_control>

1
controller_manager:
2
  ros__parameters:
3
    update_rate: 100
4
    diff_drive_controller:
5
      type: diff_drive_controller/DiffDriveController
6
    joint_state_broadcaster:
7
      type: joint_state_broadcaster/JointStateBroadcaster
8

9
diff_drive_controller:
10
  ros__parameters:
11
    left_wheel_names: ['wheel_left_joint']
12
    right_wheel_names: ['wheel_right_joint']
13
    wheel_separation: 0.4      # 轮距 0.4m
14
    wheels_per_side: 1
15
    wheel_radius: 0.1          # 轮半径 0.1m
16
    command_interfaces: ['velocity']
17
    state_interfaces: ['position', 'velocity']
18
    publish_rate: 50.0
19
    odom_frame_id: odom
20
    base_frame_id: base_link
21
    pose_covariance_diagonal: [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
22
    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]

1
左轮速度 = (v - ω × wheel_separation / 2) / wheel_radius
2
右轮速度 = (v + ω × wheel_separation / 2) / wheel_radius
3
其中 v=线速度，ω=角速度

1
// my_controller.cpp 骨架：自定义控制器
2
#include <controller_interface/controller_interface.hpp>
3
#include <rclcpp_lifecycle/state.hpp>
4

5
namespace my_robot {
6

7
class MyController : public controller_interface::ControllerInterface {
8
public:
9
  // 1. 声明参数（如控制增益）
10
  controller_interface::CallbackReturn on_init() override {
11
    auto_declare<std::vector<std::string>>("joints", {});
12
    auto_declare<double>("gain", 1.0);
13
    return controller_interface::CallbackReturn::SUCCESS;
14
  }
15

16
  // 2. 配置：获取硬件接口引用（命令 + 状态）
17
  controller_interface::CallbackReturn on_configure(
18
      const rclcpp_lifecycle::State &) override {
19
    joint_ = get_node()->get_parameter("joints").as_string_array()[0];
20
    gain_ = get_node()->get_parameter("gain").as_double();
21
    command_itf_ = std::find_if(
22
        command_interfaces_.begin(), command_interfaces_.end(),
23
        [this](const auto &itf) {
24
          return itf.get_interface_name() == "position" &&
25
                 itf.get_prefix_name() == joint_;
26
        });
27
    return controller_interface::CallbackReturn::SUCCESS;
28
  }
29

30
  // 3. 激活
31
  controller_interface::CallbackReturn on_activate(
32
      const rclcpp_lifecycle::State &) override {
33
    return controller_interface::CallbackReturn::SUCCESS;
34
  }
35

36
  // 4. 控制循环：读取参考，写命令（核心逻辑）
37
  controller_interface::return_type update(
38
      const rclcpp::Time &, const rclcpp::Duration &) override {
39
    // 简化：命令 = 固定值 × 增益（实际应订阅目标并计算）
40
    command_itf_->get().set_value(gain_ * 0.5);
41
    return controller_interface::return_type::OK;
42
  }
43

44
private:
45
  std::string joint_;
46
  double gain_;
47
  std::vector<controller_interface::InterfaceReference>::iterator command_itf_;
48
};
49

50
}  // namespace my_robot
51

52
#include "pluginlib/class_list_macros.hpp"
53
PLUGINLIB_EXPORT_CLASS(my_robot::MyController, controller_interface::ControllerInterface)

1
controller_manager:
2
  ros__parameters:
3
    my_controller:
4
      type: my_robot/MyController
5
my_controller:
6
  ros__parameters:
7
    joints: ['joint1']
8
    gain: 2.0