コンテンツにスキップ

シミュレータ上で地図作成と自律走行を動かしてみる

まずは、turtlebot3シミュレーションでROS2の地図作成と自律走行を動かしてみましょう。

手順としては下記になります。
1.SLAMで地図を作成する
2.地図を保存する
3.地図を読み込み、自己位置推定をする
4.自己位置推定をしながら自律走行する

使用するlaunchファイルがあるパッケージは下記です。

  • turtlebot3_gazebo
  • nav2_bringup
  • slam_toolbox

インストールした時点で地図作成と自律走行を動かすためのlaunchファイルは/opt/ros/humble/share/[パッケージ名]/launch/ディレクトリにあります。(pixi環境の場合は$PIXI_PROJECT_ROOT/.pixi/envs/default/share/[パッケージ名]/launch/ディレクトリ)

シミュレータ上で地図作成(SLAM)を動かしてみる

turtlebot3シミュレーションとrivzを起動します。
さらにnav2_bringupパッケージのslam_launch.pyにuse_sim_time:Trueの引数を与えて起動します。
use_sim_time:Trueはシミュレーション時間を使用するという意味で、実機の場合にはFalseにします。
launchファイルの引数に何が指定できるかは、ros2 launch [パッケージ名] [launchファイル名] -sで確認できます。

# turtlebot3シミュレーションをturtlebot3_worldで起動
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
# Rviz2(navigation2の設定済み)を起動
ros2 launch nav2_bringup rviz_launch.py
# SLAMの起動(slam_toolboxパッケージ)
ros2 launch nav2_bringup slam_launch.py use_sim_time:=True
起動に成功するとrvizは下記のようになります。

速度指令をパブリッシュして地図を作成しましょう。

# キーボードで操作
ros2 run teleop_twist_keyboard teleop_twist_keyboard

地図作成(SLAM)で作成した地図を保存する

navigation2で地図を保存するにはmap_serverパッケージを用います

  • map_serverパッケージには地図保存の方法が2種類提供されています
    • map_saver_cliノード
      -f [保存先パス]オプションで任意名で保存ができます。 
      ros2 run nav2_map_server map_saver_cli -f map
    • map_saver_serverノード 
      # 今回はslam_launch.py内で立ち上げているので下記は不要
# ros2 run nav2_map_server map_saver_server
# ros2 lifecycle set /map_saver 1
# ros2 lifecycle set /map_saver 3
# サービスコマンドで地図保存ができます
ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"

上記のどちらかを実行すると、map.yamlとmap.pgmが作成されます。
このpgmファイルとyamlファイルがROS2での自律走行の地図になります。
詳細については補足資料で説明しますが、pgmファイルが占有格子地図の各グリッドの値を画像ファイルとして持ち、yamlファイルが原点座標などの設定値を持っています。 

# map.yaml
image: map.pgm
mode: trinary
resolution: 0.05
origin: [-2.95, -2.57, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

ここまでで、1度すべての端末を終了します。

シミュレータ上で自己位置推定を動かしてみる

先ほど保存した地図上で自己位置推定(地図上での自分の位置を推定する)を動かしてみましょう。

turtlebot3シミュレーションとrivzを再度起動します。
さらにnav2_bringupパッケージのlocalization_launch.pyにmap:=[マップのyamlファイルパス]とuse_sim_time:=Trueの引数を与えて起動します。

# turtlebot3シミュレーションをturtlebot3_worldで起動
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
# Rviz2(navigation2の設定済み)を起動
ros2 launch nav2_bringup rviz_launch.py
# 自己位置推定の起動
ros2 launch nav2_bringup localization_launch.py map:=map.yaml use_sim_time:=True

起動すると下記のような状態でまだ初期位置が与えられていない状態になっています。

Rviz2のツールにある2DPopseEstimateで初期位置を与えてみましょう。 2DPopseEstimateをクリックして、初期位置付近をクリック&ドラッグで、位置と向きを指定します。

この時、2DPopseEstimateは/initialposeトピックを出力しています。

なので、コマンドで/initialposeトピックを出力することでも初期位置を設定できます。 

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: -2.0, y: -0.5, z: 0.0}, orientation: {w: 1.0}}}}" -1

初期位置が設定されるとTFと赤い点でLaserScan、緑矢印で自己位置推定のパーティクルフィルタ(粒子フィルタ)の値が可視化されます。

速度指令をパブリッシュして緑矢印の自己位置推定のパーティクルの動きを観察しましょう。

# キーボードで操作
ros2 run teleop_twist_keyboard teleop_twist_keyboard
ロボットの動きに合わせてパーティクルの位置が更新されていきます。

シミュレータ上で自律走行を動かしてみる

自己位置推定ができたので、続けて自律走行を動かしまししょう。
nav2_bringupパッケージのnavigation_launch.pyにuse_sim_time:=Trueの引数を与えて起動します。 

# 自律走行の起動
ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True

起動すると下記のような色のコストマップと呼ばれるものが表示されます。

コストマップは地図全体のコストを表し、大域的な経路計画に使用されるglobal_costmapとロボット周辺のコストを表し、局所的な経路計画に使用されるlocal_costmapの2種類あります。

表示されているコストマップの色は、観測点(障害物)がピンク色で、その周囲に水色の衝突エリア(ロボットがその座標に行くと障害物に当たってしまう)、さらに、赤→紫→青→白の順に障害物との距離に応じて、コストが低くなるように表示されています。

それでは、Rviz2のツールにあるNav2 Goalでゴール座標を与えて動かしてみましょう。

動かし方は2DPopseEstimateと同様です。

経路が計画され、経路に沿ってロボットが移動すれば成功です。

また、ゴールを障害物上に設定した場合には、失敗します。

他にも左下のNavigation2ツールを使用して、Waypoints NavigationやPath Through Navigationを行うことができます。

Waypoints Navigation(順番にWaypointへ移動します)

Path Through Navigation(Wapointを全て繋げた経路を生成して移動します)

また、裏でGazeboが動いていますので、追加の障害物を置くことも可能です。

シミュレータ上で地図作成(SLAM)中に自律走行(explore)を動かしてみる

また、自己位置推定中に自律走行が動かせたので、SLAM中にも自律走行が動かせます。(SLAMは自己位置推定中に地図作成しているので)

一度全ての端末を終了して下記を実行してみましょう。

# turtlebot3シミュレーションをturtlebot3_worldで起動
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
# Rviz2(navigation2の設定済み)を起動
ros2 launch nav2_bringup rviz_launch.py
# SLAMの起動(slam_toolboxパッケージ)
ros2 launch nav2_bringup slam_launch.py use_sim_time:=True
# 自律走行の起動
ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True

今回はturtlebot3_worldを使用しましたが、他に用意されているturtlebot3_houseを使用するともう少し広い空間で行うことができますし、自作Worldでも可能です。 

# turtlebot3シミュレーションをturtlebot3_houseで起動
ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py

自律走行の簡易的な手順は、地図を作成して保存し、地図上で自己位置推定を行いながら自律走行ような手順になります。
以上でシミュレータ上で地図作成と自律走行を動かしてみるは終了です。