《動手學ROS2》9.3為FishBot配置兩輪差速控制插件

《動手學ROS2》9.3為FishBot配置兩輪差速控制插件

本系列教程作者：小魚

公衆号：魚香ROS

QQ交流群：139707339

教學視訊位址：小魚的B站

完整文檔位址：魚香ROS官網

版權聲明：如非允許禁止轉載與商業用途。

9.3為FishBot配置兩輪差速控制插件

小魚又來了，完成了上節課的Gazebo加載FishBot，但是機器人還是不會動，你一定很不開心吧，本節課小魚就帶你一起通過配置兩輪差速控制插件，讓我們的機器人動起來~

最終效果：

1. Gazebo插件介紹

之前小魚說過Gazebo是一個獨立于ROS的軟體，對外提供了豐富的API可以使用，gazebo的插件按照用途大緻可以分為兩種：

1. 用于控制的插件，通過插件可以控制機器人關節運動，可以進行位置、速度、力的控制，比如我們這節課的兩輪差速控制器。
2. 用于資料采集的插件，比如IMU傳感器用于采集機器人的慣性，雷射雷達用于采集機器人周圍的點雲資訊。

當然上面兩類插件功能也可以寫到一個插件裡，兩輪差速插件就是一個二合一加強版。

2.兩輪差速插件介紹

兩輪差速插件用于控制機器人輪子關節的位置變化，同時該插件還會擷取輪子的位置以及速度的資訊的回報，根據回報的位置資訊結合運動學模型即可計算出目前機器人的位姿（裡程計）。

該插件的名稱為：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的輸入和輸出資訊的總結，可以很直覺的看到該插件主要輸入控制指令，主要輸出裡程計資訊。接着小魚帶你分别認識一下輸入和輸出兩個部分。

2.2 輸入參數

2.2.1 配置參數

不知道你是否還記得在第七章中，小魚對兩輪差速底盤的運動學正的介紹。如果要完成底盤的正逆解和裡程計的推算就必須要知道輪子的直徑和間距。

同時該插件還提供了一些可以控制輸出的選項，因為是仿真，是以還要告訴插件輪子對應的joint名稱等資訊，這樣就有了下面這個參數表格：

配置項	含義
ros	ros相關配置，包含命名空間和話題重映射等
update_rate	資料更新速率
left_joint	左輪關節名稱
right_joint	右輪關節名稱
wheel_separation	左右輪子的間距
wheel_diameter	輪子的直徑
max_wheel_torque	輪子最大的力矩
max_wheel_acceleration	輪子最大的加速度
publish_odom	是否釋出裡程計
publish_odom_tf	是否釋出裡程計的tf開關
publish_wheel_tf	是否釋出輪子的tf資料開關
odometry_frame	裡程計的framed ID，最終展現在話題和TF上
robot_base_frame	機器人的基礎frame的ID

2.2.2 控制指令

兩輪差速控制器預設通過訂閱話題

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。

2.3 輸出參數

2.3.1 裡程計

裡程計資訊預設的輸出話題為

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，其代表協方差矩陣，後面小魚寫篇文章來介紹下，這裡隻需了解其含義即可。

2.3.2 裡程計TF資訊

設為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
           

2.3.3 左右輪子TF資訊

設為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
           

3.在URDF中配置兩輪差速模型

上面該介紹的我們都給介紹了，接着我們直接來配置。

因為是給Gazebo的插件，是以在

URDF

中，我們需要使用

<gazebo>

進行配置，因為是要給

gazebo

配置插件，是以要在

gazebo

标簽下添加

plugin

子插件。

話不多說，上代碼

<gazebo>
    <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
          <ros>
            <namespace>/</namespace>
            <remapping>cmd_vel:=cmd_vel</remapping>
            <remapping>odom:=odom</remapping>
          </ros>
          <update_rate>30</update_rate>
          <!-- wheels -->
          <left_joint>left_wheel_joint</left_joint>
          <right_joint>right_wheel_joint</right_joint>
          <!-- kinematics -->
          <wheel_separation>0.2</wheel_separation>
          <wheel_diameter>0.065</wheel_diameter>
          <!-- limits -->
          <max_wheel_torque>20</max_wheel_torque>
          <max_wheel_acceleration>1.0</max_wheel_acceleration>
          <!-- output -->
          <publish_odom>true</publish_odom>
          <publish_odom_tf>true</publish_odom_tf>
          <publish_wheel_tf>true</publish_wheel_tf>
          <odometry_frame>odom</odometry_frame>
          <robot_base_frame>base_footprint</robot_base_frame>
      </plugin>Copy to clipboardErrorCopied
           

将這段代碼加到我們的URDF中，然後對着上面小魚介紹的配置項，一一看下，接着我們就可以來測試運作了。

完整的URDF可以參考：

4.兩輪差速插件測試

修改完了URDF模型我們将代碼編譯一下，讓更新後的URDF檔案安裝到install目錄，接着就可以運作9.2中的launch檔案，将模型加載到Gazebo中。

4.1 編譯-啟動

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了。

4.3 使用鍵盤控制fishbot

你還記得第二章中小魚帶你玩的小烏龜嗎？當時我們用鍵盤來控制小烏龜運動，現在我們可以用鍵盤來控制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話題，移動機器人觀察資料變化。

此時你應該玩一會，體驗一下各種花式走法。

4.4 使用rqt顯示速度資料

接着我們嘗試使用rqt将資料在rqt中可視化出來，打開終端輸入rqt。

rqtCopy to clipboardErrorCopied
           

選擇Plugin->Visualization->Plot

在上方Topic輸入

/cmd_vel/linear/x

，再輸入

/cmd_vel/angular/z

，然後用鍵盤控制機器人移動。

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

4.5 在RVIZ2中顯示Fishbot及其軌迹

打開rviz2

rviz2Copy to clipboardErrorCopied
           

1. 修改FixedFrame為odom
2. 添加插件，Add->Odometry->OK
3. 選擇話題，Odometry->Topic->選/odom
4. 去除協方差顯示，Odometry->Covariance>取消勾選
5. 鍵盤控制節點，點個U，原地轉圈圈

最終結果：

4.6 在RVIZ2中顯示機器人模型

雖然機器人的軌迹已經在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）的版權保護計劃