Nav2を使った地図生成と自己位置推定

Nav2でサポートされているslam_toolboxを使ってSLAMを行う．SLAM Toolboxの詳しい説明は，ここのビデオを見ること．

自己位置推定にはモンテカルロ位置推定（AMCL)が実装されているnav2_amclを用いる．

slam-toolboxのインストールを下記のコマンドで実行

sudo apt install ros-humble-slam-toolbox

多分，FullでROS 2を入れている人はすでに入っている．

２Dコストマップ

Nav2のコストマップはnav2_costmap_2dを用いて実装する．

パッケージは、静的レイヤー、インフレーション レイヤー、範囲レイヤー、障害物レイヤー、およびボクセル レイヤーで構成されてい る．

今回は，静的レイヤー、障害物レイヤー、ボクセル レイヤー、インフレーション レイヤーを使用する構成例を示す．

障害物レイヤー、ボクセル レイヤーがLidarからのLaserScan型のトピック/scanを使用するように設定する．

ということで，下記のyamlファイルを理解すればよいのだが．．．どこに保存するかはチュートリアルには記載がされていない．．．ロボット用にnavigation.yamlを生成してそこに保存する情報の一部です．

global_costmap:

  global_costmap:

    ros__parameters:

      update_frequency: 1.0

      publish_frequency: 1.0

      global_frame: map

      robot_base_frame: base_link

      use_sim_time: True

      robot_radius: 0.22

      resolution: 0.05

      track_unknown_space: false

      rolling_window: false

      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

      static_layer:

        plugin: "nav2_costmap_2d::StaticLayer"

        map_subscribe_transient_local: True

      obstacle_layer:

        plugin: "nav2_costmap_2d::ObstacleLayer"

        enabled: True

        observation_sources: scan

        scan:

          topic: /scan

          max_obstacle_height: 2.0

          clearing: True

          marking: True

          data_type: "LaserScan"

          raytrace_max_range: 3.0

          raytrace_min_range: 0.0

          obstacle_max_range: 2.5

          obstacle_min_range: 0.0

      inflation_layer:

        plugin: "nav2_costmap_2d::InflationLayer"

        cost_scaling_factor: 3.0

        inflation_radius: 0.55

      always_send_full_costmap: True

local_costmap:

  local_costmap:

    ros__parameters:

      update_frequency: 5.0

      publish_frequency: 2.0

      global_frame: odom

      robot_base_frame: base_link

      use_sim_time: True

      rolling_window: true

      width: 3

      height: 3

      resolution: 0.05

      robot_radius: 0.22

      plugins: ["voxel_layer", "inflation_layer"]

      voxel_layer:

        plugin: "nav2_costmap_2d::VoxelLayer"

        enabled: True

        publish_voxel_map: True

        origin_z: 0.0

        z_resolution: 0.05

        z_voxels: 16

        max_obstacle_height: 2.0

        mark_threshold: 0

        observation_sources: scan

        scan:

          topic: /scan

          max_obstacle_height: 2.0

          clearing: True

          marking: True

          data_type: "LaserScan"

      inflation_layer:

        plugin: "nav2_costmap_2d::InflationLayer"

        cost_scaling_factor: 3.0

        inflation_radius: 0.55

      always_send_full_costmap: True

Slam-toolboxの動作確認

まず，Gazebo環境を立ち上げる．

cd ros2_ws

colcon build

. install/setup.bash

ros2 launch sam_bot_description display.launch.py

slam-toolboxを別のターミナルから下記コマンドで実行

ros2 launch slam_toolbox online_async_launch.py

このコマンドを打つだけで，なぜかSlamが始まる．設定をどこですればよいか不明なチュートリアル．

rvizを立ち上げて左のDisplaysに「Add」でMapを追加して，Topicに「/map」を選択すると下図の通り，地図が表示される．

Nav2の設定

下記２つのパッケージをインストールしておく．

sudo apt install ros-humble-navigation2

sudo apt install ros-humble-nav2-bringup

パラメータを入力しなくても一応デフォルトで動く？

ros2 launch nav2_bringup navigation_launch.py

自分の設定を反映したいときは，configディレクトリの下に，nav2_params.yamlファイルを生成して，ここのリンク先のデータをペーストする．

robot_model_type: "nav2_amcl::DifferentialMotionModel"

の記載を上記の様に変更．古いのでdifferentiationになっているはず．

他にも，odom_topic:/demo/odomとなっているので/odomに変更．２箇所ある

修正面倒なので，turtlebot3_navigation2のディレクトリにあるburger.yamlを使えばOK．もしくは，ここから拾ってくる．

turtlebot3のチュートリアルをやったことがある人なら，PC内の下記のディレクトリにburger.yamlがある．

/opt/ros/humble/share/turtlebot3_navigation2/param/burger.yaml

これの内容をnav2_params.yamlとして保存すればOK.　こちらも，foxyの古い設定なので，robot_model_type: "nav2_amcl::DifferentialMotionModel"に修正しておくこと．

その後ビルドする．（しなくても良いかも．．．）

下記で実行．２行に見えるが１行で入力して実行．

ros2 launch nav2_bringup navigation_launch.py params_file:=/home/takubo/ros2_ws/src/sam_bot_description/config/nav2_params.yaml

例と同じように作成していてもfull pathは皆さんの環境のパスを記載してください．

nav2を実行後にSLAMでマップが描画されていたTopicのトグルをクリックすると別のトピックを選ぶことができる様になっている．

/local_costmap/costmapを選択したときは下記画面になる

保存したマップでのナビゲーション

保存したマップを使ってナビゲーションするときは下記のコマンド

ros2 launch sam_bot_description display.launch.py

上記launchで同時に立ち上がっているrviz2の中で，左メニューの下の「Add」を押して「Map」を追加．

追加したMapの中のTopicを「map」と記載しておく．

また，左メニューの上の方にあるFixed Frameも「map」と手動で書き換えておく．

その後，別端末で下記コマンドでnav2を立ち上げる．

 ros2 launch nav2_bringup bringup_launch.py params_file:=/home/takubo/ros2_ws/src/sam_bot_description/config/nav2_params.yaml use_sim_time:=True map:=/home/takubo/tmp/test_map.yaml

するとrviz上に地図がloadされて表示されるので，

メニューの上にある「＋」ボタンで出てくるウィンドウから「SetInitialPose」を押して，上部メニューに初期位置の矢印を置くことのできるツールを追加．