navigationスタック探訪その１（map_server)

ちょっと暇を見てすこしずつnavigation stackの中を見ていきたいと思います。

navigationは自己位置推定、経路計画、障害物回避、くらいが主なコンテンツだと思いますが、どこから見ていくかセンスが問われますね。

それらはすべてマップをベースとして成り立っているので、
まずはマップサーバから見ることにします。

マップサーバ
http://www.ros.org/wiki/map_server

マップはYAMLファイルの設定と、マップ本体の画像ファイルのペアで管理されます。

画像ファイルはグレーが濃いほど障害物として扱われます。
そのスレッショルドはYAMLファイルで決定します。
画像ファイルはカラーでもグレーでもどっちでもいいみたいです。

ROSメッセージとしてはoccupancyは0~100の数値（0がフリーで100が完全に壁)で表されます。また、未観測領域は-1になります。

画像ファイルのフォーマットはSDLで読み込めるものならなんでもいいです。

YAMLファイルは以下のようなかんじです。


image: testmap.png
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0

• image: イメージファイル（YAMLからの相対・もしくは絶対パス）
• resolution: [meters/pixel]
• origin: マップの左下端の座標(x,y,θ)
• occupied_thresh: この数値より大きいところを障害物とみなす
• free_thresh: この数値より小さいところをフリースペースとみなす
• negate: 白黒反転させるかどうか

マップサーバノード

$ rosrun map_server map_server hoge_map.yaml

として起動します。

このノードは起動時にファイルを読み込み、マップをpublishします。

ただし、latched topic（Subscribeされたときに一度だけ値を書き込む）です。

サービスでも取得できますが、サービスはなくなる予定のようです。

ROS APIの仕様は以下です。

• Published Topics
• map_metadata (nav_msgs/MapMetaData)
• latched topicでYAMLファイルにあたるデータ（メタデータ）を送ります
• map (nav_msgs/OccupancyGrid)
• latched topicで画像ファイルにあたるデータを送ります（メタデータも含みます）
• Services
• static_map (nav_msgs/GetMap)
• サービス版です
• Parameters
• ~frame_id (string, default: "map")
• The frame to set in the header of the published map.

また、SLAM結果を保存するmap_saverというノードもあります。

$ rosrun map_server map_saver -f hoge_map

のようにして保存します。保存形式はpgmになります。(hoge_map.pgm, hoge_map.yaml)

ということでマップサーバーでした。

たいしたことやっていませんね。

試しに適当に自宅のマップを作って読み込ませてみました。

image: OTLHouse.png

resolution: 0.1

origin: [0.0, 0.0, 0.0]

occupied_thresh: 0.65

free_thresh: 0.196

negate: 0

$ rosrun map_server map_server OTLHouse.yaml

[ INFO] 1272284808.416589000: Loading map from image "./OTLHouse.png"

[ INFO] 1272284808.423908000: Read a 200 X 200 map @ 0.100 m/cell

$ rostopic list

/clock

/map

/map_metadata

/rosout

/rosout_agg

/time

となり、ちゃんとpublishされています。

ためしに見てみると

$ rostopic echo /map_metadata

---

header: 

  seq: 1

  stamp: 1272284808423832000

  frame_id: map

info: 

  map_load_time: 1272284808423828000

  resolution: 0.10000000149

  width: 200

  height: 200

  origin: 

    position: 

      x: 0.0

      y: 0.0

      z: 0.0

    orientation: 

      x: 0.0

      y: 0.0

      z: 0.0

      w: 1.0

data: (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

という感じでちゃんとマップになっています。

次回はこれをつかってパスプランをさせてみます。