小鱼又来了，完成了上节课Gazebo加载FishBot，但是机器人还是不会动，你一定很不高兴，这节课小鱼会带你配置两轮差速控制插件，让我们的机器人移动~

最终效果：

小鱼之前说过Gazebo是独立的ROS该软件提供了丰富的外部软件API可以使用，gazebo根据用途，插件大致可分为两种：

两轮差速插件用于控制机器人轮轮关节的位置变化。同时，插件还将获得车轮位置和速度信息的反馈。当前机器人的位置（里程计）可根据反馈位置信息和运动模型计算。

插件名称为：gazebo_ros_diff_drive

源码地址：https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_diff_drive.cpp

两轮差速控制器和Gazebo的关系

两轮差速控制器可以发送轮子的目标转速Gazebo，并从Gazebo获取实际速度和位置。

注：发送给Gazebo是目标速度，反馈是实际速度。=实际上，比如车轮卡住了，什么目标速度，实际速度都是0。

要快速了解系统的功能，最直接的就是看系统的外部输入和输出是什么？什么都不说，看下图:

上图就是对gazebo_ros_diff_drive总结输入输出信息，可以直观地看到插件的主要内容输入控制指令，主要输出里程计信息。然后小鱼带你去了解输入和输出。

我不知道你是否还记得小鱼在第七章介绍了两轮差速底盘的运动学。如果要完成底盘的正反解决方案和里程计计算，您必须知道车轮的直径和间距。

与此同时，该插件还提供了一些可以控制输出的选项，因为它是模拟的，所以告诉插件轮对应的joint名称等信息，所以有以下参数表：

默认情况下，两轮差速控制器订阅主题cmd_vel获取目标线速度和角速度。主题类型如下：geometry_msgs/msg/Twist

我们通过ros2的CLI来看一下这个消息包含的内容有哪些？

ros2 interface show geometry_msgs/msg/TwistCopy to clipboardErrorCopied 

# This expresses velocity in free space broken into its linear and angular parts.  Vector3  linear Vector3  angularCopy to clipboardErrorCopied 

我们可以看到线速和角速，我们使用它proto看看包含哪些基本数据类型？

ros2 interface proto geometry_msgs/msg/TwistCopy to clipboardErrorCopied 

"linear:   x: 0.0   y: 0.0   z: 0.0 angular:   x: 0.0   y: 0.0   z: 0.0 "Copy to clipboardErrorCopied 

线速和角速都包括在内x、y、z，代表坐标系三个方向的对应速度。

收到主题数据后，两轮差速控制器将角速和线速转换为两轮的转速Gazebo。

里程计信息默认的输出话题为odom，其消息类型为：nav_msgs/msg/Odometry

同样的使用CLI看一下其消息的组成结构：

ros2 interface show nav_msgs/msg/OdometryCopy to clipboardErrorCopied

# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id

# Includes the frame id of the pose parent.
std_msgs/Header header

# Frame id the pose points to. The twist is in this coordinate frame.
string child_frame_id

# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose

# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist
Copy to clipboardErrorCopied

ros2 interface proto nav_msgs/msg/OdometryCopy to clipboardErrorCopied

"header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
child_frame_id: ''
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance:
  - 0.0
  - 0.0
    ...
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance:
  - 0.0
  - 0.0
    ...
"Copy to clipboardErrorCopied

可以看到其数据主要包含三个部分：

• header，表示该消息发布的时间
• pose，表示当前机器人位置和朝向
• twist，表示当前机器人的线速度和角速度

数据中还包含一个covariance，其代表协方差矩阵，后面小鱼写篇文章来介绍下，这里只需了解其含义即可。

设为true，订阅tf话题里你就可以看到像下面的msg，建议后面配置好后，手动修改下，对比区别

- header:
    stamp:
      sec: 6157
      nanosec: 907000000
    frame_id: odom
  child_frame_id: base_footprint
  transform:
    translation:
      x: 0.0005557960241049835
      y: -0.0007350446303238693
      z: 0.01599968753145574
    rotation:
      x: 4.691143395208505e-07
      y: 7.115496626557812e-06
      z: -0.018531475772549166
      w: 0.9998282774331005Copy to clipboardErrorCopied

设为true，订阅tf话题里你就可以看到像下面的msg，建议后面配置好后，手动修改下，对比区别

- header:
    stamp:
      sec: 6157
      nanosec: 941000000
    frame_id: base_link
  child_frame_id: left_wheel_link
  transform:
    translation:
      x: -0.02
      y: 0.1
      z: -0.06
    rotation:
      x: 0.0
      y: 0.049519025127821005
      z: 0.0
      w: 0.9987731805321918
- header:
    stamp:
      sec: 6157
      nanosec: 941000000
    frame_id: base_link
  child_frame_id: right_wheel_link
  transform:
    translation:
      x: -0.02
      y: -0.1
      z: -0.06
    rotation:
      x: 0.0
      y: -0.0663387077034509
      z: 0.0
      w: 0.9977971616817898Copy to clipboardErrorCopied

上面该介绍的我们都给介绍了，接着我们直接来配置。

修改完了URDF模型我们将代码编译一下，让更新后的URDF文件安装到install目录，接着就可以运行9.2中的launch文件，将模型加载到Gazebo中。

colcon buildCopy to clipboardErrorCopied

ros2 launch fishbot_description gazebo.launch.pyCopy to clipboardErrorCopied

接着你可以使用CLI工具看一下系统有哪些节点在运行

ros2 node list
ros2 topic listCopy to clipboardErrorCopied

ros2 node list
---------------
/diff_drive
/gazebo

ros2 topic list
---------------
/clock
/cmd_vel
/odom
/parameter_events
/performance_metrics
/rosout
/tfCopy to clipboardErrorCopied

相信此时你已经看到了我们插件订阅的的/cmd_vel和发布的/odom了。

你还记得第二章中小鱼带你玩的小乌龟吗？当时我们用键盘来控制小乌龟运动，现在我们可以用键盘来控制fishbot动起来了。

你需要一个键盘控制工具，可以用下面的指令安装

sudo apt install ros-foxy-teleop-twist-keyboardCopy to clipboardErrorCopied

这个功能包下有一个节点，这个节点会监听键盘的按键事件，然后发布cmd_vel话题，该话题被gazebo的两轮差速插件所订阅。所以我们就可以通过这个节点来控制fishbot。

ros2 run teleop_twist_keyboard teleop_twist_keyboardCopy to clipboardErrorCopied

如果你想让这个节点不是发布cmd_vel话题，而是别的，可以采用ROS2的话题重映射功能。 eg: ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=cmd_vel1

接着尝试使用来控制机器人运动

U    I    O
   J    K    L
   M    <    >Copy to clipboardErrorCopied

点一下I，你就能看到fishbot在Gazebo中飞速的移动。接着打开终端，打印一下odom话题和tf话题，移动机器人观察数据变化。

此时你应该玩一会，体验一下各种花式走法。

接着我们尝试使用rqt将数据在rqt中可视化出来，打开终端输入rqt。

rqtCopy to clipboardErrorCopied

选择Plugin->Visualization->Plot

在上方Topic输入/cmd_vel/linear/x，再输入/cmd_vel/angular/z，然后用键盘控制机器人移动。

cmd_vel中的速度代表目标速度，接着我们显示一下当前速度（在odom.twist中）

打开rviz2

rviz2Copy to clipboardErrorCopied

1. 修改FixedFrame为odom
2. 添加插件，Add->Odometry->OK
3. 选择话题，Odometry->Topic->选/odom
4. 去除协方差显示，Odometry->Covariance>取消勾选
5. 键盘控制节点，点个U，原地转圈圈

最终结果：

虽然机器人的轨迹已经在RVIZ中显示出来了，但是并没有机器人的模型，也看不到轮子的转动，小鱼来带你一起解决这个问题。

前面小鱼介绍过，要发布机器人模型我们所使用的节点是robot_state_publisher,所以我们在gazebo.launch.py中加入这个节点，同时再加上rviz2的启动节点，最终的gazebo.launch.py内容如下：

import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    robot_name_in_model = 'fishbot'
    package_name = 'fishbot_description'
    urdf_name = "fishbot_gazebo.urdf"

    ld = LaunchDescription()
    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    # Start Gazebo server
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'],
        output='screen')

    # Launch the robot
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ], output='screen')
    
    # Start Robot State publisher
    start_robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )

    # Launch RViz
    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        # arguments=['-d', default_rviz_config_path]
        )

    ld.add_action(start_gazebo_cmd)
    ld.add_action(spawn_entity_cmd)
    ld.add_action(start_robot_state_publisher_cmd)
    ld.add_action(start_rviz_cmd)


    return ldCopy to clipboardErrorCopied

保存编译启动

colcon buildCopy to clipboardErrorCopied

ros2 launch fishbot_description gazebo.launch.pyCopy to clipboardErrorCopied

然后继续启动键盘控制，Enjoy It!

技术交流&&问题求助：

• 微信公众号及交流群：鱼香ROS
• 小鱼微信：AiIotRobot
• QQ交流群：139707339
• 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划