概要
Nav2のSpeedFilterを使用して、速度制限エリアを設定する方法を記載します。
Nav2ではSpeedFilterというコストマップフィルタープラグインがあり、これを使用することで、速度制限エリアを指定することができるようになります。
参考
環境
以下の環境を前提とします。
Turtlebot3のシミュレーションについては
- ROS 2 Humble
- Turtlebot3 Burgerのシミュレーションを使用
Turtlebot3のシミュレーションについては以下を参考に環境構築をします。
使用方法
今回はチュートリアルで公開している以下のリポジトリがあるので、こちらを活用していきます。
まず、チュートリアルのリポジトリをクローンします。ワークスペースはご利用のものに置き換えてください。
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_serverとcostmap_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にマップがパブリッシュされていることがわかります。
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.pyNav2の起動。
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についての詳細は、以下にソースコードがありますので、こちらが参考になります。
コメント