【ROS 2】Nav2 SpeedFilterを使用して速度制限エリアを設定する

ROS

概要

Nav2のSpeedFilterを使用して、速度制限エリアを設定する方法を記載します。

Nav2ではSpeedFilterというコストマップフィルタープラグインがあり、これを使用することで、速度制限エリアを指定することができるようになります。

参考

Navigating with Speed Limits — Nav2 1.0.0 documentation
GitHub - ros-planning/navigation2_tutorials: Tutorial code referenced in https://navigation.ros.org/
Tutorial code referenced in - ros-planning/navigation2_tutorials

環境

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

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

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

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

ROBOTIS e-Manual
e-Manual wiki

使用方法

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

https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_costmap_filters_demo

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

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/speed_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/speed_mask.yaml

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

filter_mask_server

filter_mask_serverはmap_serverで、速度制限エリアのpgmの情報を/speed_filter_maskにパブリッシュします。速度制限エリアのpgmはlaunchのparamsで指定している、src/navigation2_tutorials/nav2_costmap_filters_demo/maps/speed_mask.yamlで指定しています。

pgmは以下のようなファイルになります。黒やグレーの部分が制限速度エリアになります。

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

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

costmap_filter_info_server

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

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

 ros2 topic echo /costmap_filter_info
header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
type: 1
filter_mask_topic: /speed_filter_mask
base: 100.0
multiplier: -1.0
---

参考

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

SpeedFilterの指定

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

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

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

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

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

コントローラサーバに、speed_limit_topicを指定します。

これで準備が完了したので、以下のコマンドで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
ros2 launch nav2_costmap_filters_demo costmap_filter_info.launch.py params_file:=src/navigation2_tutorials/nav2_costmap_filters_demo/params/speed_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/speed_mask.yaml

うまくいくと、以下のようにRViz2上に制限速度エリアが反映されて、Nav2 Goalで指示すると、制限速度エリアで減速することがわかります。

制限速度エリアを表示したい場合は、MapのTopicを/speed_filter_maskに切り替えると表示されます。

参考

上記起動時のノードグラフを貼り付けます。赤い部分がSpeedFilterに関連する部分になります。

SpeedFilterについての詳細は、以下にソースコードがありますので、こちらが参考になります。

https://github.com/ros-planning/navigation2/blob/humble/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

コメント