背景

制作ROS小车，底盘使用 STM32 驱动两轮差速小车，与 ROS2 上位机串口通信，通过 ros2 control 框架控制底盘，控制器使用 diff-drive-controller， 新建 hardware interface  读取串口传输的角速度，计算角位移；同时 diff-drive-controller 接收 ROS2 topic  /cmd_vel， 计算下发各轮角速度至底盘。编写主要参考B站视频 ros2_control教程，全(内)网第一份_哔哩哔哩_bilibili，ros2 control 框架介绍可看这篇博文 【ROS2】ros2-control介绍 - 知乎

底盘硬件

• 芯片STM32 F103C8T6

• 串口转USB芯片CH340G

• 电机驱动TB6612 两轮差速结构

开发环境                    

• Ubuntu 22.04 LTS ros2 humble

• STM32CubeIDE 1.9.0

• ros2 control 框架 

  Welcome to the ros2_control documentation! — ROS2_Control: Rolling Mar 2025 documentationhttps://control.ros.org/rolling/index.html

实现

一. 环境准备

1. 安装 ros2 control 和 controllers

sudo apt install ros-$ROS_DISTRO-ros2-control
sudo apt install ros-$ROS_DISTRO-ros2-controllers

2. 使用 RosTeamWorkspace (RTW)  工具，生成 ros2 control hardware interface 代码模版。

RTW 是由卡尔斯鲁厄理工学院 (KIT) 的 Dr. Denis Stogl 发起的一个框架工具，可生成 ROS 的一些代码模版。目前由他的公司 Stogl Robotics 维护。

Welcome the documentation of ROS Team Workspace-Framework — ROS Team Workspace Documentation: Humble Mar 2025 documentationhttps://rtw.b-robotized.com/master/index.html

 按如下地址 clone 代码仓库

git clone https://github.com/StoglRobotics/ros_team_workspace.git

 二. 创建 hardware 功能包

1. 新建 ros 功能包 并使用RTW初始化

• ros2 新建 ament_cmake 类型的 hardware 功能包 myfs_hw_interface，colcon build 后 source install/setup.bash

• 切换 clone 的 ros_team_workspace 目录下 source setup.bash

• 再次返回 ros2 功能包 myfs_hw_interface 下使用如下命令初始化，其中 FILE_NAME  指的是包中的 /src 下的cpp 文件名， [CLASS_NAME] 指文件中 hardware interface 实际的类名。根据指示选择 license-header, 控制类型(system, sensor 或 actuator)。执行后生成代码模版

ros2_control_setup-hardware-interface-package FILE_NAME [CLASS_NAME]

2. 在生成的代码框架中编写硬件接口URDF

• 新建 URDF文件夹，新建 myxxxbot.urdf.xacro 内容如下。(省略机器人模型）

  <?xml version="1.0"?>
  <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mybot">   
      <xacro:include filename="文件路径/myxxxbot.ros2_control.xacro"/>
      ......
  
      <xacro:myxxxbot_ros2_control />    
      ......
  </robot>

• 新建硬件接口描述文件 myxxxbot.ros2_control.xacro 使用 <ros2_control> 标签，type= 可选三个值 system, actuator 或 sensor。<hardware> 下的 <plugin> 内写我们要用到的硬件接口，即我们之后要写的接口类，其格式为 pkgname / [CLASS_NAME] 即我们上一步生成模板时定义的类名。本例为myfs_hw_interface/MyfsBotHwInterface。 <joint> 为要控制的节点，本例为两个轮子，每个轮子反馈当前角速度，角位移，并接收角速度指令，由 <state_interface name="velocity"/> ，<state_interface name="position"/> 和 <command_interface name="velocity"/>描述。角位移 position 是由角速度积分计算的，如不需要使用，此处可以不写，并在下面第四步的 controller manager 的 .yaml 配置文件中声明 position_feedback=false

• <?xml version="1.0"?>
  <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot_name">
    <xacro:macro name="myxxxbot_ros2_control">
      <ros2_control name="xxxbot" type="system">
        <hardware>
          <plugin>功能包/类名</plugin>
        </hardware>
        <joint name="left_wheel_joint">
          <command_interface name="velocity"/>
          <state_interface name="position"/>
          <state_interface name="velocity"/>
        </joint>
        <joint name="right_wheel_joint">
          <command_interface name="velocity"/>
          <state_interface name="position">
          <state_interface name="velocity"/>
        </joint>
      </ros2_control>
    </xacro:macro>
  </robot>

• 真实硬件的配置就到此为止。如没有硬件，使用Gazebo仿真的话，<hardware> 下使用 <plugin> gazebo_ros2_control/GazeboSystem </plugin> 并在 </ ros2_control>外新增如下内容。.yaml 文件为下面第四步编写的控制器配置描述文件。<remapping> 将控制器订阅和发布的话题名字重映射为其他节点(如键盘控制节点) 发布的一般话题名。

  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <parameters>
        $(find myfs_hw_interface)/config/mybot_ros2_controller.yaml
      </parameters>
      <ros>                   
        <remapping>控制器名/cmd_vel_unstamped:=/cmd_vel</remapping>
        <remapping>/控制器名/odom:=/odom</remapping>
      </ros>
    </plugin>
  </gazebo>

3. 在生成的代码框架中编写hardware interface

生成的头文件如下，类名 myfs_hw_interface::MyfsBotHwInterface 继承自基类hardware_interface::SystemInterface（类定义见 ros2 control 源码仓库：ros2_control/hardware_interface/include/hardware_interface/system_interface.hpp at master · ros-controls/ros2_control · GitHub)。私有变量 std::vector<double> hw_commands_, hw_states_ vel_ 和 hw_states_pos_ 是三个 double 一维向量数组，用于和硬件交互。本项目需要下发角速度给两个车轮，及读取两个车轮角速度，再由反馈的角速度积分计算角位移。根据 URDF 文件描述的硬件定义，共有左右车轮两个 joint，hw_commands_[ ] 用来存储两个元素即左右轮速度命令，hw_states_vel_[ ] 和 hw_states_pos_[ ] 也仅存储两个元素即左右车轮的角速度反馈及角位移。（如果还定义了 erfort 等其他 state_interface 的话 ，需要新增向量数组例如 hw_states_erf_[ ] 来存储）

• hw_states_ vel_[ ] 用来存储从各个 <joint> ( 或者 sensor， GPIO) 读来的 state_interface 数值，本例中，hw_states_val_[0] 存储读来的 left_wheel_joint 的角速度，hw_states_vel_[1] 存储读来的 right_wheel_joint 的角速度。之后在 export_state_interfaces() 方法中将元素的指针传递给 state_interfaces[ ] 向量, 供 resource manager 使用。

• hw_states_ pos_[ ] 用来存储根据角速度积分计算的角位移值。计算在 read() 函数中实现。

• hw_commands_[ ] 用来存储将要写入各个 <joint> ( 或sensor，GPIO) 的command_interface 值。本例中，hw_commands_[0] 存储将要写入 left_wheel_joint 的速度。 hw_commands_[1] 存储将要写入 right_wheel_joint 的速度。之后在 export_command_interfaces() 方法中将元素的指针传递给 command_interfaces[ ] 向量, 供 resource manager 使用。

• SerialData serial_data_ 为自己定义的串口通信类。创建对象初始化串口。

头文件内容：

namespace myfs_hw_interface
{
class MyfsBotHwInterface : public hardware_interface::SystemInterface
{
public:
  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
  std::vector<double> hw_commands_; // 存储将要写入硬件两个轮子 joint 的速度
  std::vector<double> hw_states_vel_;   // 存储硬件两个轮子 joint 传来的速度
  std::vector<double> hw_states_pos_;   // 存储硬件两个轮子 joint 的位置(通过计算)

  SerialData serial_data_;          // 创建串口通讯类对象 初始化串口
};

}  // namespace myfs_hw_interface

•  on_init 方法，初始化。入参 info 为 hardware_interface::HardwareInfo 类型，该类型为结构体，包含 URDF 文件中定义的信息，主要如本例中设备名字 xxxbot，硬件类型 system, plugin 名字，ComponentInfo 类的向量数组 joints， sensors等。其中数组 .joints 中存储 joint 的名字，state_interfaces，command interface 等信息 。首先通过 on_init(info) 将入参赋给info_。  info_.joints.size() 记录硬件 joint 的数量，本例为2。将 hw_commands_[ ], hw_states_vel_[ ] 及 hw_states_pos_[ ] 初始化为长度为2的向量，初值NaN。

hardware_interface::CallbackReturn MyfsBotHwInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
  {
    return CallbackReturn::ERROR;
  }

  // TODO(anyone): read parameters and initialize the hardware
  hw_states_vel.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_states_pos_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  return CallbackReturn::SUCCESS;
}

• on_corfige 方法，硬件初始化。在类的私有变量中已定义了串口通信对象serial_data_，我在该类的构造函数中对串口进行了初始化，此处不别特别操作。

hardware_interface::CallbackReturn MyfsBotHwInterface::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces


  return CallbackReturn::SUCCESS;
}

• export_state_interfaces()方法, 将读到的数值(的地址)与其 joint (或sensor) 的描述等封装成成 hardware_interface::StateInterface 类的向量 state_interfaces[ ] 返回. 该向量的每个元素对应 URDF 硬件描述中的一条 <joint>（或者sensor， gpio）的 state_interface，如本例是两个轮子：<joint name="left_wheel_joint"> 的 <state_interface name="velocity"/>, <state_interface name="position"/>  和<joint name=right_wheel_joint">的 <state_interface name="velocity"/>, <state_interface name="position"/>。StateInterface 类构造函数有三个参数: 字符串 prefix_name, interface_name, 和指针 double * value_ptr。prefix_name, interface_name 会组合成类似 "left_wheel_joint /velocity" 形式的 handle_name_，指针参数则需要将 hw_states_vel_ 各值的指针传入，如 &hw_states_vel_[0]。循环中构造函数第二个参数对应写 hardware_interface::HW_IF_POSITION 或 hardware_interface::HW_IF_VELOCITY（或者info_joints[i].state_interfaces[0].name）。

std::vector<hardware_interface::StateInterface> MyfsBotHwInterface::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_vel_[i]));

    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_pos_[i]));    
  }

  return state_interfaces;
}

• export_command_interfaces(), 将要写入的数值（的地址）与其 joint（或sensor）的描述等封装成成 hardware_interface::CommandInterface 类的向量 command_interfaces 返回。和上面类似，该向量的每一个元素对应 URDF 硬件描述中的一条 command_interface 及其要写入的值的地址。如本例，第一个元素 command_interfaces_[0] 对应 <joint name="left_wheel_joint"> 的 <command_interface name="velocity"/> 和 hw_commands_[0] 的地址。将默认生成的 hardware_interface::HW_IF_POSITION 改为 HW_IF_VELOCITY

std::vector<hardware_interface::CommandInterface> MyfsBotHwInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      // TODO(anyone): insert correct interfaces
      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
  }

  return command_interfaces;
}

• on_activate 方法，初始化 hw_commands_[ ]， hw_states_vel_[ ] 和 hw_states_pos_[ ]。

hardware_interface::CallbackReturn MyfsBotHwInterface::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to receive commands
  
  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    hw_commands_[i] = 0;
    hw_states_vel_[i] = 0;
    hw_states_pos_[i] = 0;
  }
  return CallbackReturn::SUCCESS;
}

•  on_deactivate 方法 无需特别释放。此处可以不更改。

hardware_interface::CallbackReturn MyfsBotHwInterface::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to stop receiving commands

  return CallbackReturn::SUCCESS;
}

•  read 函数，从串口读取底盘传来的速度值，调用.serial_data_.getLeftSpeed() 和 serial_data_.getRightSpeed() 方法存入向量数组 hw_states_vel_[ ]。我在下位机编写的串口通信时对数据进行了 *10 的处理，这里需要 /10 还原。hw_states_pos_[ ] 由角速度积分计算。通过前一次的角位移值加上 period 传入的时间值 * 角速度计算。默认生成的函数参数 period 为注释状态，需去掉注释。

hardware_interface::return_type MyfsBotHwInterface::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
  // TODO(anyone): read robot states

  hw_states_vel_[0] = serial_data_.getLeftSpeed() / 10;  
  hw_states_vel_[1] = serial_data_.getRightSpeed() / 10;  // 将串口传来的*10的数据还原 单位rad/s

  hw_states_pos_[0] = hw_states_pos_[0] + period.seconds() * hw_states_vel_[0];  // 通过角速度积分计算角位移
  hw_states_pos_[1] = hw_states_pos_[1] + period.seconds() * hw_states_vel_[1];

  return hardware_interface::return_type::OK;
}

•  write 函数，通过 serial_data_.sendCmdSpeed() 方法将 hw_commands_[ ] 数组中的速度值写入串口。  

hardware_interface::return_type MyfsBotHwInterface::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // TODO(anyone): write robot's commands'
serial_data_.sendCmdSpeed(int(hw_commands_[0] * 1000), int(hw_commands_[1] * 1000));
  
return hardware_interface::return_type::OK;
}

•  PLUGINLIB_EXPORT_CLASS 导出共享库插件。在最后需要导出插件类。该部分自动生成无需更改。第一个参数就是这个自定义的接口类，第二个参数是其基类。

  #include "pluginlib/class_list_macros.hpp"
  
  PLUGINLIB_EXPORT_CLASS(
    myfs_hw_interface::MyfsBotHwInterface, hardware_interface::SystemInterface)

•  共享库插件描述.xml文件。自动生成，无需更改。

  <library path="myfs_hw_interface">
    <class name="myfs_hw_interface/MyfsBotHwInterface"
           type="myfs_hw_interface::MyfsBotHwInterface"
           base_class_type="hardware_interface::SystemInterface">
      <description>
        ros2_control hardware interface.
      </description>
    </class>
  </library>

• CMakeList，自动生成，按需修改。这里我将自己写的串口类和其依赖加入。

   

  ......
  
  add_library(
    myfs_hw_interface
    SHARED
    src/myfsbot_hw_interface.cpp
    src/ser_data.cpp  # 自定义的串口通信类
  )
  
  ament_target_dependencies(
    myfs_hw_interface
    hardware_interface
    rclcpp
    rclcpp_lifecycle
    serial  # 自定义的串口类的依赖
    pluginlib
  )
  ......

4. 编写 controller manager 的 .yaml 文件配置控制器

加载两个控制器：

1. 使用两轮差速控制器 diff_drive_controller 订阅速度命令 /cmd_vel，通过计算，耦合上面编写的hardware_interface 最终下发左右轮角速度至底盘。

2. 使用 joint_state_broadcaster，发布 /joint_states 等给节点 robot_state_publisher 发布 /tf

• controller_manager: 定义两个控制器，名字随意起，type 为ros2 controller 定义的，格式为 功能包名/类名。 本例需要两轮差速控制器 diff_drive_controller/DiffDriveController 和节点状态发布控制器 joint_state_broadcaster/JointStateBroadcaster。ros2 control控制器源官方源码GitHub - ros-controls/ros2_controllers: Generic robotic controllers to accompany ros2_control

• 之后在mybot_diff_drive_controller: 下配置控制器的参数。注意左右轮名称应与 URDF 硬件描述一致。需要设置左右轮距，半径等物理量。更多参数配置请参考官方文档：diff_drive_controller — ROS2_Control: Rolling May 2025 documentation

• 如不需要返回 position 参数，写 position_feedback=false，这样硬件只需返回velocity。不写默认为 position_feedback=true。

• 如果使用gazebo仿真，在controller_manager 下的 ros__parameters 里配置 use_sim_time: true。注意不仿真时候一定要去掉，否则会导致控制器加载超时卡死controller_manager

controller_manager:
  ros__parameters:      # 注意是两个下划线
    update_rate: 10     # Hz
    # use_sim_time: true 不使用仿真时不要打开 不然 controller manager 会卡死
    einbot_joint_state_bro:
      type: joint_state_broadcaster/JointStateBroadcaster
    einbot_diff_drive_controller:
      type: diff_drive_controller/DiffDriveController
    
einbot_diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]

    wheel_separation: 0.19
    wheel_radius: 0.0325
    
    publish_rate: 50.0       # Hz 里程计和TF的发布频率 默认50
    odom_frame_id: odom
    base_frame_id: base_footprint
    pose_covariance_diagonal: [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]

    # position_feedback: false # 硬件不返回 position, joint 的 state_interface 无 position
    open_loop: true
    enable_odom_tf: true     # 发布odom_frame_id 和 base_frame_id 间的TF
  
    cmd_vel_timeout: 0.5
    #publish_limited_velocity: true
    use_stamped_vel: false

5. 编写 Launch 文件，顺序启动 controller manager 和 controller

lauch 文件需要实现启动 controller_manager，joint_state_broadcaster 和 diff_drive_controller。controller_manager 需订阅模型描述话题 /robot_description 从而加载 URDF 的硬件描述，故还需启动 robot_state_publisher 节点读取 URDF 文件发布 /robot_description。代码如下：

import os
import launch
import launch.event_handlers
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros.parameter_descriptions

def generate_launch_description():

    package_name = 'myfs_hw_interface'
    pkg_share_path = get_package_share_directory(package_name);

    # 获取各配置文件的路径
    model_path = os.path.join(pkg_share_path, 'urdf', 'einbot.urdf.xacro')
    controllers_config = os.path.join(pkg_share_path, 'config', 'einbot_ros2_controller.yaml')
    
    # 通过 xacro 获取 URDF 描述文件
    robot_description_content = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(['xacro ', model_path]),
        value_type=str
    )
    robot_description = {'robot_description': robot_description_content};

    # 加载 ros2 controller manager
    controller_manager_node = launch_ros.actions.Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[controllers_config], # 不需要传入 robot_description, node会订阅
        output="both",
        remappings=[("~/robot_description", "/robot_description"),          # controller_manager 订阅的话题是 '~/robot_description, 需要重映射为robot_state_publisher发布的话题名
                    ("/einbot_diff_drive_controller/cmd_vel", "/cmd_vel"),  # 将差速控制器默认发布的话题重映射为一般的 /cmd_vel
                    # ("/einbot_diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"), # use_stamped_vel: falses 时的 cmd_vel
                    ("/einbot_diff_drive_controller/odom", "/odom"),],      # 将差速控制器默认发布的话重映射成/odom                    
    )
    # 状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[robot_description]
    )  
    # 启动各 controller
    joint_state_broadcaster_spawner = launch_ros.actions.Node(
        package="controller_manager",
        executable="spawner",
        arguments=["einbot_joint_state_bro",   # 写入在yaml文件中定义的控制器名字
                   "--controller-manager", 
                   "/controller_manager"], 
    )
    diff_drive_spawner = launch_ros.actions.Node(
        package="controller_manager",
        executable="spawner",
        arguments=["einbot_diff_drive_controller",  # 写入在yaml文件中定义的控制器名字
                   "--controller-manager", 
                   "/controller_manager"], 
    )
    # 在启动controller manager后, 启动joint_state_controller 的代码块
    delay_joint_state_broadcaster_spawner = launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessStart(
            target_action=controller_manager_node, 
            on_start=[launch.actions.TimerAction(period=2.0, actions=[joint_state_broadcaster_spawner])], # 延时5秒
        )
    ) 
    # 按顺序激活控制器
    delay_diff_drive_spawner = launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessStart(
            target_action=joint_state_broadcaster_spawner, 
            on_start=[launch.actions.TimerAction(period=2.0, actions=[diff_drive_spawner])], # 延时5秒

        )
    )

    return launch.LaunchDescription([
        controller_manager_node,
        robot_state_publisher_node,

        # 需在启动controller manager后, 启动各控制器
        delay_joint_state_broadcaster_spawner,
        delay_diff_drive_spawner,

    ])

• contronller manager 控制节点， parameters=[ ] 写入之前定义的 .yaml 配置文件的路径，这里不需要特别写入URDF描述文件，因为节点启动后会订阅话题 /robot_description。remappings=[ ]，做一些重映射，请注意要加载的控制器的话题重映射都在这里完成：1. 将 manager 订阅的描述文件名重映射为 robot_state_publisher 发布的 /robot_description；2. 将之后要加载的差速控制器 diff-drive-controller 发布的里程计 xxx/odom 和订阅的 xxx/cmd_vel 话题名重映射，去掉命名空间xxx。注意 diff-drive-controller 根据 .yaml 中的控制器配置use_stamped_vel: true或false 订阅的话题分为 cmd_vel 或 cmd_vel_unstamped。

• robot_state_publisher 节点， parameters=[ ] 写入完整的 URDF 文件描述。

• joint_state_broadcaster_spawner，通过 controller_manager 的 spawner 加载控制器， arguments=[ ]，传入在 .yaml 文件中自己定义的状态发布控制器名称，本例是einbot_joint_state_bro。

• diff_drive_spawner ，同样 spawner 加载控制器， arguments=[ ]，传入在 .yaml 定义的两轮差速控制器名称，本例是 einbot_diff_drive_controller。

• delay_joint_state_broadcaster_spawner ，delay_diff_drive_spawner，在加载控制器时，controller manager 需要先运行，在此做延迟处理。

6. 运行

colcon build 工作包后，运行 launch 文件，各节点及话题情况如下图：

可以在命令行查看 hardware_interface 的情况，输入以下命令，可以看到控制器已经正确加载，命令接口处于[claimed] 状态。

xxx:~$ ros2 control list_hardware_interfaces 

command interfaces
	left_wheel_joint/velocity [available] [claimed]
	right_wheel_joint/velocity [available] [claimed]
state interfaces
	left_wheel_joint/position
	left_wheel_joint/velocity
	right_wheel_joint/position
	right_wheel_joint/velocity 

总结

用 ros2 control 框架驱动两轮差速小车的一般步骤：

• 建立 ros 工作包，使用 RTW 工具 (或使用官方demo修改) 生成 hardware_interface 的框架

• 编写硬件描述 URDF 文件，定义 joint 及接口 

• 编写 hardware_interface 文件，对串口进行读取及发送操作。生成共享库插件类。

• 编写 .yaml 配置文件配置控制器 diff_drive_controller 和 joint_state_broadcaster

• 编写 launch 文件：1. 调用 robot_state_publisher 加载 URDF 传给 controller manager；2. 调用controller manager， 传入配置文件.yaml；3. 再调用控制器 diff_drive_controller 和 joint_state_broadcaster。

最终实现通过上位机的 /cmd_vel 命令接口，控制两轮差速小车底盘的功能。