2010年4月27日火曜日

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

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

2010年4月26日月曜日

navigationスタック探訪その1(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,

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

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

2010年4月17日土曜日

ARToolKitのサンプル

simpleTestっていうバイナリです。

動画をとったのでupしておきます。



カメラはELECOM のUCAM-DLU130HSVですがUVC対応カメラならなんでもいけると思います。

結構テンションあがります。

動画の撮影は
gtk-recordmydesktop
動画のコンバートはffmpegでやりました。

カメラのキャリブレーション前ですが結構ちゃんとできますね。

ARToolKitをUbuntu 9.04にインストールする

突然ですがARToolKitをインストールします。

案外Windowsでやっている人が多くてLinuxの情報が少なかったです。



を参考にしたらできました。

1。ファイルをDL
ARToolKit-2.72.1.tgz

2。パッチをDL

3。パッチを当てる
$ patch -p0 -d . < artk-yuv422-v4l2-2.72.1.20090801.patch

4。ConfigureでVideo4Linux2を選択
5。Inlucdeパスを通す
5。make

でできます。

結構難しいですね。

patterns/pattHiro.pdf
を印刷して、

bin/simpleTest
でパターンを撮影すると立体が見えました!
やった。

2010年4月15日木曜日

Roombaのインタフェースを変える

前回見せたビデオでは内部的には
ルンバへの指令は
速度と回転半径で与えていました。

ROSは全方位移動台車を持つPR2のために、速度をTwistというメッセージ(geometry_msgsパッケージ)で与えるようになっていますので、
ルンバもこのインタフェースで動かせるようにしたいです。


そもそもなぜ、速度と回転半径にしたかというと、
Roombaは基本的に速度を

速度と回転半径

で与えるようになっています(DRIVEコマンド)。
なのでシンプルにそのまま使いました。


しかし、実はもう一つ操作方法があって、

左車輪速度と右車輪速度

というインタフェースのコマンド(DIRECT DRIVEコマンド)もあります。

今回は直接的にうごかすDIRECT DRIVEコマンドを利用して、
Twistメッセージを受け取ることができるようにしました。
といっても、
Twist.linear.xとTwist.angular.zしか使っていないですし、
うーん、挙動がちょっと間違っていますね。

今後自律移動プログラムを作っていくことになるので、
そっちが見通しついてからまた考えます。

2010年4月14日水曜日

rosdepの話

先日書いたroombaのプログラムはpySerialを使っています。
標準パッケージではないので、これを使うにはUbuntuだと

$ sudo apt-get install python-serial
としなければいけません。

これを自動化するのがrosdepでした。

ということでこれをmanifestに書きます。


  <rosdep name="pyserial"/>

の一行を加えました。


しかし、
これだけでは
$ rosdep install otl_roomba
はできません。

pyserialがOSごとにどのパッケージを示すのか記述しなければいけません。

これを書くのはstackの直下に置いたrosdep.yamlになります。
packageの下では読み込んでくれません。

なので、stackを作り、その直下に置く必要があります。


pyserial:
  ubuntu: python-serial

をrosdep.yamlに書きました。


これで無事
$ rosdep install otl_roomba
ができました。

2010年4月11日日曜日

Pythonコードをunittestでテストする

たまにはまじめにコードを書こうと思い、テストの勉強です。


ROSではPythonのテストはそのパッケージであるunittestを使ってテストするのが標準のようです。
C++はgtestというgoogleが作ったテスト環境を使っています。

pythonのunittestもよく分かっていないので、その勉強からです。

↓のへんを読みました。


ようするに
unittest.TestCaseクラスにメソッドを定義しておくと、それを順に呼んでくれる。
assert_, assertRaises, assertEqualsなどを使って結果が想定どおりかどうかを確かめる。
テスト前に呼ばれるsetUp(), テスト後に呼ばれるtearDown()というメソッドが使える。

という感じですかね。(他にも機能はいっぱいあります)

普通テストドライバで関数呼び出しをずら〜っと手書きすると思いますが、
その辺の自動化と結果のレポートをしてくれるようです。

さっそくルンバのインタフェースプログラムに使いました。

TestRoombaOpen, TestRoombaCommand
という二つのテストケースクラスを作ったとすると、
テストプログラムから
test_support.run_unittest(TestRoombaOpen, TestRoombaCommand)

として呼び出すと思いますが、
rostestの場合は、

import rostest
rostest.unitrun('test_roombaif', sys.argv[0], TestRoombaOpen)
rostest.unitrun('test_roombaif', sys.argv[0], TestRoombaCommand)

とするみたいです。

そして、
CMakeList.txtに
rosbuild_add_pyunit(test/test_roombaif.py)
を追加します。

この状態で
$ make test
するとテストが実行され、結果が
~/.ros/test_results/otl_roomba/test_roombaif.py.xml
として保存されます。

中身は↓みたいな感じです。
<testsuite errors="0" failures="0" name="unittest.TestSuite" tests="35" time="17.229">
  <testcase classname="__main__.TestRoombaCommand" name="test_ascii" time="0.6085"></testcase>
......
  <testcase classname="__main__.TestRoombaCommand" name="test_vel9" time="0.5080"></testcase>
  <system-out><![CDATA[speed limit over -501 < -500
rad limit over -2001 < -2000
speed limit over 501 > 500
rad limit over 2001 > 2000
speed limit over -501 < -500
rad limit over -2001 < -2000
]]></system-out>
  <system-err><![CDATA[]]></system-err>
</testsuite>

これは単体テスト(ROSノード無関係)用です。

いっぽうでROSノードレベルのテストをする場合は以下のようにして呼び出します。
rostest.rosrun(PKG, 'test_bare_bones', TestBareBones)

あとはrostestを使ったテストですがこれはまた次回にしましょう。

2010年4月10日土曜日

PS3コントローラでルンバを動かす(今回はROS使ったよ)

やっとRoombaとROSとがつながりました。
やったのは前回のi-SOBOTの時と同じでPS3コントローラとつないだだけ、なんですけど。

これでROSがRoombaにのりました
(正確にはPCから操作しているだけですが、そのPCもRoombaに乗っているのでよしとしましょう)。



ノードの関係は↓のようになっています。

roombaノードがルンバへの指令を受け付けます。
ps3_joyからの指令をroombaps3ノードが仲介して
roombaノードに渡しています。
roombaノードが実際にルンバを動かします。

万が一ソースを見たい人がいれば↓をどうぞ。

http://code.google.com/p/otl-ros-pkg/

Roomba動いた!

なんか、ルンバが動くんですが、挙動がおかしくて
なんか変だな、と思っていたら、
今は
Roomba OI (Roomba Open Interface)と名前が変わっていることに気づきました。

見る資料も間違っていました。
http://www.irobot.lv/uploaded_files/File/iRobot_Roomba_500_Open_Interface_Spec.pdf

通信速度が57600から115200に変更になっていますので注意が必要です。
(過去記事は修正しました)

なんと。。。ぐぐり力が足りないですね。反省。

ということでやっとルンバが動きました!

LEDや7セグメント(4桁)などが使え、音楽作曲もできますので、
ロボットとしての表現力もばっちりありますね。

とりあえず動いたので動画にしておきました。

回転時にLEDが光ったりします。
7セグメントLEDを使って「OTL」を表示していますが
分かりますかね?



構成:
Roomba 577 <==> FT232RL <==> Thinkpad X61 (Ubuntu) + i-SOBOT(意味なし)
プログラム環境:python (pyserial)

pyserialは
$ sudo apt-get install python-serial
としてインストールしました。
こういう低レベルな操作はCとかのほうが簡単ですね。
Pythonで書くの結構苦労しました。適材適所ですな。
逆に面白かったですけど。

2010年4月9日金曜日

Roomba OI (Open Interface) (旧SCI (Serial Control Interface))の勉強

やっと家にネットが開通しました!
そろそろ勉強を始めますか。
RO(omba)S(ystem)の勉強を!

前回紹介した公式コマンドPDFは間違っていました。
500シリーズ用のドキュメントがありましたので、こちらを読んでいきます。
http://www.irobot.lv/uploaded_files/File/iRobot_Roomba_500_Open_Interface_Spec.pdf

まず、通信スピードですが、ボーレート15520です。
設定すればもっと遅くできますが、今回はこのままにします。

ルンバには4つのSCIモードがあるようです。
off, passive, safe, full
の4つです。

充電中や電源がやばいときはoffになります。

スターとコマンドを受け取るとpassiveモードになります。
passiveモードでできるのは

  • センサデータの取得
  • 掃除の開始・終了
  • 歌の設定
  • 充電開始
だけです。
なのでロボットのように操作はできません。

で、コマンドを送るとsafeモードにうつります。
セーフモードでは以下の3つの安全機能が有効な状態で自由にルンバを操作できます。
  • 落下防止機能
  • 脱輪検知
  • 充電器合体による充電
これが発生すると自然にpassiveモードに移行してしまいます。

コマンドを送ればこれらの制限のないfullモードへ移行できます。

今回はsafeモードでいいかな。

コマンドは基本1バイト。データがあとにつづきます。

主なコマンド([]の中は指令の数字。)

Start [128] .... スタート指令。passiveモードへ遷移。最初にかならずこれをやる。
Safe[131] ...  safeモードに遷移
Full[132] ....  safe -> fullに遷移
Drive[137][vh][vl][rh][rl] ... 並進速度V[mm/s], 回転半径R[mm]で動く。
どちらも正にすると左前に進む。
-500 < V < 500
-2000 < R < 2000
32768 = 0x8000hのとき直進
-1で時計回り、1で反時計回りにその場回転。
V, Rはsignedの2バイト。

あとはLEDとか、センサとか、音楽とか楽しいのが残っていますが、
まあ、とりあえずこれくらいでためしてみますかねぇ。