環境

この記事は以下の環境で動いています。

インストールについては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"}
  inflation_layer:
    inflation_radius: 0.15

roslaunch nav_lecture costmap_test.launch type:=local2

コメント

実際にplanningをすることを考えるとglobalのinflation > localのinflationである必要があります。逆だとglobalで計算したpathがlocalでは障害物とぶつかる判定になります。

参考

costmap2d
Staticmap Layer
Obstacles Layer
Inflation Layer
Range Sensor Layer

目次ページへのリンク

ROS講座の目次へのリンク