ROS navigation memo

#更新履歴
- 2019/07/31 Video に "Demo @ fooma japan 2019" を追加。

#Cartographer with LaserScan(ydlidar x4)
![5EgdsxaiQc6OaaG3cYJArg.jpg](https://qiita-image-store.s3.amazonaws.com/0/124209/7e8dbf51-b45d-e08c-6501-80ed9db54edd.jpeg)

#move_base with ydlidar x4 and cartographer
![xtEG0iE%T42IKO7yK2xB9g.jpg](https://qiita-image-store.s3.amazonaws.com/0/124209/d9fee642-7108-5141-b322-75deae9cc943.jpeg)
![IbOc0YKjR%KTThkByafsQQ.jpg](https://qiita-image-store.s3.amazonaws.com/0/124209/75429f9a-ab24-8ec4-fd4b-71f96e911c5e.jpeg)

#Video
- [Navigation with our robot base](https://www.dropbox.com/s/sy17w5pwa6ec3g3/IMG_3212.mov?dl=0)
- [Follow the waypoints](https://www.dropbox.com/s/idy5xfv30kykg3s/IMG_3664.mov?dl=0)
- [Demo @ fooma japan 2019](https://youtu.be/tYYMFGtg2y0)

#map
- [costmap_2d](http://wiki.ros.org/costmap_2d?distro=melodic)
    - [obstacles are not cleared completely in costmap](https://answers.ros.org/question/257286/obstacles-are-not-cleared-completely-in-costmap/)
    - [Clearing local costmap in ROS when using PointCloud2](https://foxypanda.me/clearing-local-costmap-in-ros-with-pointcloud2/)
    - local_costmap
        - static_map: false
        - rolling_window: true
        - pluginはobstacle_layerとinflation_layer
        - センサによる障害物検出(obstacle_layer)を元に障害物影響レイヤー(inflation_layer)を作るもので暫定的なmapの意味を持つように設定すると良い。
    - global_costmap
        - static_map: true
        - rolling_window: false
        - pluginはstatic_layerとinflation_layer
        - SLAMで作られるmap(static_layer)とセンサによる障害物検出(obstacle_layer)を元に障害物影響レイヤー(inflation_layer)を作るもので恒常的なmapの意味と暫定的なmapの両方を兼ねるように設定すると良い。
        - static_layerからのinflation_layerだけだとstatic_layerが把握していない一時的な、もしくは新たな障害物が置かれた場合、global_plannerはそれを無視して経路を作るが、近ずいた時に発見したlocal_plannerは新たな経路をglobal_plannerに求める。しかしglobal_plannerは情報が無いので同じ経路を提供し、結局スタックから抜け出せなくなる。これを避ける為にglobal_costmapにもリアルタイムセンサ情報による障害物検出(obstacle_layer)を元にした障害物影響レイヤー(inflation_layer)も使用する必要があるのだ。
        - SLAMとしてcartographerとocupancy_grid_nodeを動かしていると、確率的に障害物mapが作成されるので時間と共にinflation_layerが生成されていく様は見ていてとても面白い。
    - obstacle_layer
        - obstacle_range
            - この長さ以内を検出したら、それを障害物とする。
            - local_costmapの一辺の長さの半分程度が良いように思える。
        - raytrace_range
            - この長さ以上を検出したら、そこまでの間をクリアする。
            - obstacle_range+0.5程度だろう。
    - inflation_layer
        - inflation_radius: 0.55
            - 障害物から影響を受ける距離
        - cost_scaling_factor: 10.0
            - 障害物から上記inflation_radiusまで指数関数的に減衰しながら影響を受けるが、その減衰具合を設定する。大きいほど減衰が早くなる。
- [map_server](http://wiki.ros.org/map_server?distro=melodic)
- [cartographer_occupancy_grid_node](https://google-cartographer-ros.readthedocs.io/en/latest/ros_api.html)
    - cartographerを使用する場合、これをmap_serverとして利用する。

#base_global_planner
- [global_planner/GlobalPlanner](http://wiki.ros.org/global_planner?distro=melodic)
    - 現在利用中。
- [navfn/NavfnROS](http://wiki.ros.org/navfn?distro=melodic)
- [carrot_planner/CarrotPlanner](http://wiki.ros.org/carrot_planner?distro=melodic)
    - 障害物手前まで直線で経路を作る。これは実験用だろう。

#base_local_planner
- [base_local_planner/TrajectoryPlannerROS](http://wiki.ros.org/base_local_planner?distro=melodic)
    - なんだかんだで計算速度が速く変な動作が無い。
    - dwa: false にして現在利用中。
    - [want to completely follow the global path](https://answers.ros.org/question/241474/move_base-want-to-completely-follow-the-global-path/)
- [dwa_local_planner/DWAPlannerROS](http://wiki.ros.org/dwa_local_planner?distro=melodic)
    - -x方向に移動する必要がある場合にうまく機能しない時がある。
    - [robot unable to rotate in place for dwa_local_planner](https://answers.ros.org/question/210914/robot-unable-to-rotate-in-place-for-dwa_local_planner/)
    - 障害物回避パラメータについて
        - scaling_speed を超えた並進スピードの時に自分のfootprintのスケールを拡大して障害物と接触する確率を下げる。最大並進速度max_vel_transの時の拡大率がmax_scaling_factor+1となる。max_scaling_factorが0.2の場合、scaling_speedはmax_vel_transの4割程度が良いように思える。（個人の感想です）
- [eband_local_planner/EBandPlannerROS](http://wiki.ros.org/eband_local_planner)
    - melodic用は無い。
    - [gitにmelodic用のブランチ](https://github.com/mintar/eband_local_planner/tree/update_melodic)があったのでやってみた。
    - コンパイルはできたが実行時にtf2::doTranstateが参照できないと言うエラーになり実行できなかった。navigation等はaptで入れているが、これはソースからコンパイルしているので、plugin関連で何らかの実行時環境コンフリクトが起きているのかもしれないが、詳細は探求できていない。
    - もしかしたらtf2とかnavigationとかもソースからコンパイルするようにすれば良いのかもしれない。明日にでもやってみようかな。
- [teb_local_planner/TebLocalPlannerROS](http://wiki.ros.org/teb_local_planner)
    - Timed-Elastic-Band 時限-弾力-バンド
    - 計算量が多いという噂であるが実証してみる必要を感じている。
        - 確かに多い。ローカルコストマップのサイズにも依るようだ。
    - 移動開始時にその場回転を期待したのだが、乗用車のスイッチバックのような方向転換を繰り返してしまう。
    - 目的地に達してもスピードが落とさないで通り越してから戻ってくる感じ。
    - パラメータの設定をまだ良く理解していないのが要因かもしれないが、使用は控えようと思う。
- [simple_local_planner/SimpleLocalPlanner](https://github.com/DLu/simple_local_planner)
    - 雛形として使えそうだ。
    - 残念ながらmelodic用ではない。
    - 未評価

#other_planner
- [sbpl](http://wiki.ros.org/sbpl)

#recovery_behaviors
- [rotate_recovery/RotateRecovery](http://wiki.ros.org/rotate_recovery?distro=melodic)
    - 回転してリカバリーを試みる。
- [move_slow_and_clear/MoveSlowAndClear](http://wiki.ros.org/move_slow_and_clear?distro=melodic)
    - グローバルとローカルマップの自分の周りを四角くクリアする。
    - 前後左右clearing_distanceの距離。
    - 設定した並進速度limited_trans_speedと角速度limited_rot_speedに変更する。
    - 指定した移動距離limited_distanceになるまでそれを続ける。
    - 指定したプランナーplanner_namespaceがmax_trans_velとmax_rot_velを持っていることを想定している。
    - つまりDwaPlannerROSでないと使えない。
- [clear_costmap_recovery/ClearCostmapRecovery](http://wiki.ros.org/clear_costmap_recovery?distro=melodic)
    - 自己周りのローカルマップを消去してリカバリーを試みる。
    - 人などの障害物が移動して経路に入った時に有用だ。

#slam
- [gmapping/slam_gmapping](http://wiki.ros.org/gmapping)
    - melodic用は現在用意されていないが利用できる。（下記参照）
    - realsenseのPointCloud2をLaserScan化して使ってみたが広いところではズレまくっていた。realsenseだと同時に取得できるのは前方左右90度程度で距離も5m程度なのでそれが要因かもしれない。次にLiDAR使ってから判断してみるつもり。
- [amcl/amcl](http://wiki.ros.org/amcl)
    - 自己位置同定用。マップ作成機能は無い。
    - [Tuning AMCL's diff-corrected and omni-corrected odom models](https://answers.ros.org/question/227811/tuning-amcls-diff-corrected-and-omni-corrected-odom-models/)
- [cartographer_ros/cartographer_node](https://google-cartographer-ros.readthedocs.io/en/latest/)
    - LiDARと共に利用した場合、とても優れている。現在使用中。
    - realsense2のpointcloudでやってみたが、うまく動作しない。設定方法がよくわからない。
    - [github](https://github.com/googlecartographer/cartographer)
- [mrpt_slam](http://wiki.ros.org/mrpt_slam?distro=melodic)
    - 未評価
- [hector_slam](http://wiki.ros.org/hector_slam)
    - odometry情報を必要としないが微妙。
    - 未評価
- [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)
    - RGB-D情報を入力する。realsenseなんかで良いかも。
    - 未評価

#別系統navigation
- [mrpt_navigation](http://wiki.ros.org/mrpt_navigation)

#全般のチューニング
- [ROS Navigation Tuning Guide](http://kaiyuzheng.me/documents/navguide.pdf)
    - 各プランナーのパラメータについて詳しく記述されている。
    - 必読！

#install navigation to melodic
```.sh
$ sudo apt install ros-melodic-navigation
```

#install gmapping to melodic
```.sh
$ cd ~/catkin_ws
$ git clone https://github.com/ros-perception/openslam_gmapping src/openslam_gmapping
$ git clone https://github.com/ros-perception/slam_gmapping src/slam_gmapping
$ rosdep install --from-paths src/ -i
$ catkin_make
$ source devel/setup.zsh #if you use bash, setup.bash
```

#install cartographer to melodic
```.sh
$ sudo apt install ros-melodic-cartographer-ros
```

#install ydlidar
2018/12/27日現在、[本家ydlidar](https://github.com/EAIBOT/ydlidar)はmapやodomをFixedFrameにするとrvizで表示されないバグ（LaserScanのtopicの時間単位がsecなのにnano secにしている！）がある。
pull requestは出しておいたが、僕のgithubに修正したものを置いてあるので以下はそちらからcloneするようにしている。#行はコメント。

```.sh
$ cd ~/catkin_ws
$ git clone https://github.com/koki-ogura/ydlidar.git src/ydlidar
# prepare for use /dev/ttyUSB* as /dev/ydlidar
$ sudo sh ~/catkin_ws/src/ydlidar/startup/initenv.sh
# disconnect and connect usb cable
$ catkin_make
$ source devel/setup.zsh #if you use bash, setup.bash
# activate ydlidar
$ roslaunch ydlidar lidar.launch
# you can check LaserScan topic via rviz
$ rviz
# If you use ydlidar for your robot, you had better edit lidar.launch
```

#install gimp for view/rewrite map
```.sh
$ sudo apt install gimp
```

#how to save current map and view it
```.sh
$ rosrun map_server map_saver -f map_file_path_name
$ gimp map_file_path_name.pgm
```

#how to load saved map to cartographer
map_saverで保存したmapデータはcartographerで直接利用できない。
必要な確率的情報が欠如しているため。
下記のwrite_stateで保存したものが利用できる。

#how to save cartographer slam data
```.sh
#finish_trajectory
$ rosservice call /finish_trajectory 0
#save slam data to ~/.ros/
$ rosservice call /write_state test.pbstream
```

#how to load cartographer slam data
次のようにlaunchファイルのargsに/write_stateで~/.ros/に保存した.pbstreamファイルを記述して起動すればよい。

```cartographer_with_state.launch
<launch>
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.02" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find the_robot)/params
          -configuration_basename cartographer.lua
          -load_state_filename test.pbstream"
      output="screen">
  </node>
</launch>
```

#所感
- PlannerのControlFrequency(Hz)のボトルネックはtfによるmap->odomのbroadcastの更新時間にあるようだ。例えばamclを使っているのであれば、update_min_dやupdate_min_aやtransform_toleranceの値が小さいほど更新時間が小さくなるのでControlFrequencyが大きくなる。勿論、外界センサからのデータ(LaserScanやPointCloud2）の更新速度も速い方が良い。
PointCloud2をrealsenseから得る場合ではカメラのfps（5,15,30）が大きい方が良いのだが、これが大きいとCPUの処理時間も増えていくのでそれはそれで問題になる。
LiDARを利用していれば一周のデータ全体の取得は10Hz位なので外界データが得られていない時でもtfをbroadcastするようにしないとならない。
何れにせよPlannerのControlFrequencyを大きくしなければ高速動作での安全性を担保することが出来なくなるのでうまくバランスを取ることが大切だ。
- Cartographerのmap作成と自己位置同定にはLiDARのみを使う。
cost_mapのobstacle_layerにはLiDARとrealsenseの両方を使うのが良い。LiDARは長距離で精度も良いのだが一定の高さにある障害物しか検知できない。その欠点を短距離で精度が良く無いが上下左右広範囲の距離を同時に検知できるrealsenseで補うのだ。
global_costmapにはstatic_layerとobstacle_layerとinflation_layer。
local_costmapにはobstacle_layerとinflation_layerを使用する。
この組み合わせが良いという結論に達した。

#LinkLinkLink
- [ROS Discourse](https://discourse.ros.org)
    - 日本向け(下記)のもあるが、やっぱ本家の方が賑やかで良い。
    - 今はROS2の話題が多いみたい。
- [ROS Japan Users Group](https://discourse.ros.org/c/local/japan)
    - あまり活動していないようだ。
- [stackoverflow - ros](https://stackoverflow.com/questions/tagged/ros)
    - ROS関連の質問回答
- [ROSを用いた自律移動ロボットのシステム構築](https://www.slideshare.net/hara-y/ros-nav-rsj-seminar)
    - 基本的なことが良くまとめられていると感じる。