移動ロボットのGazebo対応

移動ロボットのモデルを動力学シミュレータGazeboに対応するために設定を行っていきます．

オドメトリの設定

マップが記載されている座標系があったときに，その座標系からみた移動ロボットの相対位置を表す座標系をodomフレームとしてTFで設定します．

マップ座標系は，複数マップも設定することができますが，複数マップを作ったときは，基準となるマップからのオドメトリのフレームodomを設定する必要があります．複数マップの上位にはearth（地球）の座標フレームが有り，GPSの計算でも用いられるECEF (earth-centered earth-fixed)で定められるＸＹＺの直交座標系があります．詳しくは，リンク先のREP 105 を確認してください．

TFのつながりは下記の通り．

earth -> map -> odom -> base_link

オドメトリの定義はnav_msgs/Odometryで定義されており，下記の項目となります．

# Includes the frame id of the pose parent.

std_msgs/Header header

# Frame id the pose is pointing at. 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

ロボットの速度情報がtwist型で管理されているので，車輪の回転速度から並進速度と回転速度を計算します．

並進速度をlinear，回転速度をangular，左右の車輪の回転により発生している推定速度をright_wheel_est_vel，left_wheel_ext_vel，左右の車輪間の距離をwheel_separationとすると下記のように計算される．

linear = (right_wheel_est_vel + left_wheel_est_vel) / 2

angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation

ROS 2では，ros2_controlのdiff_drive_controllerで差動移動ロボットの管理が行えるようになっているので上記計算を手動で計算する必要はありません．詳しくは公式のros2_control_documentationを見てください．

Gazebo Classicとの接続

未だにClassicの接続のほうが簡単なので，まずこちらの接続から確認．

gazebo_ros_pkgをインストールしておく．

sudo apt install ros-humble-gazebo-ros-pkgs

URDFにGazeboプラグインを追加

まず，差動駆動のプラグインとIMUのプラグインをURDFに追加してみる．

差動駆動のプラグインは，UDFDファイルに下記を追記．

追記場所はURDFファイルの</robot>の直前．

<gazebo>

  <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>

    <ros>

      <namespace>/demo</namespace>

    </ros>

    <!-- wheels -->

    <left_joint>drivewhl_l_joint</left_joint>

    <right_joint>drivewhl_r_joint</right_joint>

    <!-- kinematics -->

    <wheel_separation>0.4</wheel_separation>

    <wheel_diameter>0.2</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>false</publish_odom_tf>

    <publish_wheel_tf>true</publish_wheel_tf>

    <odometry_frame>odom</odometry_frame>

    <robot_base_frame>base_link</robot_base_frame>

  </plugin>

</gazebo>

太字にしているところを確認するとわかるが，<odometory_frame>のタグでodomという名前を指定している．

<robot_base_frame>のタグにはbase_linkの名前がしている．

これらの名前がURDFで設定しているフレームの名前とずれていると動かない．

IMUのプラグインは，シミュレーションで２次元のマップを作るときは使うことはないのですが，３次元マップや，２Dでもジャイロオドメトリを試したいときには取り付けた方が良い．

先ほど追加した，差動駆動のプラグインの直後に下記の記載をURDFに追加．

記載が長いので折りたたんであります．

下のラインをクリックするとIMUの追記内容が表示されます．

追記例では，Gazeboのセンサーノイズモデルでノイズも載せてあります．

Gazeboの起動のためのlaunchファイルの修正

前回（移動ロボットのモデル）作ったdisplay.launch.pyファイルを編集する．

display.launch.py内のgenerate_launch_description()内にある動作確認用に記載していたGUIである下記joint state publisher_gui_nodeを削除する．

    joint_state_publisher_gui_node = launch_ros.actions.Node(

        package='joint_state_publisher_gui',

        executable='joint_state_publisher_gui',

        name='joint_state_publisher_gui',

        condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))

    )

また，return launch.LaunchDescription()内の最初に記載していた下記パラメータ設定の部分も削除．

        launch.actions.DeclareLaunchArgument(name='gui', default_value='True',

                                            description='Flag to enable joint_state_publisher_gui'),

と下記の行も削除

        joint_state_publisher_gui_node,

下記の行を「joint_state_publisher_node,」の前の行に追加．

launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen'),

（ここまでreturn launch.LaunchDescription()内の編集）

return launch.LaunchDescription()の直前に下記をペースト

spawn_entity = launch_ros.actions.Node(

  package='gazebo_ros',

  executable='spawn_entity.py',

  arguments=['-entity', 'sam_bot', '-topic', 'robot_description'],

  output='screen'

)

joint_state_publisher_nodeには，下記のように引数を２行削除して，１行追加．

joint_state_publisher_node = launch_ros.actions.Node(

  package='joint_state_publisher',

  executable='joint_state_publisher',

  name='joint_state_publisher',

  parameters=[{'robot_description': Command(['xacro ', default_model_path])}], # この行削除

  condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')) # この行削除

  arguments=[default_model_path] #この行を追加

)

また，「spawn_entity, 」の記載を「return launch.LaunchDescription()」の中の最後の方にある「rviz」の直前に追加．

下記は修正後のreturn launch.LaunchDescription()

    return launch.LaunchDescription([

        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,

                                            description='Absolute path to robot urdf file'),

        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,

                                            description='Absolute path to rviz config file'),

        launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen'),

        joint_state_publisher_node,

        robot_state_publisher_node,

        spawn_entity,

        rviz_node

    ])

修正後のdisplay.launch.pyは下記の通り．

import launch

from launch.substitutions import Command, LaunchConfiguration

import launch_ros

import os

def generate_launch_description():

    pkg_share = launch_ros.substitutions.FindPackageShare(package='sam_bot_description').find('sam_bot_description')

    default_model_path = os.path.join(pkg_share, 'src/description/sam_bot_description.urdf')

    default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')

    robot_state_publisher_node = launch_ros.actions.Node(

        package='robot_state_publisher',

        executable='robot_state_publisher',

        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]

    )

    joint_state_publisher_node = launch_ros.actions.Node(

        package='joint_state_publisher',

        executable='joint_state_publisher',

        name='joint_state_publisher',

        arguments=[default_model_path]

    )

    rviz_node = launch_ros.actions.Node(

        package='rviz2',

        executable='rviz2',

        name='rviz2',

        output='screen',

        arguments=['-d', LaunchConfiguration('rvizconfig')],

    )

    spawn_entity = launch_ros.actions.Node(

        package='gazebo_ros',

        executable='spawn_entity.py',

        arguments=['-entity', 'sam_bot', '-topic', 'robot_description'],

        output='screen'

    )

    return launch.LaunchDescription([

        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,

                                            description='Absolute path to robot urdf file'),

        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,

                                            description='Absolute path to rviz config file'),

        launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen'),

        joint_state_publisher_node,

        robot_state_publisher_node,

        spawn_entity,

        rviz_node

    ])

次に，package.xmlファイルを開いて下記の行を削除．

<exec_depend>joint_state_publisher_gui</exec_depend>

以上で修正終了なので，gazeboで表示できるかを確認．

cd ~/ros2_ws

colcon build 

. install/setup.bash

ros2 launch sam_bot_description deiplay.launch.py

下記画面のようにgazeboが立ち上がって移動ロボットが表示される．

内界センサを使った自己位置推定

robot_localizationパッケージを使ってタイヤの回転角やIMUなどの内界センサを使った位置推定を行う．

まずは下記で，robot_localizationをインストール．

sudo apt install ros-humble-robot-localization

robot_localizationの拡張カルマンフィルタを使うためにconfigディレクトリを作成してその中にekf.yamlを作成する．

cd ~/ros2_ws/src/sam_bot_description

mkdir config

cd config

gedit ekf.yaml

ekf.yamlの中身は下記の内容を記載．

### ekf config file ###

ekf_filter_node:

    ros__parameters:

# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin

# computation until it receives at least one message from one of theinputs. It will then run continuously at the

# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.

        frequency: 30.0

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is

# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar

# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected

# by, for example, an IMU. Defaults to false if unspecified.

        two_d_mode: false

# Whether to publish the acceleration state. Defaults to false if unspecified.

        publish_acceleration: true

# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.

        publish_tf: true

# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.

#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.

# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"

#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.

# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark

#    observations) then:

#     3a. Set your "world_frame" to your map_frame value

#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node

#         from robot_localization! However, that instance should *not* fuse the global data.

        map_frame: map              # Defaults to "map" if unspecified

        odom_frame: odom            # Defaults to "odom" if unspecified

        base_link_frame: base_link  # Defaults to "base_link" ifunspecified

        world_frame: odom           # Defaults to the value ofodom_frame if unspecified

        odom0: demo/odom

        odom0_config: [true,  true,  true,

                       false, false, false,

                       false, false, false,

                       false, false, true,

                       false, false, false]

        imu0: demo/imu

        imu0_config: [false, false, false,

                      true,  true,  true,

                      false, false, false,

                      false, false, false,

                      false, false, false]

設定ファイルは，frequency, two_d_mode, publish_acceleration, publish_tf, map_frame, odom_frame, base_link_frame, world_fram に対して設定をしている．

詳細な設定は， Parameters of state estimation nodes を参考にしてください．

ekf.yamlのサンプルは，ここ．

ちなみに，odom0_configとimu0_configは，x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az の15個のフィルター後に出力される値の有効・無効を表している．

odom0の入力にdemo/odomを設定，imu0にはdemo/imuを設定している．

odom0は，trueの場所を見るとx, y, z,  vyaw が有効ということになる．

imu0は，trueの場所を見るとroll, pitch, yawとなっている．

launchファイルの作成

display.launch.pyをまた修正する．

launchファイルの最後の方にあるreturn launch.launchDescription()の直前に下記のrobot_localization_nodeを追加する．

robot_localization_node = launch_ros.actions.Node(

       package='robot_localization',

       executable='ekf_node',

       name='ekf_filter_node',

       output='screen',

       parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}]

)

return launch.launchDescription()の中にuse_sim_timの下記設定を加える．

launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',

                                            description='Flag to enable use_sim_time'),

また，同じくreturn launch.launchDescription()の中の最後の方に，robot_localization_nodeを追加する．具体的な場所は下記．

      robot_state_publisher_node,

      spawn_entity,

      robot_localization_node,

      rviz_node

])

CMakeLists.txtにconfigファイルがインストールされるように，install()の部分を修正．

install(

  DIRECTORY src launch rviz config

  DESTINATION share/${PROJECT_NAME}

)

以上で終了なので，ビルドと実行をしてみる．

cd ~/ros2_ws/

colcon build

. install/setup.bash

ros2 launch sam_bot_description display.launch.py

rviz内の左のメニューのFramesを展開してTFが下の図の通り登録されていることを確認する．