一、ros2_control驱动

1、为什么需要？

ROS的一大宗旨就是拒绝重复造轮子，但拿前面创建移动机器人来说，当使用仿真完成一个一个验证后，需要移植到真机调试时，真机上并没有像两轮差速控制器这样的插件供你使用，这时需要重头编写运动控制算法，显然这种重复造轮子的行为是被ROS所拒绝的。

ros2_control就是使用ROS2进行（实时）机器人控制的框架，他的目标是简化硬件的集成。通过它就可以避免上述例子中的重复造轮子，让机器人对硬件移植更加友好。

2、介绍：

ros2_control目的是减少重复造轮子，简化硬件集成。因此第一步我们要找出重复在哪里？以gazebo两轮差速插件为例，该插件至少包含了两部分逻辑

1）第一部分是数据的收发部分，即从gazebo中拿到两个轮子速度传感器的当前转速，并将目标速度发送出去。

2）第二部分是控制部分，控制器的组成有两个：第一个是控制器负责根据轮子的反馈的转速，结合轮子直径和轮距计算里程计；第二个是根据收到机器人控制命令，计算轮子的目标速度。

对于一个真实的两轮差速结构的机器人来说，控制器和数据接口两部分都不能少，只不过数据接口是用来和真实硬件进行交互的。由于数据接口要对接硬件层，这里不算重复的部分，所以说重复的部分就是控制器部分。

了解了哪里到哪里重复以后，下一步就是考虑如何设计才能避免重复造轮子，那既然控制器重复了，把控制器抽出来不就行了吗？让真实硬件和gazebo都用同一个控制器。但控制器是依赖于数据接口的，所以要实现同一个控制器，就要先统一数据接口，ros2_control的设计就这样的，而且比我们想的更强大。

ros2_control结构图

从下往上看，先从数据接口部分来看，ros2_control将硬件资源分为传感器还有执行器以及系统三类，不仅可以控制关节转动，还可以读取关节的角度信息，这种复杂的设备就可以使用系统来表示。从ros图中可以看出，这三种类型的资源数据接口，根据用途可以分为只读（状态）接口和用于控制命令的接口，最终使用接口管理器来管理这三种接口。上面部分：控制器可以有多个，不同的控制器可能使用不同的数据接口，所以ros2_control使用控制器管理器来实现对各个控制器的管理。

3、安装：

sudo apt install ros-$ROS_DISTRO-ros2-controllers

ros2 control --help

查看有哪些控制器：sudo apt info ros-$ROS_DISTRO-ros2-controllers

apt info 指令用于查看某包的具体信息，其中里面的Depends这部分是这个包的依赖列表，当安装这个包的时候，其他依赖也会被安装，可以看到ros2提供的控制器都被放到这个包的依赖中了。就有之前用到过的ros-humble-diff-drive-controller（两轮差速控制器）、ros-humble-joint-state-broadcaster（关节状态发布控制器）、ros-humble-imu-sensor-broadcaster（imu数据发布控制器）、 ros-humble-position-controllers（关节位置控制器）

二、gazebo接入ros2_control

1、介绍：

Gazebo接入ros2_control，其实就是让Gazebo按照ros2_control指定的接口提供数据。在ROS2中利用相应的Gazebo插件，可以方便的实现Gazebo和ros2_control的对接。

2：安装Gazebo ROS 2 control 插件：

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

这个插件就叫Gazebo ROS 2 control，他的功能就是说调用Gazebo的接口获取硬件传感器的信息和执行器的信息，然后导出为状态接口和命令接口，被接口管理器所管理。

3、实现：

书210页代码，代码上有对应标签解释。

fishbot_ros2_control.yaml 配置文件

4、查看：

使用 ros2 launch bot_description gazebo_sim.lanuch.py ——启动

如果启动不了，请查看配置文件是否有问题。

使用 ros2_control 启动成功的话，是可以看到 gazebo_ros2_control 相关信息的。

ros2_control它一系列的命令行接口都是通过服务来实现的（可以在终端使用命令 ros2 control 来查 ros2_control 的帮助 ）

list_controller_types：控制器类型；

list_hardware_interfaces：硬件接口；

ros2 service list | grep controller ——查看 ros2_control 相关服务

查询所有控制器controller的类型 ros2 control list_controller_types

查看接口管理器：

command interfaces：命令接口，有四个，我们代码里自定义的；

unclaimed：表示这个接口未被占用，因为同一个控制器的接口，同一时间是能给一个控制器使用（如果两个控制器使用同一个接口，一个让轮子正转，一个让轮子反转，轮子就乱套了。）。

查看硬件组件： ros2 control list_hardware_components

硬件组件：我们设置的 FishBotGazeboSystem 就是一个硬件组件（系统的硬件组件）

三、使用关节状态发布控制器

1、介绍：

使用关节状态发布控制器，听名字就知道是用于发布关节的状态的，也就是发布joint_states 这个话题。

我们使用 rviz2 可以看到具体问题。

我们可以看到两个轮子的位置是错误的，提示没有位置变换，为什么没有使用 ros2_control 的时候能找到TF呢，因为之前使用了<xacro:gazebo_control_plugin/>，而现在没有TF，我们要使用 ros2_control 的插件，这个插件叫做“关节状态发布器”这样一个插件，在ros2_control里我们可以使用插件来占用接口，上面我们的接口里有left_wheel_joint/position（左轮位置接口），right_wheel_joint/position（右轮位置接口），这个位置，其实就是轮子的状态，直到位置了，就可以发布joint_state的话题，joint_state 话题会被 robot_state 话题订阅。

joint_state_broadcaster/JointStateBroadcaster 这个控制器就可以发布joint_state 话题，它的原理是使用了 left_wheel_joint/position 和 right_wheel_joint/position 这两个状态接口，去使用这两个接口，然后把接口数据变成 joint_state 话题然后发布出去。

2、配置控制器：

控制器添加后，是需要加载和激活的。

修改配置文件 fishbot_ros2_control.yaml

3、加载、激活：

ros2 control load_controller fishbot_joint_state_broadcaster --set-state active

--set-state active：设置状态 为 “激活”；

load_controller fishbot_joint_state_broadcaster：加载控制器，后面名字要和你配置文件中的名字一致。

加载成功后，控制台也可以看到提示。

在截图中可以看到：joints' or 'interfaces' parameter is empty. All available state interfaces will be published 因为我们没有设置joints 和 interfaces 的参数，所有有效的状态接口将被发布出去。

可以看到轮子关系就出来了。

4、优化启动：

启动gazebo后再使用命令行加载、激活控制器比较麻烦，我们要让他自动加载、激活控制器。

修改launch文件：这里有两种，由于我的显卡问题，gazebo的渲染比较慢，所以我的加载都是延迟了的，图片里有介绍。

action_load_joint_state_controller 这个控制器要等gazebo启动之后再执行，不然你gazebo（control_manager）没有启动，你去加载，是不会成功的。

5、查询状态：

ros2 control list_controllers（输入命令到终端，ros2前面不要有空格）

6、改变状态：

ros2 control set_controller_state fishbot_joint_state_broadcaster inactive

将状态改为未激活状态。

7、卸载控制器：

ros2 contorl unload controller fishbot_joint_state_broadcaster

四、使用力控制器控制轮子

1、介绍：

力控：在机器人中要试下更加柔顺地控制，力控是比较常见的一种策略，ros2_control就提供了这一控制器，类型为effort_controllers/JointGroupEffortController

2、配置控制器：

joints：左右关节；

command_interfaces：命令接口（控制接口）；

state_interfaces：状态接口，不同于控制接口，状态接口是可以被多次使用的，因为大家都是读同一个数据而已，并没有什么影响。

3、查看

ros2 topic list -t | grep effort

4、发布：

ros2 topic pub /fishbot_effort_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.0001,0.001]}"

ros2 control list_hardware_interfaces ：我们可以看到现在接口内容有点不一样了。

left_wheel_joint/effort [available] [claimed]：可以看到对应左右轮的力控接口被占用了。

五、两轮差速控制器控制机器人

1、介绍：

状态发布控制器和力控制器只是对状态接口的简单调用，本节我们来学习如何使用两轮差速控制器，这个控制器相对而言要复杂很多，该控制器不仅仅是单纯的数据转发，还涉及到了运动学计算，从而得到里程计信息和两个轮子的目标速度。

2、添加配置：

修改：fishbot_ros2_control.yaml

ros__parameters：给ros空间的配置；

wheel_separation：轮距；

wheel_separation_multiplier：轮距系数

轮距=wheel_separation*wheel_separation_multiplier

wheel_separation_multiplier、left_wheel_radius_multiplier、right_wheel_radius_multiplier在实际的使用过程中去矫正里程计的时候要使用的三个参数，1.0相当于不进行矫正；

publish_rate：发布速率；

odom_frame_id：里程计的 frame_id；

pose_covariance_diagonal：位姿协方差矩阵对角；

twist_covariance_diagonal：速度协方差矩阵对角；

open_loop：是否开环控制，本来是false的，只要里程计准，我们使用true；

enable_odom_tf：是否启用里程计的坐标转换（TF）；

cmd_vel_timeout：接收cmd_vel命令的超时时间（S），超过0.5秒没有收到，机器人就停下来了；

use_stamped_vel：是否使用Stamped Twist消息来代表速度命令；

3、添加、激活control

在添加DiffDriveController之前，我们要删除原来的力控制器JointGroupEffortController因为力控制器也是可以控制轮子的，两轮差速控制器也是可以控制轮子的，最好不要同时提供多个接口来机器人的风险，到最后使用的时候就会乱了，到时就搞不清楚了，比较麻烦。

修改：gazebo_sim.lanuch.py ，注意我这里有两种方法，我的显卡问题，所以只能延迟加载控制器，书本上的方法也写了，但实际使用过程中只能选择一种。

4、查看：

新终端输入：ros2 topic list

两轮差速器提供如下话题（topic）：

/fishbot_diff_drive_controller/cmd_vel_unstamped：cmd_vel话题；
/fishbot_diff_drive_controller/odom：里程计话题；

因为控制器产生的话题，都是以控制器的名字为前缀的，所以为了我们方便键盘控制和后续使用，我们可以将两轮差速器的话题名称进行重映射。

5、控制器名称重映射：

修改：fishbot.ros2_control.xacro