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 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
controller_manager:ros__parameters:update_rate: 100 # Hzuse_sim_time: true至此，声明了fishbot_ros2_control的宏，我们还需要在fish_robot.urdf.xacro中引用并使用。
注意，这里跟之前的gazebo两轮差速控制有冲突，所以，暂时注释掉。
<?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>重新构建并启动后。可以查询控制器管理器服务
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也可以查询控制器硬件接口；这个前面标签配置的一样
bohu@bohu-TM1701:~$ ros2 control list_hardware_interfaces
command interfacesleft_wheel_joint/effort [available] [unclaimed]left_wheel_joint/velocity [available] [unclaimed]right_wheel_joint/effort [available] [unclaimed]right_wheel_joint/velocity [available] [unclaimed]
state interfacesleft_wheel_joint/effortleft_wheel_joint/positionleft_wheel_joint/velocityright_wheel_joint/effortright_wheel_joint/positionright_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 添加
controller_manager:ros__parameters:update_rate: 100 # Hzuse_sim_time: truefishbot_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话题
bohu@bohu-TM1701:~$ ros2 topic echo /joint_states --once
header:stamp:sec: 3278nanosec: 486000000frame_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 查看
使用命令不方便，可以加入到启动脚本里
import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directorydef 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
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 添加
controller_manager:ros__parameters:update_rate: 100 # Hzuse_sim_time: truefishbot_joint_state_broadcaster:type: joint_state_broadcaster/JointStateBroadcasterfishbot_effort_controller:type: effort_controllers/JointGroupEffortControllerfishbot_effort_controller:ros__parameters:joints:- left_wheel_joint- right_wheel_jointcommand_interfaces:- effortstate_interfaces:- position- velocity- effortlaunch脚本
import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directorydef 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])
6.5.5 使用两轮差速控制器控制机器人
在fishbot_ros2_controller.yaml 添加
controller_manager:ros__parameters:update_rate: 100 # Hzuse_sim_time: truefishbot_joint_state_broadcaster:type: joint_state_broadcaster/JointStateBroadcasterfishbot_effort_controller:type: effort_controllers/JointGroupEffortControllerfishbot_diff_drive_controller:type: diff_drive_controller/DiffDriveControllerfishbot_effort_controller:ros__parameters:joints:- left_wheel_joint- right_wheel_jointcommand_interfaces:- effortstate_interfaces:- position- velocity- effortfishbot_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 signalwheel_radius: 0.032wheel_separation_multiplier: 1.0left_wheel_radius_multiplier: 1.0right_wheel_radius_multiplier: 1.0publish_rate: 50.0odom_frame_id: odombase_frame_id: base_footprintpose_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: trueenable_odom_tf: truecmd_vel_timeout: 0.5#publish_limited_velocity: trueuse_stamped_vel: false#velocity_rolling_window_size: 10参数比较多，参见书上表格6-3
修改启动launch脚本，为了防止冲突，注释掉力控制器
import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directorydef 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
