はじめに

　本記事のシチュエーションは、

実機のロボットを動作させる環境（実環境）の図面や大きさ寸法が予め既知
実環境で実機を頻繁に動かせない（展示会場、学会会場、人通りの多い校舎 .etc）
実環境で将来的にデモンストレーションや発表を行うにあたり、予め実環境でのシミュレーションを行う必要あり

を想定しています。ユースケースとしては稀かもしれませんが、どなたかのお役に立てれば幸いです。

実環境のモデリングからGazeboでの召喚

　筆者は以下の記事の手順を上からなぞることで、問題なく、自作の3DデータをGazeboでSpawnさせることができました。

　重複するので詳しく説明はしませんが、モデリングしたデータを.fbx形式にしてBlenderで読み込んでから、.sdf形式と.dae形式に出力します。
　そのうえで、適切なファイル構成や設定ファイルの編集を行います。

　今回はFusion360で以下のような壁と柱によって構成される実環境をモデリングしました。

　以下のlaunchのコードをベースにファイルの指定や初期位置の指定を行います。
　新しくワークスペースを建てるなり、ビルドして使っている方はファイルを同じ階層でコピペして編集するなりしましょう。

　以下は変更箇所の一部抜粋です。

custom_world.launch.py

use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    x_pose = LaunchConfiguration('x_pose', default='8.0') #X座標の初期位置
    y_pose = LaunchConfiguration('y_pose', default='4.0') #Y座標の初期位置

    world = os.path.join(
        get_package_share_directory('turtlebot3_gazebo'),
        'worlds',
        'custom_world.world' #ワールドの指定
    )

　Turtlebot3のシミュレーション関連のLaunchで用意したモデルを召喚したのが以下の図です。

自律移動のナビゲーション

　基本的な手順はROBOTIS E-manualに準拠しています。

実環境の地図生成

　E-manualと同じくCartographerを使用して地図を作っていきます。

問題点

　今回対象とした実空間はそこそこ広い空間であり、柱や壁と壁の距離が最大7mあり、全体は一辺20m近くあるスペースを使用しました。
そこで、発生したのが、Cartographerで地図がうまく生成されない、という問題です。

　具体的には、以下の画像のように広い空間で自己位置がロストしてしまっているのか、すでに推定した実環境の輪郭が徐々に乱れるという問題です。

　いくつかの懸念点を精査した結果、

シミュレーション上のLidarの設定
Cartographerの設定

を適切に調整することで地図を適切に処理することができました。

シミュレーション上のLidarの設定

　Gazeboに召喚されるTurtlebot3の設定については、turtlebot3_gazeboパッケージ内のmodelsに集約されています。

　使用しているロボットモデルの.sdfファイルを開き、以下の部分を編集します。
具体的にはLidarで取得する最大レンジと最小レンジです。

model.sdf

<sensor name="hls_lfcd_lds" type="ray">
        <always_on>true</always_on>
        <visualize>true</visualize>
        <pose>-0.064 0 0.121 0 0 0</pose>
        <update_rate>5</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1.000000</resolution>
              <min_angle>0.000000</min_angle>
              <max_angle>6.280000</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.20000</min> <!-- 元の値: 0.12000 -->
            <max>15.0</max> <!-- 元の値: 3.5 -->
            <resolution>0.015000</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </ray>
        <plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
          <ros>
            <!-- <namespace>/tb3</namespace> -->
            <remapping>~/out:=scan</remapping>
          </ros>
          <output_type>sensor_msgs/LaserScan</output_type>
          <frame_name>base_scan</frame_name>
        </plugin>
      </sensor>

　最小レンジを修正する必要はこの文脈では無いように思われますが、筆者が試したところ元の値のままだと、角度の一部分が測定できたりできなかったり、ノイズが走るような挙動が見られたりしました。
　これに対して、最小レンジを大きくすることで解決しました。

Cartographerの設定

　E-manualで使用しているcartographer.launch.pyでは設定ファイルとしてturtlebot3_lds_2d.luaがデフォルトで設定されているので、これの中身を編集します。

　具体的には、先ほどのTurtlebot3のLidarの設定と同様、採用するレンジの最大最小の設定です。
　以下はその一部抜粋です。

turtlebot3_lds_2d.lua

TRAJECTORY_BUILDER_2D.min_range = 0.12
TRAJECTORY_BUILDER_2D.max_range = 15.0
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

　この中のTRAJECTORY_BUILDER_2D.min_rangeとTRAJECTORY_BUILDER_2D.max_rangeの編集を行いました。

　ここまで出来たら、Turtlebot3パッケージのビルドを忘れずに行いましょう。

$ cd (PATH_TO_TB3_WORKSPACE)
$ colcon build --symlink-install

　無事、うまくいけば、Gazebo上で可視化されているLidarの光線が指定した分延長されているのが確認できるはずです。

完成したもの

　いくつかの設定を適切にしたうえで、Cartographerを実行しました。

ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True

　Rviz2上の推定範囲が設定に伴って大きくなっていることが確認できるはずです。

　地図の保存は以下のノードから実行します。

ros2 run nav2_map_server map_saver_cli -f ~/map

ナビゲーション

　地図まで作成できれば、あとはNavigation2のチュートリアルと同様です。

$ ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True map:=$HOME/map.yaml

$ ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/rviz/nav2_default_view.rviz

補足

　今回は実環境を模した広めの空間をシミュレーション上でマッピングするにあたり、Lidarの能力を底上げするかたちで無理やり解決したかたちになっています。
　デフォルトのLidarの設定は最大半径3.5mに設定されており、筆者の所持しているLidarは最大半径8mなので、地図が取れ次第、実機のパラメータに調整する必要があると思っています。

参考