ros2笔记-6.5 使用ros2_control驱动机器人

ros2_control 是使用ros2进行机器人控制的框架。简化硬件的集成。

6.5.1 ros2_control安装

为什么要用ros2_contrl.书上、视频上小鱼老师介绍的比较清楚，这里放个control框架图。

安装：

sudo apt install ros-$ROS_DISTRO-ros2-control

sudo apt install ros-$ROS_DISTRO-ros2-controllers

6.5.2 使用gazebo 介入ros2_control

安装依赖插件：

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

在目录src/fishbot_description/urdf/fishbot/下新建文件fishbot.ros2_control.xacro，代码如下：

XML 复制代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="fishbot_ros2_control">
        <ros2_control name="FishBotGazeboSystem" type="system">
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>
            <joint name="left_wheel_joint">
                <!--控制接口-->
                <command_interface name="velocity">
                    <param name="min">-1</param>
                    <param name="max">1</param>
                </command_interface>
                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <!--状态接口 -->
                <state_interface name="position" />
                <state_interface name="velocity" />
                <state_interface name="effort" />
            </joint>
            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1</param>
                    <param name="max">1</param>
                </command_interface>
                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <state_interface name="position" />
                <state_interface name="velocity" />
                <state_interface name="effort" />
            </joint>
        </ros2_control>
        <gazebo>
        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
            <parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
        </plugin>
        </gazebo>
    </xacro:macro>
</robot>

定义了一个fishbot_ros2_control 宏，标签ros2_control 描述硬件资源。

下面的关节joint,定义了左右两个轮子，有控制接口跟状态接口。

下面的gazebo标签定义了用来解析ros2_control的标签的插件。里面的参数需要config目录新建文件：fishbot_ros2_controller.yaml

XML 复制代码

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_time: true

至此，声明了fishbot_ros2_control的宏，我们还需要在fish_robot.urdf.xacro中引用并使用。

注意，这里跟之前的gazebo两轮差速控制有冲突，所以，暂时注释掉。

XML 复制代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fishbot">
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro" />
    <!-- 传感器组件 -->
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro" />
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.xacro" />
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/camera.urdf.xacro" />
    <!-- 执行器组件 -->
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/wheel.urdf.xacro" />
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/caster.urdf.xacro" />
    <!--gazebo插件-->
    <!-- <xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_control_plugin.xacro" /> -->
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro" />
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/fishbot.ros2_control.xacro" />


    <xacro:base_xacro length="0.12" radius="0.1" />
    <!-- 传感器 -->
    <xacro:imu_xacro xyz="0 0 0.02" />
    <xacro:laser_xacro xyz="0 0 0.10" />
    <xacro:camera_xacro xyz="0.10 0 0.075" />
       <!-- 执行器主动轮+从动轮 -->
    <xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06" />
    <xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06" />
    <xacro:caster_xacro caster_name="front_caster" xyz="0.08 0.0 -0.076" />
    <xacro:caster_xacro caster_name="back_caster" xyz="-0.08 0.0 -0.076" />

    <!-- <xacro:gazebo_control_plugin /> -->
    <xacro:gazebo_sensor_plugin />
    <xacro:fishbot_ros2_control />
</robot>

重新构建并启动后。可以查询控制器管理器服务

bash 复制代码

bohu@bohu-TM1701:~$ ros2 service list|grep controller
/controller_manager/configure_controller
/controller_manager/describe_parameters
/controller_manager/get_parameter_types
/controller_manager/get_parameters
/controller_manager/list_controller_types
/controller_manager/list_controllers
/controller_manager/list_hardware_components
/controller_manager/list_hardware_interfaces
/controller_manager/list_parameters
/controller_manager/load_controller
/controller_manager/reload_controller_libraries
/controller_manager/set_hardware_component_state
/controller_manager/set_parameters
/controller_manager/set_parameters_atomically
/controller_manager/switch_controller
/controller_manager/unload_controller

也可以查询控制器硬件接口；这个前面标签配置的一样

bash 复制代码

bohu@bohu-TM1701:~$ ros2 control list_hardware_interfaces 
command interfaces
	left_wheel_joint/effort [available] [unclaimed]
	left_wheel_joint/velocity [available] [unclaimed]
	right_wheel_joint/effort [available] [unclaimed]
	right_wheel_joint/velocity [available] [unclaimed]
state interfaces
	left_wheel_joint/effort
	left_wheel_joint/position
	left_wheel_joint/velocity
	right_wheel_joint/effort
	right_wheel_joint/position
	right_wheel_joint/velocity

6.5.3 使用关节状态发布控制器

在刚做完的6.5.2,此时进入rviz会发现轮子异常，缺少了轮子到base_footprint的TF转换。跟我之前报错一样，这是因为刚注销了gazebo插件的导致的。我们可以使用ros_control的关节状态控制器，发布/joint_states 话题，然后有robot_sate_publiser转换为TF数据发布。

在fishbot_ros2_controller.yaml 添加

bash 复制代码

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_time: true
    fishbot_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

构建后启动。并使用命令行加载激活fishbot_joint_state_broadcaster控制器。

bohu@bohu-TM1701:~$ ros2 control load_controller fishbot_joint_state_broadcaster --set-state active

Successfully loaded controller fishbot_joint_state_broadcaster into state active

此时，rviz能显示正常，也能通过命令行查看joint_states话题

bash 复制代码

bohu@bohu-TM1701:~$ ros2 topic echo /joint_states --once
header:
  stamp:
    sec: 3278
    nanosec: 486000000
  frame_id: ''
name:
- left_wheel_joint
- right_wheel_joint
position:
- -0.00011715882368168451
- 0.00010725719480220164
velocity:
- -3.353251441562582e-06
- -5.942979131065872e-07
effort:
- 0.0
- 0.0
---

rqt 查看

使用命令不方便，可以加入到启动脚本里

python 复制代码

import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取功能包默认路径
    urdf_tutorial_path = get_package_share_directory('fishbot_description')
    default_model_path = urdf_tutorial_path + '/urdf/fishbot/fish_robot.urdf.xacro'
    default_world_path = urdf_tutorial_path + '/world/custom_room.world'
    # 为 Launch 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', default_value=str(default_model_path),
        description='加载模型文件的绝对路径')
    # 获取文件内容生成新的参数
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
    # 状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )


    # 通过 IncludeLaunchDescription 包含另外一个 launch 文件
    launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource([get_package_share_directory(
            'gazebo_ros'), '/launch', '/gazebo.launch.py']),
      	# 传递参数
        launch_arguments=[('world', default_world_path),('verbose','true')]
    )
    #请求gazebo 加载机器人
    action_spawn_entity = launch_ros.actions.Node(
        package= 'gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic', '/robot_description','-entity', 'fishbot']
    )

        # 加载并激活 fishbot_joint_state_broadcaster 控制器
    load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
            'fishbot_joint_state_broadcaster'],
        output='screen'
    )


    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        robot_state_publisher_node,
        launch_gazebo,
        action_spawn_entity,
        #事件动作，当加载机器人结束后执行
        launch.actions.RegisterEventHandler(
            event_handler= launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[load_joint_state_controller],
            )
        )
    ])

查看已经加载的控制器和状态

bohu@bohu-TM1701:~$ ros2 control list_controllers

fishbot_joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active

bash 复制代码

bohu@bohu-TM1701:~$ ros2 control set_controller_state fishbot_joint_state_broadcaster inactive
[INFO] [1736821412.447120508] [_ros2cli_80444]: waiting for service /controller_manager/list_controllers to become available...
Successfully deactivated fishbot_joint_state_broadcaster
bohu@bohu-TM1701:~$ ros2 control list_controllers
fishbot_joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster  inactive
bohu@bohu-TM1701:~$ ros2 control unload_controller fishbot_joint_state_broadcaster
Successfully unloaded controller fishbot_joint_state_broadcaster
bohu@bohu-TM1701:~$ ros2 control list_controllers
No controllers are currently loaded!

有不同的命令来设置状态及卸载控制器。

6.5.4 使用力控制器控制轮子

在fishbot_ros2_controller.yaml 添加

bash 复制代码

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_time: true
    fishbot_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    fishbot_effort_controller:
      type: effort_controllers/JointGroupEffortController

fishbot_effort_controller:
  ros__parameters:
    joints:
      - left_wheel_joint
      - right_wheel_joint
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

launch脚本

python 复制代码

import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取功能包默认路径
    urdf_tutorial_path = get_package_share_directory('fishbot_description')
    default_model_path = urdf_tutorial_path + '/urdf/fishbot/fish_robot.urdf.xacro'
    default_world_path = urdf_tutorial_path + '/world/custom_room.world'
    # 为 Launch 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', default_value=str(default_model_path),
        description='加载模型文件的绝对路径')
    # 获取文件内容生成新的参数
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
    # 状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )


    # 通过 IncludeLaunchDescription 包含另外一个 launch 文件
    launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource([get_package_share_directory(
            'gazebo_ros'), '/launch', '/gazebo.launch.py']),
      	# 传递参数
        launch_arguments=[('world', default_world_path),('verbose','true')]
    )
    #请求gazebo 加载机器人
    action_spawn_entity = launch_ros.actions.Node(
        package= 'gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic', '/robot_description','-entity', 'fishbot']
    )

        # 加载并激活 fishbot_joint_state_broadcaster 控制器
    load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
            'fishbot_joint_state_broadcaster'],
        output='screen'
    )
        # 加载并激活 fishbot_effort_controller 控制器
    load_fishbot_effort_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_effort_controller'], 
        output='screen')



    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        robot_state_publisher_node,
        launch_gazebo,
        action_spawn_entity,
        #事件动作，当加载机器人结束后执行
        launch.actions.RegisterEventHandler(
            event_handler= launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[load_joint_state_controller],
            )
        ),
         launch.actions.RegisterEventHandler(
            event_handler= launch.event_handlers.OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[load_fishbot_effort_controller],
            )
        )
    ])

重新构建，运行，可以查看力控的话题

bohu@bohu-TM1701:~$ ros2 topic list -t |grep effort

/fishbot_effort_controller/commands [std_msgs/msg/Float64MultiArray]

/fishbot_effort_controller/transition_event [lifecycle_msgs/msg/TransitionEvent]

使用命令控制轮子扭矩

bohu@bohu-TM1701:~$ ros2 topic pub /fishbot_effort_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.0001, 0.0001]}"

publisher: beginning loop

publishing #1: std_msgs.msg.Float64MultiArray(layout=std_msgs.msg.MultiArrayLayout(dim=[], data_offset=0), data=[0.0001, 0.0001])

publishing #2: std_msgs.msg.Float64MultiArray(layout=std_msgs.msg.MultiArrayLayout(dim=[], data_offset=0), data=[0.0001, 0.0001])

此时，在gazebo中机器人会缓缓移动

6.5.5 使用两轮差速控制器控制机器人

在fishbot_ros2_controller.yaml 添加

python 复制代码

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_time: true
    fishbot_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    fishbot_effort_controller:
      type: effort_controllers/JointGroupEffortController
    fishbot_diff_drive_controller:
      type: diff_drive_controller/DiffDriveController
  

fishbot_effort_controller:
  ros__parameters:
    joints:
      - left_wheel_joint
      - right_wheel_joint
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

fishbot_diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]

    wheel_separation: 0.20
    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
    wheel_radius: 0.032

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    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]

    open_loop: true
    enable_odom_tf: true

    cmd_vel_timeout: 0.5
    #publish_limited_velocity: true
    use_stamped_vel: false
    #velocity_rolling_window_size: 10

参数比较多，参见书上表格6-3

修改启动launch脚本，为了防止冲突，注释掉力控制器

python 复制代码

import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取功能包默认路径
    urdf_tutorial_path = get_package_share_directory('fishbot_description')
    default_model_path = urdf_tutorial_path + '/urdf/fishbot/fish_robot.urdf.xacro'
    default_world_path = urdf_tutorial_path + '/world/custom_room.world'
    # 为 Launch 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', default_value=str(default_model_path),
        description='加载模型文件的绝对路径')
    # 获取文件内容生成新的参数
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
    # 状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )


    # 通过 IncludeLaunchDescription 包含另外一个 launch 文件
    launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource([get_package_share_directory(
            'gazebo_ros'), '/launch', '/gazebo.launch.py']),
      	# 传递参数
        launch_arguments=[('world', default_world_path),('verbose','true')]
    )
    #请求gazebo 加载机器人
    action_spawn_entity = launch_ros.actions.Node(
        package= 'gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic', '/robot_description','-entity', 'fishbot']
    )

        # 加载并激活 fishbot_joint_state_broadcaster 控制器
    load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
            'fishbot_joint_state_broadcaster'],
        output='screen'
    )
        # 加载并激活 fishbot_effort_controller 控制器
    # load_fishbot_effort_controller = launch.actions.ExecuteProcess(
    #     cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_effort_controller'], 
    #     output='screen')

    load_diff_drive_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_diff_drive_controller'], 
        output='screen')


    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        robot_state_publisher_node,
        launch_gazebo,
        action_spawn_entity,
        #事件动作，当加载机器人结束后执行
        launch.actions.RegisterEventHandler(
            event_handler= launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[load_joint_state_controller],
            )
        ),
         launch.actions.RegisterEventHandler(
            event_handler= launch.event_handlers.OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[load_diff_drive_controller],
            )
        )
    ])

重新构建，运行；

打开终端， ros2 run teleop_twist_keyboard teleop_twist_keyboard

使用键盘控制机器人运动。