Choreonoid tips ⑦【gmappingを使ったSLAM】

本記事では、ChoreonoidとROSを使ってSLAM（Simultaneous Localization and Mapping）を行う手順を紹介します。
今回は地図生成にgmappingパッケージを使用しました。gmappingは占有格子地図をベースとしたFastSLAMのパッケージで、LRFのデータと車輪の回転数から移動量を計測するオドメトリの情報を用いてSLAMを行います。
本記事は、ChoreonoidとROSディストリビューションのインストールを行っていることを前提としています。インストールを行っていない方は、Choreonoid公式HPを参考に「ROSのインストール」と「Choreonoid関連パッケージのビルド」まで行ってください。
https://choreonoid.org/ja/documents/latest/ros/index.html#

1. 使用した環境

・Ubuntu 20.04 LTS
・Choreonoid 1.8
・ROS Noetic Ninjemys
・gmapping 1.4.2
・map_server 1.17.2

2. gmappingとmap_serverパッケージのインストール

以下のコマンドを実行し、gmappingとmap_serverパッケージをインストールします。

sudo apt install -y ros-noetic-gmapping ros-noetic-map-server

上記パッケージをインストールしていないと、launchファイル実行時に以下のエラーが発生します。

roslaunch cnoid_turtlebot3_slam cnoid_turtlebot3_world_slam_2d.launch
ERROR: cannot launch node of type [gmapping/slam_gmapping]: gmapping

3. TurtleBot3関連パッケージのダウンロード・インストール

TurtleBot3関連パッケージでturtlebot3_msgsパッケージを使用しているので、以下のコマンドを実行しturtlebot3_msgsパッケージをインストールします。

sudo apt install -y ros-noetic-turtlebot3-msgs

上記パッケージをインストールしていないと、ビルド時に以下のエラーが発生します。

catkin build
Errors     << turtlebot3_fake:check /home/usr/catkin_ws/logs/turtlebot3_fake/build.check.012.log
CMake Error at /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "turtlebot3_msgs"
  with any of the following names:

    turtlebot3_msgsConfig.cmake
    turtlebot3_msgs-config.cmake

  Add the installation prefix of "turtlebot3_msgs" to CMAKE_PREFIX_PATH or
  set "turtlebot3_msgs_DIR" to a directory containing one of the above files.
  If "turtlebot3_msgs" provides a separate development package or SDK, be
  sure it has been installed.

次に、以下のコマンドを実行しTurtleBot3関連パッケージをダウンロードします。Ubuntu 20.04を使用しているためgit cloneを行う際に、-bオプションを付けてブランチを指定しています。

cd ~/catkin_ws/src/
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git

4. .bashrcファイルの編集

以下のコマンドを実行し、ワークスペースセットアップスクリプトの読み込みと環境変数の設定を追加します。今回はTURTLEBOT3_MODELにwaffle_piを指定しています。

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
echo "export TURTLEBOT3_MODEL=waffle_pi" >> ~/.bashrc

5. Choreonoidのmaterials.yamlファイルの編集

使用するTurtleBot3モデルのマテリアル設定を追加します。以下のコマンドを実行しmaterials.yamlファイルを開きます。

cd ~/catkin_ws/src/choreonoid/share/default
cp -fp materials.yaml materials_bkyyyyMMdd.yaml

※バックアップファイル作成時のyyyyMMddは、編集年月日を指定します。

gedit materials.yaml

「materials」キー(74行目)と「contact_materials」キー(最終行)に以下を追加します。
・materials

  -
    name: Tb3_tire
    youngs_modulus: 1e10
    roughness: 1.0
    viscosity: 1.0
  -
    name: Tb3_caster
    youngs_modulus: 1e10
    roughness: 1.0
    viscocity: 1.0

・contact_materials

  -
    materials: [ Ground, Tb3_tire ]
    youngs_modulus: 1e10
    friction: 1.0
    restitution: 0.0
    friction_model: [ cone, direct ]
  -
    materials: [ Ground, Tb3_caster ]
    youngs_modulus: 1e10
    friction: 0.1
    restitution: 0.0
    friction_model: [ cone, direct ]

6. cnoid_turtlebot3_pkgsパッケージのダウンロード

cnoid_turtlebot3_pkgs.zipをダウンロードし、catkin_ws/src以下に移動したらZIPファイルを解凍します。
https://rtc-fukushima.jp/wp/wp-content/uploads/2022/08/cnoid_turtlebot3_pkgs.zip
ここでは、ダウンロードディレクトリにcnoid_turtlebot3_pkgs.zipファイルが保存されている場合の手順となります。別のディレクトリに格納している場合は適宜ファイルパスを変更してください。

mv ~/ダウンロード/cnoid_turtlebot3_pkgs.zip ~/catkin_ws/src
cd ~/catkin_ws/src
unzip cnoid_turtlebot3_pkgs.zip

7. シンボリック・リンクの追加

cnoid_turtlebot3_pkgsパッケージのモデルファイルでturtlebot3パッケージとturtlebot3_simulationsパッケージのメッシュファイルを参照しているので、以下のコマンドを実行しシンボリック・リンクを追加します。

cd ~/catkin_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/model/resource/
ln -s ~/catkin_ws/src/turtlebot3/turtlebot3_description/ turtlebot3_description
ln -s ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_world/meshes/ turtlebot3_world

8. ChoreonoidとTurtleBot3関連パッケージのビルド

以下のコマンドを実行し、ChoreonoidとTurtleBot3関連パッケージのビルドを行います。

cd ~/catkin_ws
catkin build
source devel/setup.bash

9. TurtleBot3による2D地図生成

以下のコマンドを実行し、TurtleBot3のSLAM用launchファイルを起動します。

roslaunch cnoid_turtlebot3_slam cnoid_turtlebot3_world_slam_2d.launch

次に別ターミナルを起動し、TurtleBot3の遠隔操作launchファイルを起動します。

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

TurtleBot3の遠隔操作launchを起動したターミナル上で、キーボードからw(直進), x(後退), d(右旋回), a(左旋回), s(停止)を入力することでTurtleBot3を動かし、2D地図を生成することができます。

図 1. Choreonoid画面

Choreonoidを起動後にメッセージ欄に[did not find expect key.]と表示され、強制終了する場合があります。materials.yamlへの追記部分を間違えて追記している可能性がありますので確認してください。
また、Choreonoidがプロジェクトファイルの読み込みを完了しても、シーン上にモデルが表示されない場合があります。その場合、設定ダイアログからライティングモードを標準から最小限に変更してみてください。シーン上にモデルが表示されます。

図 2. RViz画面

10. 作成した地図の保存

別のターミナルを起動し、以下のコマンドで作成した地図を保存します。-fオプションのあとは出力ファイル名を指定してください。

mkdir ~/catkin_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_slam/map
rosrun map_server map_saver -f ~/catkin_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_slam/map/map

図 3. SLAMで作成したマップ

11. モデルファイルの解説

使用しているTurtleBot3モデルは、Waffle_pi_LiDAR_2d.bodyとなります。このモデルファイルにRangeSensorの記述をすることでChoreonoidからLRFの情報をROSトピックとして出力できます。今回は水平方向のみのLaserScanトピックを出力するので、垂直方向の取得範囲(pitchRange)を0としています。

      -
        type: RangeSensor
        name: LiDAR
        translation: [ 0.0135, 0.013, 0.016 ]
        rotation: [ [ 1, 0, 0, 90 ], [ 0, 1, 0, -90 ] ]
        yawRange: 360.0
        yawStep:  1.0
        pitchRange: 0.0
        pitchStep: 1.0
        scanRate:  5.0
        maxDistance: 3.5
        minDistance: 0.1
        on: true

12. シンプルコントローラの実装

SLAMを行うためにシンプルコントローラからodomトピックをpublishする処理を記載しています。シミュレーション実行中に繰り返し処理されるcontrol関数でオドメトリの計測を行っています。オドメトリは車輪の回転数から移動量を計算しています。オドメトリの計測処理は、ROSの公式サイトを参考に実装しています。
http://wiki.ros.org/ja/navigation/Tutorials/RobotSetup/Odom

13. プロジェクトファイルの構成

Choreonoidでセンサの値をROSトピックとして出力するには以下の構成にする必要があります。

- World
　- ロボットモデル
　　- シンプルコントローラ
　　- BodyROS (追加必須)
　- 環境モデル
　- AISTSimulator
　　- GLVisionSimulator (追加必須)
　- WorldROS (追加必須)

GLVisionSimulator はChoreonoidでセンサのシミュレーションを行うために、AISTSimulatorの子アイテムとして追加します。次に、BodyROSはChoreonoidからセンサの値をROSトピックとして出力するために、ロボットモデルの子アイテムとして追加します。最後に、WorldROSはROSシステムの時間(ROS Time)にシミュレーション時間を使用するため、Worldの子アイテムとして追加します。

14. TurtleBot3のSLAM用launchファイルの解説

cnoid_turtlebot3_world_slam_2d.launch　の16~21行目でChoreonoidノードを起動しています。引数にプロジェクトファイルを指定することでプロジェクトファイルが読み込まれ、--start-simulationオプションでノード起動時に自動でシミュレーションが開始されます。

  <node pkg="choreonoid_ros" name="choreonoid" type="choreonoid" 
    args="$(find cnoid_turtlebot3_bringup)/project/$(arg model)_turtlebot3_world_2d.cnoid --start-simulation">
    <remap from="/waffle_pi/LiDAR/scan" to="/scan"/>
    <remap from="imu" to="/imu"/>
    <remap from="odom" to="/odom"/>
  </node>

23~24行目でstatic_transform_publisherノードを起動し、LiDARの静的座標位置を出力しています。引数には、x y z qx qy qz qw frame_id child_frame_id period_in_msを与えており、frame_id->child_frame_idの座標変換を指定しています。period_in_msは座標位置を出力する周期を指定します。ここでは、100msごとに出力しています。

　<node pkg="tf" type="static_transform_publisher" name="lidar_broadcaster"
      args="0 0 0 0 0 0 1 base_link LiDAR 100" />

26~28行目でcnoid_turtlebot3_gmapping.launchを起動し、slam_gmappingノードを起動しています。

  <include file="$(find cnoid_turtlebot3_slam)/launch/cnoid_turtlebot3_$(arg slam_methods).launch">
    <arg name="model" value="$(arg model)" />
  </include>

30~34行目でrobot_state_publisherノードを起動し、ロボットの関節位置を出力しています。

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" value="$(arg multi_robot_name)" />
    <remap from="joint_states" to="/$(env TURTLEBOT3_MODEL)/joint_states" />
  </node>

37~40行目でRVizを起動しています。

  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find cnoid_turtlebot3_slam)/rviz/cnoid_turtlebot3_$(arg slam_methods).rviz" />
  </group>

いかがでしたでしょうか。
ChoreonoidとROSでSLAMを行いたい方の参考になれば幸いです。