環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | noetic |
| Gazebo | 11.10.2 |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
move_baseの要素を大きく分けるとcostmapとplannerに分かれます。中でもcostmapは設定項目が多く、きちんと設定しないとうまく動きません。
ここではcostmapのパラメーターの設定方法の解説を行っていきます。
コストマップの設定
move_baseではcostmapのパラメーターは
- globalコストマップ：move_base/global_costmap/***
- localコストマップ：move_base/local_costmap/***
の名前のrosparamで管理されています。このようにglobalとlocalが別々のインスタンスとしてmove_baseの中で使われていますが、元は同じクラスで実装されています。パラメーターの設定によってlocalコストマップのようにふるまったり、globalのもののようにふるまったりするだけです。
またcostmapの使用がHydroから大きく変わっていて、パラメーター設定の方法がpre-Hydro形式と最新の形式があります。pluginsというパラメーターがないとpre-Hydro形式になって色々なパラメーターを良しなに内部で設定してくれます。ネットで調べるとpre-Hydro形式が多く出てくるのですが、最新の形式のほうが詳細に設定をすることができるのでこの方式を説明します。
pluginのシステム
costmapの中身はpluginを積み重ねていくことで実装されます。今回は以下の4つのプラグインを扱います。
- costmap_2d::StaticLayer
gmappingなどで生成したmapをそのまま取り込みます。 - costmap_2d::ObstacleLayer
LaserScanやPointCloud型のLidarのデータをmapに取り込みます。 - range_sensor_layer::RangeSensorLayer
Range型の超音波、PSDセンサーのデータをmapに取り込みます。 - costmap_2d::InflationLayer
マップ上の障害物をを機体の大きさに合わせて膨張処理を行います。
インストール
range_sensor_layer::RangeSensorLayerはnavigation stackのものではないので、これだけ別にインストールします。
sudo apt install ros-kinetic-range-sensor-layer
ソースコード
move_baseの代わりにcostmapだけを立ち上げるノードを製作しました。my_costmapという名前空間でこのコストマップの設定ができます。
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
int main(int argc, char** argv){
ros::init(argc, argv, "costmap");
tf::TransformListener tf(ros::Duration(10));
costmap_2d::Costmap2DROS costmap("my_costmap", tf);
ros::NodeHandle n;
costmap.start();
ros::spin();
return 0;
}
ビルド
cd ~/catkin_ws
catkin_make
実行
global1
globalマップとして動くものでStaticLayerのみの例です。SLAMの生成したマップを入力して、それをそのまま出力します。
my_costmap:
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
global_frame: dtw_robot1/map
robot_base_frame: dtw_robot1/base_link
update_frequency: 2.0
publish_frequency: 2.0
static_layer:
map_topic: "dtw_robot1/map"
track_unknown_space: false
subscribe_to_updates: true
-
pluginsで使用するpluginをリストアップします。 -
global_frameとrobot_base_frameはそれぞれのtfの名前を入れます。 -
update_frequencyとpublish_frequencyはそれぞれコストマップの更新の頻度とそれをmsgとしてpublishする頻度です。publish_frequencyが0だと可視化がうまくできません。 -
static_layer/map_topicは入力とするmapのトピック名です。 -
static_layer/track_unknown_spaceはunknown領域の扱いです。trueならunknown領域として扱いますが、falseならfree領域として扱います。globalではunknown領域をそのまま扱うと、そこに行くパスが生成されなくなるので、falseがおすすめです。 -
static_layer/subscribe_to_updatesはmapトピックに加えて、map_updateトピックを受け入れるかのフラグです。trueが良いでしょう。
roslaunch nav_lecture costmap_test.launch type:=global1
↓元となるSLAMの出すmap
↓costmapの出力（unknownがfreeとして扱われています）
global2
global1にInflationLayerを追加したものです。指定の距離だけコストマップが膨張しています。
my_costmap:
plugins:
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
inflation_layer:
inflation_radius: 0.25
-
inflation_radiusはコストマップを膨張させる距離です。 -
my_costmap/footprint: [[-0.2, -0.12], [-0.2, 0.12], [0.05, 0.12], [0.05, -0.12]]などのようにfootprint要素を書くとその値がinflation_radiusよりも優先されるようです。
roslaunch nav_lecture costmap_test.launch type:=global2
local1
lidarのLaserScanがマップに反映されています。
my_costmap:
plugins:
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
global_frame: dtw_robot1/odom
robot_base_frame: dtw_robot1/base_link
# footprint: [[-0.2, -0.12], [-0.2, 0.12], [0.05, 0.12], [0.05, -0.12]]
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
update_frequency: 2.0
publish_frequency: 2.0
obstacle_layer:
observation_sources: laser_scan_sensor
laser_scan_sensor:
sensor_frame: dtw_robot1/front_laser_link
topic: /dtw_robot1/front_laser/scan
data_type: LaserScan
marking: true
clearing: true
-
rolling_windowは自分の周りの一部だけのcostmapを使うオプションです。ローカルではtrueにします。width、heightは切り取るサイズです。 -
obstacle_layerではLaserScan、PointCloud、PointCloud2の3つの型のデータから複数のデータを受けれます。observation_sourcesでは受けるデータの名前を空白区切りのリストで書き込みます（ここではlaser_scan_sensorと名前を指定しています）。 -
laser_scan_sensor内のdata_typeでは上記の3つの型の中のどれなのか、topicではtopic名、sensor_frameではframe_idを指定します。 -
markingとclearingはこのデータをマップを埋めるのに使うか、クリアするのに使うかの指定です。通常はは両方ともtrueでしょう。
roslaunch nav_lecture costmap_test.launch type:=local1
local2
local1に加えてSonarの値がマップに反映されています。
my_costmap:
plugins:
- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
sonar_layer:
topics: [ /dtw_robot1/sonar_back/range ]
clear_on_max_reading: true
-
sonar_layerではRangeセンサーの値をマップに反映できます。 -
topicsではRangeセンサーのトピックをリストで記載します。 -
clear_on_max_readingではセンサー値が上限に張り付いたときにクリアと見なすかを指定します。実在のセンサーでは無限大という出力をするものは少ないのでtrueにするのが良いでしょう。
roslaunch nav_lecture costmap_test.launch type:=local2
local3
local2にInflationLayerを追加したものです。指定の距離だけコストマップが膨張しています。
my_costmap:
plugins:
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
# 中略
robot_radius: 0.15
footprint_padding: 0.05
inflation_layer:
inflation_radius: 0.35
cost_scaling_factor: 10
roslaunch nav_lecture costmap_test.launch type:=local2
RvizのMapプラグインでtypeをmap->costmapに変えると以下のような表示に変わります。
costmapは以下のように0~100までの値をとります。
- 障害物があるマスは100
- それから膨張した部分は99
- それからコスト膨張する部分は0~98
コメント
実際にplanningをすることを考えるとglobalのinflation > localのinflationである必要があります。逆だとglobalで計算したpathがlocalでは障害物とぶつかる判定になります。
参考
costmap2d
Staticmap Layer
Obstacles Layer
Inflation Layer
Range Sensor Layer