【ROS 2】Nav2 Keepout Zonesを使用して侵入禁止エリアを設定する

概要
1. 参考
環境
使用方法

概要

Nav2のKeepout Zoneを使用して、進入禁止エリアを設定する方法を記載します。

Nav2ではKeepoutFilterというコストマップフィルタープラグインがあり、これを使用することで、入ってはいけない部分を指定することができるようになります。

参考

https://navigation.ros.org/tutorials/docs/navigation2_with_keepout_filter.html

GitHub - ros-navigation/navigation2_tutorials: Tutorial code referenced in https://docs.nav2.org/

Tutorial code referenced in - ros-navigation/navigation2_tutorials

環境

以下の環境を前提とします。

Turtlebot3のシミュレーションについては

ROS 2 Humble
Turtlebot3 Burgerのシミュレーションを使用

Turtlebot3のシミュレーションについては以下を参考に環境構築をします。

ROBOTIS e-Manual

e-Manual wiki

使用方法

今回はチュートリアルで公開している以下のリポジトリがあるので、こちらを活用していきます。

navigation2_tutorials/nav2_costmap_filters_demo at master · ros-navigation/navigation2_tutorials

Tutorial code referenced in - ros-navigation/navigation2_tutorials

まず、チュートリアルのリポジトリをクローンします。ワークスペースはご利用のものに置き換えてください。

cd ~/colcon_ws/src
git clone https://github.com/ros-planning/navigation2_tutorials.git

パッケージをビルドします。

cd ~/colcon_ws
colcon build --symlink-install
source install/setup.bash

以下のコマンドを起動します。

cd ~/colcon_ws
ros2 launch nav2_costmap_filters_demo costmap_filter_info.launch.py params_file:=src/navigation2_tutorials/nav2_costmap_filters_demo/params/keepout_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/keepout_mask.yaml

launchファイルを見てもらうとわかりますが、filter_mask_serverとcostmap_filter_info_serverの大きく2つのNodeが起動します。それぞれについて、以下で解説します。

filter_mask_server

filter_mask_serverはmap_serverで、進入禁止エリアのpgmの情報を/keepout_filter_maskにパブリッシュします。侵入禁止エリアのpgmはlaunchのparamsで指定している、src/navigation2_tutorials/nav2_costmap_filters_demo/maps/keepout_mask.yamlで指定しています。

pgmは以下のようなファイルになります。黒い部分が侵入禁止エリアになる部分です。この地図は全体の地図の一部分になっています。このように一部分を指定するということもできるようになっています。

全体は以下のような地図になっています。

Rviz2を見ていただくとわかりますが、/keepout_filter_maskにマップがパブリッシュされていることがわかります。

RViz2のAddから/keepout_filter_maskを追加して表示します。表示されない方は、QoSのDurability_PolicyをTransient_Localに変更をすると表示されるかと思います。

costmap_filter_info_server

costmap_filter_info_serverは、この後登場するKeepoutFilterというコストマッププラグインに進入禁止に関する情報を提供するトピックをパブリッシュするノードになります。

以下のような情報がパブリッシュされます。

$ ros2 topic echo /costmap_filter_info
header:
stamp:
sec: 0
nanosec: 0
frame_id: ''
type: 0
filter_mask_topic: /keepout_filter_mask
base: 0.0
multiplier: 1.0
---

参考

ここまでのノードとトピックのグラフ表示を参考に示します。

KeepoutFilterの指定

ここまでで、チュートリアルの起動はOKですので、一度プログラムを終了しておきます。

次に、進入禁止エリアの情報を使って、Nav2で使用できるようにしていきます。これを実現するには、進入禁止用のコストマップのフィルタプラグインをグローバルコストマップとローカルコストマップに適用していきます。

以下のファイルを編集します。

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

ローカルコストマップにfiltersを指定して、keepout_filterを指定します。

keepout_filterの指定を以下のように指定します。

グローバルコストマップ側も同じように指定します。(keepout_filterの指定も同様に)

これで準備が完了したので、以下のコマンドでGazeboやNav2を起動していきます。

Gazeboの起動。

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

Nav2の起動。

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True

進入禁止エリアのマップ情報パブリッシュ

export TURTLEBOT3_MODEL=burger
cd ~/colcon_ws
ros2 launch nav2_costmap_filters_demo costmap_filter_info.launch.py params_file:=src/navigation2_tutorials/nav2_costmap_filters_demo/params/keepout_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/keepout_mask.yaml

うまくいくと、以下のようにRViz2上に進入禁止エリアのコストが反映されて、Nav2 Goalで指示すると、進入禁止エリアを避けて経路計画されます。