costmap_2d¶

1. 概要¶

このパッケージは、空間のセンサーデータを取り込む2Dコストマップの実装を提供し、2Dまたは3Dの占有グリッドデータ（ボクセルベースの実装を使用するかどうかに依存）を構築します。そして、占有グリッドとユーザー定義のインフレーション半径でベースの2Dコストマップのコストをインフレーション（増大）します。 このパッケージは、map_serverベースのコストマップの初期化、ローリングウィンドウベースのコストマップ、およびパラメータベースのセンサートピックのサブスクリプションと設定もサポートしています。

• 管理状態：管理済み
• 管理者：David V. Lu !! <Davidvlu AT gmail DOT com>、Michael Ferguson <mfergs7 AT gmail DOT com>、Aaron Hoy <ahoy AT fetchrobotics DOT com>
• 著者：Eitan Marder-Eppstein、David V. Lu !!、Dave Hershberger、contradict @ gmail.com
• ライセンス：BSD
• ソース：git https://github.com/ros-planning/navigation.git （ブランチ：melodic-devel）

出典：http://wiki.ros.org/costmap_2d

注：上の図では、赤色のセルはコストマップ内の障害物を表し、青色のセルはロボットの内接円半径によってインフレーションした領域を表し、細い赤枠はロボットのfootprint(占有領域)を表します。ロボットが衝突を避けるためには、ロボットのfootprintが赤色のセルと交差せず、且つ、ロボットの中心点が青色のセルと交差してはいけません。

costmap_2dパッケージは、ロボットが占有グリッドの形式でナビゲートする場所に関する情報を保持した設定可能な構造体を提供します。コストマップは、静的マップのセンサーデータと情報を使用して、costmap_2d :: Costmap2DROSオブジェクトを介して、空間の障害物に関する情報を保存および更新します。 costmap_2d :: Costmap2DROSオブジェクトは、純粋に2次元のインタフェースをユーザーに提供します。つまり、障害物の判定は"列"単位でのみ行うことができます。

たとえば、XY平面で同じ位置にあるがZ位置が異なる"テーブル"と"靴"の場合、costmap_2d :: Costmap2DROSオブジェクトのコストマップの対応するセルのコスト値は同じになります(下図参照)。これは、平面空間での経路探索がしやすいように設計されています。

Hydroリリースの時点で、コストマップはレイヤー構造で、自由に作成することができます。レイヤーを操作する基本的な関数は、各レイヤー内に実装されます。たとえば、静的マップは1つのレイヤーであり、障害物は別のレイヤーです。デフォルトでは、障害物レイヤーは3次元で情報を保持します（ voxel_grid を参照）。 3D障害物データを保持することにより、レイヤーはマークとクリアをよりインテリジェントに処理できます。

主なインタフェースはcostmap_2d :: Costmap2DROSであり、ROS関連の機能の多くを持っています。各レイヤーを追跡するために使用されるcostmap_2d :: LayeredCostmapが含まれています。各レイヤーは、 pluginlib を使用してCostmap2DROSでインスタンス化され、LayeredCostmapに追加されます。レイヤー自体を個別にコンパイルして、C++インタフェースを介してコストマップに任意の変更を加えることができます。 costmap_2d :: Costmap2Dクラスは、2次元コストマップを保存およびアクセスするための基本的なデータ構造を実装します。

コストマップが占有グリッドを更新する方法についての詳細は、以下で説明します。

2. マークとクリア¶

コストマップは、ROSを介してセンサートピックを自動的にサブスクライブし、それに応じて更新されます。 各センサーは、マーク（障害物情報をコストマップに挿入）、クリア（障害物情報をコストマップから削除）、またはその両方に使用されます。 マーク操作は、セルのコストを変更するための配列への単なるインデックスです。 しかしながら、クリア操作は、レポートされる各観測について、センサーの原点から外側に向かってグリッドを通過するレイトレーシングで構成されます。 3次元構造を使用して障害物情報を保存する場合、各"列"からの障害物情報は、コストマップに配置されるときに2次元に投影されます。

3. 占有、空き、未知スペース¶

コストマップの各セルには255の異なるコスト値のいずれかを持つことができますが（ インフレーション のセクションを参照）、使用する基本構造は3つの状態のみを表すことができます。 具体的には、この構造の各セルは、空き、占有、未知スペースのいずれかです。 各ステータスには、コストマップへの投影時に割り当てられる、特別なコスト値があります。 一定数の占有セル（ mark_threshold パラメータを参照）がある"列"にはcostmap_2d :: LETHAL_OBSTACLEコストが割り当てられ、一定数の未知のセル（ unknown_threshold パラメータを参照）がある"列"にはcostmap_2d :: NO_INFORMATIONコストが割り当てられ、その他の"列"には costmap_2d :: FREE_SPACEコストが割り当てられます。

4. マップの更新¶

コストマップは、 update_frequency パラメータで指定された周期でマップ更新サイクルを実行します。 周期ごとに、センサーデータが入力され、マークとクリアの操作がコストマップの基本的な占有構造で実行されます。この構造は、 上記 のように適切なコスト値が割り当てられるコストマップに投影されます。 その後、障害物のインフレーションはcostmap_2d :: LETHAL_OBSTACLEコストを持ったそれぞれのセルで実行されます。 これは、占有されている各セルからユーザー定義のインフレーション半径までコスト値を外側に伝播することで構成されます。 このインフレーション処理の詳細を 以下 に示します。

5. tf¶

センサーソースからコストマップにデータを挿入するために、costmap_2d :: Costmap2DROSオブジェクトは tf を広範囲に使用します。 具体的には、 global_frame パラメータで指定された座標フレーム間の全ての変換、 robot_base_frame パラメータ、センサーソースが接続され、最新である想定で使用しています。 transform_tolerance パラメータは、これらの変換間で許容されるレイテンシーの最大量を設定します。 tf ツリーがこの予想されるレートで更新されない場合、 NavigationStack はロボットを停止します。

6. インフレーション¶

出典：http://wiki.ros.org/costmap_2d

インフレーションは、距離とともに減少する占有セルからコスト値を伝播するプロセスです。この目的のために、ロボットに関連するコストマップ値に5つの特定のシンボルを定義します。

• 「致命的」コスト(cost_lethal)とは、セルに実際の（ワークスペース）障害物があることを意味します。そのため、ロボットの中心がそのセルにある場合、ロボットは間違いなく衝突しています。
• 「内接」コスト(cost_inscribed)とは、セルと実際の障害物の距離が、ロボットの内接半径より小さいことを意味します。したがって、ロボットの中心が内接コスト以上のセル内にある場合、ロボットは確実に障害物と衝突しています。
• 「おそらく外接」コスト(cost_possibly_circumscribed)は「内接」に似ていますが、ロボットの外接半径を限界距離として扱います。したがって、ロボットの中心がこの値以上のセル内にある場合、障害物と衝突するかどうかはロボットの向きによって決まります。 「おそらく」という言葉を用いるのは、実際には、障害物ではなく、ユーザー設定値によりこのコスト値となっているセルの可能性があるためです。たとえば、ユーザーがロボットが建物の特定の領域を回避するように表現したい場合、障害物に関係なく、その領域のコストマップに独自のコスト値を挿入する場合があります。上記の図では値が128の例として使用されていますが、正確な値は このコード （計算式は cost_scaling_factor 参照）で定義されているように、ロボットの内接円半径と inflation_radiusパラメータ の両方の影響を受けることに注意してください。
• 「空き」コスト(freespace cost)はゼロであると想定されます。これは、ロボットがそこに行くのを妨げるものがないことを意味します。
• 「未知」コストは、特定のセルに関する情報がないことを意味します。コストマップのユーザーは、適切に解釈することができます。
• 他のすべてのコストには、「致命的」セルからの距離と、ユーザーが提供する減衰関数に応じて、「空き」と「おそらく外接」の間の値が割り当てられます。

これらの定義の背景にある理論的根拠は、正確なfootprintを気にするかどうかはプランナーの実装に任せていますが、それでも、オリエンテーションが実際に重要な状況でのみfootprintを追跡すれば済むような、十分な情報を提供することです。

7. マップタイプ¶

costmap_2d :: Costmap2DROSオブジェクトを初期化するには、主に2つの方法があります。 1つは、ユーザー生成の静的マップを元に初期化する方法です（マップの構築に関するドキュメントについては、map_server パッケージを参照してください）。この場合、静的マップによって提供される幅、高さ、障害物の情報に一致するように、コストマップが初期化されます。この構成は通常、 amcl などの自己位置推定システムと組み合わせて使用​​され、ロボットがマップフレームに障害物を登録し、環境を通過するときにセンサーデータからコストマップを更新できるようにします。このタイプは、move_baseの基本的な構成においては、グローバルコストマップに使用されます。

costmap_2d :: Costmap2DROSオブジェクトを初期化する2番目の方法は、オブジェクトに幅と高さを与え、 rolling_window パラメータをtrueに設定することです。 rolling_window パラメータは、ロボットが空間を移動するときにロボットをコストマップの中心に保ち、ロボットが移動して特定の領域から離れすぎた障害物情報を地図から削除します。このタイプの構成は、ロボットがローカルエリア内の障害物のみを対象とする走行距離座標フレームで最もよく使用されます。このタイプは、move_baseの基本的な構成においては、ローカルコストマップに使用されます。

8. コンポーネントAPI¶

1 #include <tf/transform_listener.h>
2 #include <costmap_2d/costmap_2d_ros.h>
3
4 ...
5
6 tf::TransformListener tf(ros::Duration(10));
7 costmap_2d::Costmap2DROS costmap("my_costmap", tf);

8.2. レイヤー仕様¶

コストマップは、レイヤードコストマップという仕組みで、下の階層からプラグインを順に呼び出す形で、コストマップにコスト値を埋めていきます。Navigationスタックには、下記の3つのレイヤーがあります。

• Static Layer 地図情報を使って、コスト値254のセルを埋めていくレイヤー。Globalコストマップのみ使用します。
• Obstacle Layer 測距センサで観測した障害物情報を使って、障害物が存在するセルにコスト値254を埋めます。また、障害物がなくなっているセルについては、コスト値0でクリアします。
• Inflation Layer コスト値254のセルから、ロボット内接円半径内をコスト値253で埋めます。さらに、inflation_radius等の値を使って、コストを塗り広げます。

呼び出すコストマップレイヤーは、パラメータで任意に指定可能です。独自に開発したコストマップレイヤーを間に挟むこともできます。

8.2.1. 静的マップレイヤー(Static Layer)¶

静的マップレイヤー は、SLAMによって生成されたような、コストマップのほとんど変化しない部分を表します。

8.2.2. 障害物マップレイヤー(Obstacle Layer)¶

障害物マップレイヤー は、センサーデータによって読み取られた障害物を追跡します。 ObstacleCostmapPluginは障害物を2次元でマークおよびレイトレースしますが、 VoxelCostmapPlugin は3次元で障害物をマークします。

8.2.3. インフレーションレイヤー(Inflation Layer)¶

インフレーションレイヤー は、コストマップがロボットの構成スペースを表すようにするため、致命的な障害物の周りに新しい値を追加（つまり、障害物をインフレーションする）最適化処理です。

8.2.4. その他のレイヤー¶

pluginlib を介して、コストマップで他のレイヤーを実装および使用できます。 追加のプラグインは、以下にリストされたリンクを指示します。

• Social Costmap Layer
• Range Sensor Layer