ROS講座115 costmapの設定を書く

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

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | noetic |
| Gazebo | 11.10.2 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
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のものではないので、これだけ別にインストールします。

```shell
sudo apt install ros-kinetic-range-sensor-layer 
```

# ソースコード
move_baseの代わりにcostmapだけを立ち上げるノードを製作しました。my_costmapという名前空間でこのコストマップの設定ができます。

```c++:nav_lecture/src/costmap_test.cpp
#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;
}
```

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行

## global1
globalマップとして動くものでStaticLayerのみの例です。SLAMの生成したマップを入力して、それをそのまま出力します。

```yaml:nav_lecture/config/move_base/global1_costmap_params.yaml
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が良いでしょう。

```shell:サンプルの実行
roslaunch nav_lecture costmap_test.launch type:=global1
```

↓元となるSLAMの出すmap
![nav_global0.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/afa20c2f-ef84-6516-35ae-834214d91a8b.png)

↓costmapの出力（unknownがfreeとして扱われています）
![nav_global1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/a40357d8-9b3f-4252-cbe3-7ef8b1184858.png)


## global2
global1にInflationLayerを追加したものです。指定の距離だけコストマップが膨張しています。

```yaml:nav_lecture/config/move_base/global2_costmap_params.yamlの一部（global1との差分要素）
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よりも優先されるようです。

```shell:サンプルの実行
roslaunch nav_lecture costmap_test.launch type:=global2
```

![nav_global2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/a7bcf9f7-17b3-d6ef-fa46-32619b7183e1.png)


## local1
lidarのLaserScanがマップに反映されています。

```yaml:nav_lecture/config/move_base/local1_costmap_params.yaml
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でしょう。

```shell:サンプルの実行
roslaunch nav_lecture costmap_test.launch type:=local1
```

![nav_local1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/ff645ece-a335-8e91-d827-574d9546a426.png)

## local2
local1に加えてSonarの値がマップに反映されています。

```yaml:nav_lecture/config/move_base/local2_costmap_params.yamlの一部(local1との差分要素)
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にするのが良いでしょう。

```shell:サンプルの実行
roslaunch nav_lecture costmap_test.launch type:=local2
```

![nav_local2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/c123134a-3238-9c73-661d-8af92858e52a.png)

## local3
local2にInflationLayerを追加したものです。指定の距離だけコストマップが膨張しています。

```yaml:nav_lecture/config/move_base/local3_costmap_params.yamlの一部(local2との差分)
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
```

```shell:サンプルの実行
roslaunch nav_lecture costmap_test.launch type:=local2
```

![costmap_inf1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/d88cc2f5-a5fb-403c-67b4-483a96d016f4.png)

RvizのMapプラグインでtypeをmap->costmapに変えると以下のような表示に変わります。

![costmap_inf2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/eaa5283e-e132-2fee-f4bc-a0e14e87ccb1.png)

costmapは以下のように0~100までの値をとります。

* 障害物があるマスは100
* それから膨張した部分は99
* それからコスト膨張する部分は0~98

![nav_costmap.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/454f4e59-aa9e-3f77-96f3-423de326e7ab.jpeg)

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

# 参考
[costmap2d](http://wiki.ros.org/costmap_2d)
[Staticmap Layer](http://wiki.ros.org/costmap_2d/hydro/staticmap)
[Obstacles Layer](http://wiki.ros.org/costmap_2d/hydro/obstacles)
[Inflation Layer](http://wiki.ros.org/costmap_2d/hydro/inflation)
[Range Sensor Layer](http://wiki.ros.org/range_sensor_layer)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)