自律移動ルンバ＋α完成！

ようやくmove_baseのパラメータ調整（主にbase_local_planner)が終わりました。

で、完成したビデオが↓です。


全く関係ないですが、白くまをフィーチャーしてみました。
我が家のアイドルです。

base_local_planner用の設定ファイルは以下です。

TrajectoryPlannerROS:
max_vel_x: 0.15
min_vel_x: 0.01
max_rotational_vel: 0.4
min_in_place_rotational_vel: 0.4
acc_limit_th: 0.5
acc_limit_x: 0.5
acc_limit_y: 0.0
holonomic_robot: false
path_distance_bias: 0.6
goal_distance_bias: 0.6
heading_scoring: true
occdist_scale: 0.01

経路と大きくずれるとまずいので、経路になるべく追従するように
path_distance_biasを大きくして、heading_scoringをtrueにしてみました。

ちょっと自己位置が脆弱なので、どうにかしたいですね。
でもとりあえずROSでやりたかったことの目標達成できたのでうれしいです。

あとはゆるりとソースでもよみますかねぇ。

AMCL調整成功？

ようやくAMCLが使える状態になってきました。

一番大きな変更点はマップの解像度を50mm/grid -> 15mm/gridに変更したことです。
あとはパラメータは前回紹介したブライアンさんのメーリスへの投稿のものを
そのまま使いました。調整しようとしましたがかなり難しいです。

ちょっとずれますが、なんとか修正できています。
（あと、マップ作成直後なのでマップとの乖離が少ないというのもあるかもしれません。
もしそうなら、次回やるときにはまたずれてるかも。。。。）

マップ作成時のパラメータは以下のような感じにしました。


$ rosrun gmapping slam_gmapping _delta:=0.015 _xmin:=-10 _ymin:=-10 _xmax:=10 _ymax:=10 _lstep:=0.01 _astep:=0.01 _stt:=0.3 _particles:=500

amclのパラメータは以下です。


<launch>
<node pkg="amcl" type="amcl" name="amcl">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="diff"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.8"/>
  <param name="odom_alpha2" value="0.8"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>

  <param name="laser_lambda_short" value="0.1"/>

  <param name="laser_lambda_short" value="0.1"/>

  <param name="laser_model_type" value="likelihood_field"/>

  <!-- <param name="laser_model_type" value="beam"/> -->

  <param name="laser_likelihood_max_dist" value="2.0"/>

  <param name="update_min_d" value="0.2"/>

  <param name="update_min_a" value="0.2"/>

  <param name="odom_frame_id" value="odom"/>

  <param name="resample_interval" value="1"/>

  <param name="transform_tolerance" value="0.1"/>

  <param name="recovery_alpha_slow" value="0.0"/>

  <param name="recovery_alpha_fast" value="0.0"/>

</node>

</launch>

なんとか使えそうなので、次回これにプランニングのほうも足してみます。
はぁ、むつかしかった。
パラメータの調整はやはり職人芸が必要ですね。

今回はマップのグリッドサイズでしたが。
ロボットのサイズによってグリッドサイズは大きく変える必要がありますね。

amclの調整がうまくいかない。

うーん、はまってます。

自己位置推定のパッケージであるamclの使い方がよく分かりません。

回転方向のずれが大きいので、update_min_aを小さくして、
resample_intervalも小さくして、
initial_cov_aaも大きめにしていますが、
全然だめです。


パーティクルがだんだんまとまってきちゃいます。
neff値を見てリサンプルしてるんだと思うんですが、
なかなかリサンプルしてくれません。


ううむ。。。

URG(UTM-30LX)を試す

やはり現在の技術だと自律移動にはレーザーは欠かせませんね。

マップ作成も自動でできますからね。
ということで北陽のURGを入手しました。

http://www.hokuyo-aut.co.jp/02sensor/07scanner/download/urg_programs/UrgViewer__page.html

まずはLinuxで動作確認です。

12VをURGに加えて、USB線をPCにさします。

$ dmesg


すると
[  983.876967] cdc_acm 4-1:1.0: ttyACM0: USB ACM device

と、/dev/ttyACM0にちゃんとデバイスが見えました。

そして一応ドライバ（サンプルライブラリ）を入手して、configure, make, make installします。

http://www.hokuyo-aut.co.jp/02sensor/07scanner/download/urg_programs/urg-0.8.12.zip

そして動作確認は以下のようにやりました。

$ sudo apt-get install qt4-dev-tools

$ svn co http://svn.sourceforge.jp/svnroot/qrobosdk/trunk 

$ cd trunk/programs/

$ make

$ cd UrgViewer

$ ./UrgViewer

ちゃんとデータが取れています。

よしよし。

これでROSのnavigation stackをがっつりそのまま使う準備が整ったんじゃないかと思います。

（オドメトリの取得がまだかな？プレーリードックから持ってくれば終わりでしょうか？）

$ rosmake hokuyo_node

$ rosdep install rviz hokuyo_node

$ rosmake rviz hokuyo_node

$ roslaunch hokuyo_node hokuyo_test_intensity.launch

するとrviz上でレーザー点が見えました。

navigationスタック探訪その2（move_base)

ようやくnavigation stackの全容が分かってきました。

まず、pluginlibパッケージをご存知ですか？
common stackの中にあるのでこれは必須科目でしょう。

知らなければまずそれを勉強してください。
はい、僕は知りませんでした。

次にmove_baseパッケージを見ます。
これがPR2のnavigationの実体です。

しかし、ここでは部品を実体化しているにすぎません。
中身はプラグインになっているのでいれかえが可能です。
それらはパラメータでプラグイン名を指定します。

設定は以下のようになっています。


~base_global_planner (string, default: "navfn/NavfnROS" For 1.1+ series)
~base_local_planner (string, default: "base_local_planner/TrajectoryPlannerROS" For 1.1+ series)
~recovery_behaviors (list, default: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] For 1.1+ series)

recovery_behaviorsはちょっと理解できていないので後で見るとして、

global_plannerはnavfnパッケージ、local_plannerはbase_local_plannerパッケージに

定義があることが分かります。

それらはnav_coreで定義されるインタフェースを満たすプラグインになっている。

ということになります。

いやー、複雑ですね。

つまり、pluginlib、navfn、base_local_planner、*_recovery、move_baseという感じで見ていくのが良さそうです。

あとはamcl(自己位置推定)ですね。こっちは独立してるようです。

自律移動(navigation)は形をROSのメッセージで規程するのではなく、nav_coreで決めた形にプラグインを作ってもらい、それを集めて実体化する、というやり方になっています。

全然ROSっぽくないです。

これでいいんでしょうか？

形を規程できるからこの構成がいい？

それとも結合度が強すぎてよくない？

それとも地図を同一プロセスで持ちたいとか（パフォーマンス）の問題？？

Player関係の構成をひきずっているんじゃないかと思うんですが、

Player詳しい人教えてください。

nav_view_cuを使う

マップをプログラム的に見てみたい。

そんなときはnav_viewというソフトを使います。
ただし、これはorgeを使うためにグラフィックカードがないと動きません。

しかし、orgeなしで動くバージョンがありました。
以下のようにすると取ってこれます。

$ svn checkout http://prairiedog.googlecode.com/svn/trunk/nav_view_cu nav_view_cu-read-only

ちょっと不安定ですね。
スクリーンショットもとれない。。。

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,

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

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