ROS Navigation Stack に関するもろもろ

気がついたらもう12月17日。明日の Advent Calendar の記事が一行も書けていない。。。あまり時間がないのですが、Navigation Stack に関するもろもろを120分一本勝負で書き留めます。

ROS の Navigation Stack とは

ROS が提供する、経路計画やコストマップ生成のためのパッケージです。ROS Wiki が navigation にあり、ソースコードは rosplanning/navigation にまとまっています。

Navigation Stack の構成

move_base を中心に、主に4つの機能から構成されています。

• Global Costmap : 大域的なコストマップ
• Global Planner : 大域的な経路計画 (のための Planner)
• Local Costmap : ロボット周辺の局所的なコストマップ
• Local Planner : 局所的な経路計画 (のための Planner)

自動車運転の言葉で簡単に説明すると、Google Map のような、目的地へ行くための大域的な地図が Global Costmap で、目的地までの経路案内をする機能が Global Planner、道の途中の障害物を避けたり、駐車スペースのような自分の周辺範囲を把握するための地図が Local Costmap で、そういう自分周辺の状況を把握しながら、人やものにぶつからないように経路計画する機能が Local Planner です。

なお、Global と Local は、利用目的こそ違うものの、Costmap と Planner が対になっているところとか、構成としては似ていて、共通の基盤として Costmap を共有しています。Navigation Stack では、 costmap_2d として提供されています。

コストマップって何？

一般的な意味で言うと、空間中の通れそうな領域、障害物のある領域を数値で表現した地図ですが、ここではもう少し意味を限定して、2次元の専有格子地図をコストマップと呼ぶことにします。

2次元の専有格子地図とは、ロボットが移動する2次元空間(高さ方向は一旦無視)をグリッド(10cm x 10cm とか)で区切って、各グリッドごとの「障害物っぽさ」を数値として表したものです。実際の空間は連続的ですが、計算を簡単にするために、あえてグリッドに離散化しています。たとえば 5m x 5m の空間を 10cm x 10cm のグリッドで専有格子地図にすると、50 x 50 のグリッドになります。そのグリッド1つ1つに対して、障害物っぽさの数値を割り当てたのが専有格子地図です。

ややこしいのですが、ROS Navigatio Stack では専有格子地図に対して2つの表現方法があります。

• OccupancyGrid : ROS Message としてやり取りするための型。各グリッドの値は -1 (コストが未知) または 0〜100 の数値をとる。数値は、そのグリッドの「障害物っぽさ」を表す
• Costmap2D : 具体的には こいつ の char_map_ 属性。C++ で扱いやすいように、グリッドの値を 0〜255 で表現して、 unsigned char の配列にしている。

通常の用途では OccupancyGrid だけ気にしていれば良いですが、少し突っ込んで理解しようとすると、この区別がわかっていないと混乱してくるので参考にしてください。

Costmap2D と OccupancyGrid の関係は、このあたり を読むと分かりやすいです。

    cost_translation_table_ = new char[256];

    // special values:
    cost_translation_table_[0] = 0;  // NO obstacle
    cost_translation_table_[253] = 99;  // INSCRIBED obstacle
    cost_translation_table_[254] = 100;  // LETHAL obstacle
    cost_translation_table_[255] = -1;  // UNKNOWN

    // regular cost values scale the range 1 to 252 (inclusive) to fit
    // into 1 to 98 (inclusive).
    for (int i = 1; i < 253; i++)
    {
      cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
    }

Costmap2D (0〜255) と OccupancyGrid (-1〜100) はこんな感じでマッピングされています。 ROS Answers にも関連するトピックがあるので参考にしてみてください。(OccupancyGrid vs. Costmap)

このあたりの仕組みは、 Global Costmap も Local Costmap も同じです。 SLAM ノードが配信する地図も同じです。ROS で扱う地図は、基本的に OccupancyGrid の書式でトピックに配信されています。

座標系の話

前項では、コストマップという Global と Local の共通点について述べましたが、座標系については Global と Local で異なります。よくある構成として、それぞれ以下の座標系を利用します。

• Global : map 座標系
• Local : odometry 座標系

ロボット初心者だった私は、これらの座標系の考え方に慣れておらず、はじめは戸惑いました。でも、言っていることは単純です。簡単に説明してみます。

Odometry 座標系とは、時間の連続性・空間の連続性に立脚した、連続的な座標系です。ロボットが 10m 進んだ(と思った)ら Odometry 座標系上で 10m 前進し、90度回転した(と思った)ら、 Ocometry 座標系上でも 90 度回転します。なぜカッコ書きで「思ったら」と書いたかというと、ロボット自身の「動いているつもり」は、必ずしも現実を反映しているとは限らず、常に誤差が蓄積されていくからです。その誤差を許容する代わりに、時間・空間的な連続性を担保して、今いる位置を表現したのが Odometry 座標系です。誤差が蓄積されるとは言っても、短い時間、狭い空間の中での利用には支障がなく、 Local Costmap や Local Planner との相性が良いです。なので、 Local Costmap, Local Planner は Odometry 座標系を利用します。

それに対して Map 座標系は、空間内での絶対的な位置を表現します。先ほど Google Map を例に挙げましたが、Google Map が表現しているのも、空間内での道路や建物の絶対的な位置です。これだけを聞くと、「Map 座標系があれば Odometry 座標系はいらないのではないか」と思うかもしれませんが、 Map 座標系にも欠点があります。それは 時間・空間的な非連続が発生する ということです。絶対位置を表現するということは、観測の仮定で誤差が見つかったときに地図が修正されます。そうすると、「いま、 (10, 100) にいるつもりだったけど、あ、違った、 (15, 120) だった」ということが発生するわけですね。このとき、自己位置や、観測している障害物の位置などが、非連続的にぽんっと、移動してしまいます。なので、局所的な移動には map 座標系は不向きなため、 Local では Odometry 座標系を利用するわけです。一方で、大域的な移動には絶対位置が便利なため、 Global Costmap や Global Planner には map 座標系を利用します。

座標系の詳細は REP 103 や REP 105 を参照して下さい

Costmap や Planner 以外のもの

ROS の navigation リポジトリを見ると、18 個ものパッケージから構成されています。それらを整理しつつ、上記の costmap や planner 以外のパッケージについても、簡単に説明しておきます。

なお、結構重要なはずなのに、ここにあげているパッケージの開発はあまり活発で無いように思います。

move_base

move_base は ROS Navigation Stack の全体 (SLAM 以外) を束ねる機能を提供します。後述する plugin system を用いて、 Costmap や Planner や Recovery を順番に呼び出して、全体を動かします。なお、 plugin system を利用している関係上、 ROS Node は move_base のノードだけで、 Global/Local Costmap や Global/Local Planner はその下で動作します。

• move_base
• move_base_msgs : move_base で利用する ROS Message の定義

Global Costmap や Local Costmap

• costmap_2d の中で plugin として提供されています。
  • InflationLayer
  • ObstacleLayer
  • StaticLayer
  • VoxelLayer

これ以外でも range_sensor_layer などの plugin が公開されています。plugin システムについては後述します。

Global Planner

rosplanning/navigation リポジトリでは、以下の Global Planner が提供されています。

• global_planner
• navfn
• carrot_planner : 障害物にぶつかるまで、ゴールへ直進する単純な Planner

global_planner や NavFn は A* や Dijkstra で経路計画するのですが、詳しいことはドキュメントを参照してください。

Local Planner

rosplanning/navigation リポジトリでは、以下の Local Planner が提供されています。

• dwa_local_planner
• base_local_planner

これ以外でも teb_local_planner とかがよく使われているようです。

Recovery 機能

動作がスタックしたときに発動されるのが Recovery 機能です。まだ使いこなせていないため、深入りはしません。

• move_slow_and_clear : 障害物の近くでの速度を制限して移動し、移動できた場所の周りの障害物を地図から除去します
• clear_costmap_recovery : 自分の周り半径 X m 以内の local costmap をクリアします
• rotate_recovery : その場で360度回転することで、 local costmap を更新します

その他

• amcl : Particle Filter を用いた SLAM Package です。これ以外でも、利用するセンサや目的に応じて、様々な SLAM パッケージが提供されているので、どの SLAM アルゴリズム・パッケージを利用するかは、用途に応じて検討して下さい。(これだけで一大分野)
• nav_core : global planner や local planner, recovery の仕様を提供するパッケージです。
• map_server : 画像で作った静的な地図を配信したり、地図を保存したりする、開発支援系の機能を提供しています。
• fake_localization : 使ったことはないのですが、 odometry を入力としてダミーの Robot 姿勢を出力するパッケージのようです。シミュレーション用。
• robot_pose_ekf : IMU などの複数のソースを複合して、精度の高い Odometry 座標を算出するためのパッケージです。似たようなパッケージとして robot_localization もあります。
• voxel_grid costmap_2d で利用されている voxel 実装です

move_base と SLAM の関係

先ほども書きましたが、 Global/Local Costmap, Global/Local Planner, Recovery は move_base ノードの下で plugin として動作します。 move_base は、グローバルな地図 (/map に配信される OccupancyGrid) やロボットの自己位置 (tf で map 座標系と base_footprint の間の座標変換として提供される) を入力とし、さらにセンサ情報を使って障害物位置を Costmap に反映しながら、コストマップや経路計画、リカバリの機能を提供します。

一方で、 SLAM は、センサ入力を使って地図を作成 (/map へ配信されるもの) しながら自己位置 (tf) を配信します。

SLAM から得られた地図や自己位置を move_base で利用する、というように、これらの2つは緩やかにつながっています。ロボットをスムーズに動かすのに、どちらも大切です。

Navigation Stack を理解するための近道

個人的な意見ですが、 Costmap の概念や Plugin System の概要を理解するには、「Costmap の plugin を1つ作ってみる」のが近道ではないかと思います。私も 4 つくらいのプラグインを作ってみました。

プラグインの実装には、 rosplanning/navigation の costmap_2d で定義されている plugin 実装や、 DLu/navigation_layers の実装が参考になります。後者のパッケージは、 ROS Navigation Stack の実装メンバーの一人と思われる David V. Lu の作成したプラグインです。ソースコードの量もそれほど多くないので、学びやすいです。

ROS Navigation Stack の plugin システム

何度か言及してきた plugin システムですが、 ROS Navigation Stack は、 move_base の下で複数の layer を動かすために pluginlib を利用しています。細かい中身は別として、使い方だけ整理しておきます。

plugin の提供

costmap_2d や DLu/navigation_layers での実装が参考になります。

plugin を実装して、 costmap_plugins.xml を定義しましょう。

<library path="lib/librange_sensor_layer">
  <class type="range_sensor_layer::RangeSensorLayer" base_class_type="costmap_2d::Layer">
    <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
  </class>
</library>

その他、ソースコードの実装と package.xml や CMakeLists.txt の修正が必要ですが、詳細はドキュメントや上記のソースコードを参考にしてください。(雑ですみません)

忘れがちですが、ソースコードに下記のような宣言を忘れずに。(これが無くてもコンパイルは出来てしまうので忘れがち)

PLUGINLIB_EXPORT_CLASS(range_sensor_layer::RangeSensorLayer, costmap_2d::Layer)

costmap_2d の plugin として認識されているか否かは、下記のコマンドで確認できます。

$ rospack plugins --attrib=plugin costmap_2d
rtabmap_ros /opt/ros/kinetic/share/rtabmap_ros/costmap_plugins.xml
range_sensor_layer /opt/ros/kinetic/share/range_sensor_layer/costmap_plugins.xml
costmap_2d /opt/ros/kinetic/share/costmap_2d/costmap_plugins.xml

plugin の利用

plugin の利用を意識することはあまりないと思いますが、move_base のソースコードを見ると、

pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;

こんな感じの ClassLoader やらを使って、

planner_ = bgp_loader_.createInstance(global_planner);

のようにインスタンスを生成できるようです。 global_planner には global_planner/GlobalPlanner のような文字列が入ります。

actionlib

だんだんと記述が雑になってきました ...

move_base での経路計画には actionlib が利用されます。actionlib とは、ROS Topic の上にのっかった非同期処理のための仕組みです。

クライアントの立場から話をすると、 SimpleActionClient を介して callback とともに goal を設定すると(裏では ROS Topic が利用されている)、要求の開始時(active_cb)や完了時(done_cb)や途中経過(feedback_cb)を callback として受け取ることが出来ます。

経路計画の呼び出し方には複数の方法があって複雑ですが、この機能を理解しておくと便利です。詳細は actionlib/DetailedDescription が詳しいです。サーバ側とクライアント側の状態マシンを理解しておくのが肝だと思います。

なお actionlib とは別に、 /move_base_simple/goal に対して geometry_msgs::PoseStamped を配信することで経路計画の目的地を設定することもできます。 rviz 上での目的地設定に利用されている機能です。

stage

まだ勉強途中ですが、2次元の経路計画には stage が便利なようです。 teb_local_planner_tutorials に利用例があるので、参考にしてみてください。

参考情報

• ROS Navigation Stack のページ
• github での検索
  • こんな 感じで検索してみると楽しいです。みなさん色んな planner を実装しています。
• この辺りのソースコードを読むと楽しいです。
  • rosplanning/navigation
  • DLu/navigation_layers
• 書いてから気づいたのですが MoriKen さんが Qiita ですげー詳しく解説してますね。後ほど読んでみようと思います。この2時間は何だったのか〜〜。
  • Navigation Stack を理解する - 1. 導入
  • Navigation Stack を理解する - 2.1 move_base: ROSで遊んでみる
  • Navigation Stack を理解する - 2.2 move_base: ソフトウェア構成をみる
  • Navigation Stack を理解する - 3.1 amcl: ROSで遊んでみる
  • Navigation Stack を理解する - 3.2 amcl: ソフトウェア構成をみる
  • Navigation Stack を理解する - 3.3 amcl(移動ロボットの自己位置推定): 原理をみる (準備編)
  • Navigation Stack を理解する - 3.4 amcl(移動ロボットの自己位置推定): 原理をみる (応用編)
  • Navigation Stack を理解する - 4.1 gmapping: ROSで遊んでみる
  • Navigation Stack を理解する - 4.2 gmapping: ソフトウェア構成をみる
  • Navigation Stack を理解する - 4.3 gmapping(格子ベースFast SLAM): 原理をみる(応用編)
  • move_base でRecovery 行動に遷移する条件を調べてみた

まとめ

駆け足で、自分がいま理解している内容を書き下しました。お気づきの点や、より深く知りたいことなどありましたら、コメントください！