2次元地図からGazeboのmodelを作ってナビゲーションしてみた

#目次
1.記事の内容について
2.map2gazeboを使ってみる
3.map2gazeboで生成したmodelを使ってナビゲーションをしてみる
4.おわりに
5.おまけ

1. 記事の内容について

こんにちは、12月2日を担当させていただく、BEIKEです。

今年から初めてつくばチャレンジに参加しました。
つくばチャレンジの練習走行では、まずrosbagでのデータ取りを行いました。
rosbagを取っておけば現地じゃなくても、そのデータを基にマッピングや自己位置推定のテスト
を行うなどの作業を行うことが出来ます。

しかし、rosbagのデータでは現地のような環境でナビゲーションをテストすることが出来ないので
出来たら良いのになーと考えていました。

そうだ、2次元地図からGazeboのワールドを作れば良いのでは？！
(2次元地図を自己位置推定やナビゲーションに使っていたので)
調べたらありました。

ということで、map2gazeboを使って2次元地図から
Gazeboのmodelを作ってナビゲーションしてみた話をこれからさせて頂きます。

2. map2gazeboを使ってみる

2.1 ワークスペースの構築からビルドまで

mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src
git clone https://github.com/shilohc/map2gazebo.git
cd ~/catkin_ws && catkin build
source ~/catkin_ws/devel/setup.bash

2.2 map2gazebo.launchの実行

map2gazebo.launchを実行します。
引数のparams_fileにはmodelを生成するためのパラメータが書かれたファイルのパスを渡します。
パラメータはmodelの出力形式(.stl、.dae)やmodelの高さなどがあります。

roslaunch map2gazebo map2gazebo.launch params_file:=$(rospack find map2gazebo)
/config/defaults.yaml

次に生成したいmodelの基になるマップのトピックをmap_serverにてpublishします。

rosrun map_server map_server map.yaml

そうすると、以下のようにmap2gazeboがマップを受け取ったメッセージが出るのでしばらく待ちます。
[INFO] [1638213724.217835]: Received map

ｌ
map2gazebo.launchやdefaults.yamlに出力先のパスをしていなければデフォルトでmap2gazebo/models/map/meshesに生成されたmodelが出力されます。

以下のようなメッセージが出れば完了です。
[INFO] [1638213779.569149]: Exported STL. You can shut down this node now

2.3 生成されたmodelの確認

今回modelを生成するために使用した地図はこちら(つくば市役所前から信号ありの横断歩道までです)

生成したmodelを確認してみます。
map2gazeboにあるgazebo_world.launchを使用して確認します。

roslaunch map2gazebo gazebo_world.launch

占有格子地図の占有された部分のみを押し出したようなmodelが出来ました。

衛星写真と一緒に見ると壮観です。
良い感じのナビゲーションの簡易テスト環境が出来上がりました！

3. map2gazeboで生成したmodelを使ってナビゲーションをしてみる

3.1 TurtleBot3を使って試してみる

まず、TurtleBot3のシミュレータ環境の構築を行います。
先程、map2gazeboで使用したcatkin_wsを使用します。

cd ~/catkin_ws/src
git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
catkin build
source ~/catkin_ws/devel/setup.bash

ワークスペースはこんな感じになってると思います。

hoge@hoge:~/catkin_ws/src$ tree -L 1
.
├── map2gazebo
├── turtlebot3
├── turtlebot3_msgs
└── turtlebot3_simulations

map2gazeboで生成したmodelを使用した環境を立ち上げるために
こんな感じのGazebo用のlaunchを書きます。

<launch>
  <env name="GAZEBO_MODEL_PATH" value="$(find map2gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>

  <arg name="model" default="burger"/>
  <arg name="x_pos" default="0.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find map2gazebo)/worlds/map.sdf"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro"/>

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model turtlebot3 -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description"/>
</launch>

作成したGazebo用のlaunchを立ち上げます。

roslaunch turtlebot3_gazebo hogehoge.launch

次にマップは作成済みとしてナビゲーションを立ち上げます。

roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

準備は出来たので、2D Nav Goalを適当な場所に置いてナビゲーションしてみます。

ナビゲーションできました。

3.2 実際に使ってた時の動画とつくばチャレンジの本走行の動画

• 実際にナビゲーションのテストをしている様子(waypointやパスプランの確認をした)

• 実際のつくばチャレンジでの走行（テストした通りのwaypoint通過、パスプランだった（多分））

上の2つの動画のナビゲーションはraspicat_navigationというパッケージを使用しました。

4. おわりに

今回は2次元地図からGazeboのmodelを作ってナビゲーションしてみました。

作成した環境は地面の凹凸は無いものの観測状況は現地の環境に少し近い感じで良いテスト環境が出来上がりました。
地面の凹凸なども再現したい場合は3次元データからstlなどに変換するとできるのかもしれません。

これで現地の環境に少し近いナビゲーションのテストが出来て良いのではないでしょうか？！

最後まで読んでくださって、ありがとうございました。

5. おまけ

この記事の内容に関連した情報を最近知ったので追記します。(2021/12/06)

この記事のやってることの逆で、Gazeboのワールドから2次元マップを作成する方法があるようです。