ROSを使用したCRANE+の動かし方 その11(完結)

こんにちは!長谷川です。
前回はCRANE+の状態をモデルに反映させる方法について説明しましたが、今回は、CRANE+の状態をrviz上でモニタリングしつつ、CRANE+を自動で動かす方法について説明したいと思います。

使用するプログラム

実は、今回やろうとしていることは、すでに勉強したことの組み合わせで実現できます。
CRANE+を自動で動かすプログラムは第6回目の記事で作成済みですし、CRANE+の状態をrvizに反映させるプログラムも第10回目の記事で作成済みです。それぞれのプログラムで立ち上げられるノード名、関係するトピックは異なっているので、これら2つのプログラムを同時に起動できれば、目的を達成できることになります。

launchファイルの利便性

今まで勉強してきたことを組み合わせると、今回は以下のような手続きをすればいいことになります。

1: CRANE+とPCを接続
2: 「sudo chmod 6666 /dev/ttyUSB0」と端末に入力
3: 「roslaunch my_dynamixel_tutorial controller_manager.launch」と入力
4: 新しい端末を開き、「roslaunch roslaunch my_dynamixel_tutorial start_tilt_controller.launch」と入力
5: crane_urdf/srcに移動し、「roslaunch crane_urdf display.launch model:=crane.urdf」と入力
6: 新しい端末を開き、「rosrun crane_urdf reflect_crane_in_rviz」と入力
7: 新しい端末を開き、「rosrun turtlebot_arm turtlebot_arm」と入力

・・・面倒ですね。この面倒な作業を軽減するため、新たなlaunchファイルを作ることにします。これにより、少なくとも5〜7までの手順をコマンド1回で行うことができるようになります。プログラムの方に多少手を加えれば、3〜7までの作業をコマンド1回でできるようになります。

ROSのlaunchファイルについて

前回、display.launchというlaunchファイルを作りましたが、この時にはlaunchファイルについて詳しく説明しませんでした。今回は、display.launchについても詳しく説明しつつ、新しいlaunchファイルを作っていこうと思います。
launchファイルとは、様々なプログラムを組み合わせ、複数のノードを同時に立ち上げることができるファイルです。「roslaunch」というコマンドは、このlaunchファイルを起動するためのものだった、というわけです。このファイルに書いておけば、立ち上げたい全てのノードを1回のコマンドで立ち上げることができます。いちいち「rosrun」する必要がなくなるのです。しかもこのlaunchファイル、内部で他のlaunchファイルも実行することができます。また、それぞれのプログラムに対するパラメータも、ファイル内で決めておくことができるのです。このような機能により、操作がとても楽になります。それだけでなく、プログラムを別個の処理ごとに分けることが容易になるので、モジュール化と再利用性の向上を図ることができるのです。今回の場合では、CRANE+を動かすプログラムとCRANE+の状態をrvizに反映させるプログラムは独立に動いているので、CRANE+の動かし方を変えたいときは、CRANE+を動かすプログラムのみ変更すればいいという事になります。

新しいlaunchファイルの作成

例のごとく解説は後回しにして、まずはlaunchファイルを作ってしまいましょう。
端末に、
[php]
$ roscd crane_urdf/src
$ vi auto_crane_rviz.launch
[/php]
と入力します。以下を貼り付けます。
[xml]
<launch>
<param name="robot_description" textfile="$(find crane_urdf)/src/crane.urdf" />
<param name="use_gui" value="False"/>
<rosparam param="source_list">["joint_states_source"]</rosparam>

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
<node name="reflect_crane_in_rviz" pkg="crane_urdf" type="reflect_crane_in_rviz" />
<node name="turtlebot_arm" pkg="turtlebot_arm" type="turtlebot_arm" />
</launch>
[/xml]
貼り付けたら、保存してviを終了してください。これで、新しいlaunchファイルが完成です。

実行

早速、新しいlaunchファイルを実行してみましょう。
第5回目の記事にしたがって、CRANE+とPCを接続し、「roslaunch my_dynamixel_tutorial start_tilt_controller.launch」まで入力してください。これで、1〜4までの手続きが終わりました。その後、端末に、
[php]roslaunch crane_urdf auto_crane_rviz.launch[/php]
と入力します。これで、CRANE+も動き始め、rvizも起動すると思います。天板上で動くので、天板にPC等を置くのは危険です。

launchファイルの解説

auto_crane_rviz.launchを見ながら、launchファイルの書き方について学びましょう。
[xml firstline=”1″]<launch>[/xml]
[xml firstline=”11″]</launch>[/xml]
この2つで囲まれた部分が、launchファイルとして認識されます。
[xml firstline=”2″]<param name="robot_description" textfile="$(find crane_urdf)/src/crane.urdf" />[/xml]
ここでは、robot_descriptionというパラメータに、crane_urdfパッケージのsrcディレクトリ内にあるcrane.urdfファイルを設定します。パラメータというのは、変数のことです。パラメータサーバというパラメータを共有できる仕組みがあって、ノードがそこで定義されたパラメータを読み込んで動作に反映させています。このパラメータをノードの外から変更することで、ノードの設定を変えることができます。
こちらこちらを読むと、robot_descriptionというのは、joint_state_publisherとrobot_state_publisherにおいて、ロボットのURDFデータを格納するパラメータのようです。ここに、第8回目の記事で作成したcrane.urdfを設定することで、モデルを読み込ませます。
[xml firstline=”3″]<param name="use_gui" value="False"/>[/xml]
先ほど紹介したページを読むと、use_guiというのは、joint_state_publisherにおいて、各関節の値をスライドバーで変更できるウィンドウを出すかどうかを決めるパラメータのようです。ここにFalseという値を設定することで、ウィンドウを出さない設定にします。なお、前の行ではパラメータ名を指定した後、「textfile=」と書いていましたが、これはファイルの中身をパラメータに設定するためのコマンドです。この行では、値を直接設定するため、「value=」と書いています。
[xml firstline=”4″]<rosparam param="source_list">["joint_states_source"]</rosparam>[/xml]
rosparamタグは、先ほど使ったparamタグに似ています。paramは主にひとつずつパラメータを設定しますが、rosparamはyamlファイルという、パラメータをまとめてあるファイルを読み込んで、複数のパラメータを一度に設定することが可能です。この行では、yamlファイルを読み込んでいるわけではなく、source_listという一つのパラメータしか設定していません。ですが、source_listには文字列のリスト型を設定しなくてはならず、paramタグではリスト型を設定できるのかよくわからないため、rosparamタグを使用しました。なおリスト型というのは、例えば[3, 4]のように値を並べたものです。source_listパラメータの働きについては、前回説明しましたので、割愛します。
[xml firstline=”6″]<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />[/xml]
ここではノードを立ち上げています。「name=”joint_state_publisher”」で、立ち上げるノード名をjoint_state_publisherとし、「pkg=”joint_state_publisher”」でノードの動作が書かれた実行ファイルはjoint_state_publisherパッケージにあると宣言します。次に、「type=”joint_state_publisher”」で、joint_state_publisherが実行するファイルであると宣言します。以上の項目が、ノードを立ち上げるために必要な最低限の宣言です。
[xml firstline=”8″]<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />[/xml]
ここもノードを立ち上げる文ですが、いくつかオプションが付け加えられています。「args」というのは、ノードに渡す引数です。端末上でプログラムを起動するとき、コマンドライン上で引数を渡すことができますが、あれと同じようなものですね。「required」というのは、ノードが停止した時にroslaunchを停止させるかどうかを設定できます。ここに「true」を設定しているので、rvizノードが停止すると、roslaunch全体が停止します。

これで、auto_crane_rviz.launchで何が行われていたかは大体説明できたと思います。launchファイルについてもっと詳しく勉強したい方は、こちらをご覧ください。で、読んでいくと、includeタグなるものがあります。これは、launchファイル内で他のlaunchファイルを実行するためのタグです。これを使うと、「roslaunch my_dynamixel_tutorial controller_manager.launch」と「roslaunch roslaunch my_dynamixel_tutorial start_tilt_controller.launch」もauto_crane_rviz.launch内で実行することができます。この場合、以下のように書けばOKです。
[xml]
<launch>
<param name="robot_description" textfile="$(find crane_urdf)/src/crane.urdf" />
<param name="use_gui" value="False"/>
<rosparam param="source_list">["joint_states_source"]</rosparam>

<include file="$(find my_dynamixel_tutorial)/controller_manager.launch" />
<include file="$(find my_dynamixel_tutorial)/start_tilt_controller.launch" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
<node name="reflect_crane_in_rviz" pkg="crane_urdf" type="reflect_crane_in_rviz" />
<node name="turtlebot_arm" pkg="turtlebot_arm" type="turtlebot_arm" />
</launch>
[/xml]
ところが、これだと起動当初にCRANE+が暴走します。なぜかというと、controller_manager.launchやstart_tilt_controller.launchの処理が終わっていない状態、つまり初期設定ができていない状態で他のプログラムが走りだすからです。これを解決するためには、reflect_crane_in_rviz.cppやturtlebot_arm.cppの最初にwaitを入れてやればいいです。または、ROSだけに頼らず、シェルスクリプトを使うという手もありますが、今回は割愛したいと思います。

以上、CRANE+の状態をrviz上でモニタリングしつつ、CRANE+を自動で動かす方法について説明しました。
とりあえず、CRANE+とROSの組み合わせでできることの基本は解説できたと思いますので、今回でCRANE+の連載は終わりにしたいと思います。ここまでお読み下さり、ありがとうございました。

ロボカップジャパンオープン2015 @ホームリーグ報告

出村 賢聖です!ロボカップジャパンオープン2015@ホームSPLリーグに参加してきました!
robocupathome
RoboCup@homeリーグロボット/選手集合写真

ロボカップ@ホームSPLリーグ

splrobots
RoboCup@home SPLリーグロボット集合写真

SPLリーグでは出村の1名チーム D.K.T. IcARusは6チーム中5位になることができました!後日賞状が送られてきます!Turtlebotを貸していただき、東京から福井の大会会場まで車に乗せていただいたRT Corporation、本当にありがとうございました!

RTでの準備

ロボカップ@ホームSPLリーグに向けてタスクのための開発を始めたのは大会1週間前です。それまで色々なことがあって手が回りませんでした。
今回はfollowmeタスクにしぼって開発を進めました。
followmeはロボットが家環境で特定の人を追跡する競技です。

TurtlebotのROSの人間追跡パッケージの問題点

人の追跡にturtlebot_followerという既存のturtlebot用人間追跡ROSパッケージを使用していたのですが、間違えて机の方に向かったり、壁に向かったりなど誤認識が多いです。
turtlebot_follower
誤認識の理由はturtlebot_followerの人間追跡アルゴリズムにありました。
turtlebot_followerのアルゴリズムは、kinectの深さカメラが捉えられる範囲内で最短距離の物体を追跡対象として認識し、追跡する、というものです。
followme競技中、追跡対象はこの条件を満たさないときがあります。
追跡対象である人が机を横切ったり、他の歩行者を通り過ぎると、追跡対象でない物体が追跡対象として認識されてしまいます。
そこで追跡アルゴリズムを改善する必要がありました。。

人間追跡の精度を向上させる方法

人の色情報を活用すれば、認識精度が向上した人間追跡をすることができるのではないかと思いました。
例えば、追跡される人の靴の色は赤、ズボンの色はピンク等、髪の毛は黒、のような色情報です。
そこで今回はCamShiftというアルゴリズムを人間追跡に使用しました。(CamShiftアルゴリズムの説明は後ほど行います)

人間追跡ソフトウエアの開発概要と苦労した点

kinectの画像情報 → CamShiftで追跡者の位置を特定→追跡者位置情報をもとにTurtlebotを制御する
開発するソフトウエアにはこのようなフローが必要になります。
今回はROSというロボットミドルウエア上で、画像情報を取得し、Turtlebotの移動制御を行います。
ここで問題になったのが、ROSの画像型(クラス)→Opencv画像型(クラス)変換や、Opencvの人間追跡コードをroscpp(イベント駆動)で記述するとsegmentation faultを起こすことです。(C++で開発しています)
そこでgdbでデバッグを行ったのですが、ROS Nodeではgdbでソースコードを読みながらステップ実行ができません。バックトレースを見ても参考になるサブルーチン名が出てくることはなくgdbではあまり有用なデバッグができませんでした。printfデバッグを使用すると、OpenCV用のGUIウィンドウの初期化に問題があることが判明しました。問題の箇所は判明したのですが、このときは残念ながら人間追跡のソフトウエアの開発が完了しませんでした。

会場での準備/調整

camshift_node
人間追跡ソフトウエアのデバッグを続けました。
OpenCVのGUIウィンドウの初期化をROS Topicのコールバック関数内に記述せずに、main関数内で初期化を行うとsegmentation faultの問題が解決しました。
追跡者の方位を認識した画像のピクセル位置とkinectのFOV(43°)から計算し、追跡者の距離をkinectで計測、そしてTurtlebotに比例航法とPD距離制御を使って人間追跡ができるようになりました。
これで人間追跡が可能になりました。

Basic Functionalities競技

SPL
Basic Functionalitiesでは惜しくも無得点になってしまいました。 ロボットが家で障害物回避をしながら部屋の好きな場所に移動するAvoid Thatは得点することができるはずでした。
原因は、調整中Avoid Thatのための環境地図を生成していたときスタート地点に人がいたためです。人を障害物として認識してしまいスタート地点から動くことはありませんでした。
Avoid That(家で障害物回避をしながら部屋の好きな場所に移動する)に使用したプログラム

Restaurant競技

IMG_20150503_094419
Restaurantでは惜しくも無得点になってしまいました。Restaurantでははじめに人間追跡をしながら地図生成をするのですが、ロボットが移動時に何回も転倒しそうになりました。これは重心が高い位置にあったのが原因です。そのためロボットアームを外し、下に2kgのおもりをつけることで重心を低くし転倒を防止しました。

Follow Me


Follow Meでは181点得点することができました!
僕がRoboCup@home SPL leagueで使用したfollowmeのソースコード(ROS Node)を公開していますのでぜひ活用してください!
ソースコード
人間追跡にはCAMShift(Continuous Adaptive Mean Shift)というアルゴリズムを使用しています。
CAMShiftはヒストグラムマッチング(色情報)を使った画像追跡のアルゴリズムです。
Mean Shiftアルゴリズムの数学的説明(CAMShiftはMean Shiftの拡張版)

出村のfollowme ROS Node を使用するには

※Turtlebotを動かすROSパッケージとkinectのドライバを入れる必要があります。 ROS Indigo対応
ビルド

[c]$ cd ~/catkin_ws/src[/c]
[c]$ git clone https://github.com/kendemu/followme[/c]
[c]$ rosdep install followme[/c]
[c]$ catkin_make[/c]
実行
[c]$roslaunch turtlebot_bringup minimal.launch[/c]
[c]$roslaunch turtlebot_bringup 3dsensor.launch[/c]
[c]$rosrun followme followme_node[/c]
CAMShiftと書いてあるウィンドウの方を選択し、画像ウィンドウの中の追跡対象をマウスで範囲選択してください。
このプログラムは追跡対象を認識し、その角度を算出します!
[c]$rosrun followme distance_node[/c]
このプログラムは追跡対象が位置する角度情報を元に追跡対象への距離を計測します!
[c]$rosrun followme trackme_node[/c]
このプログラムは追跡対象の角度情報と距離情報を元に追跡対象を追跡します!

Technical Challenge


Technical ChallengeではTurtlebotが屋外の走行も可能であること、ROSのセキュリティ面の問題とドローンの拡張現実ゲームであるDrone Space Defenseのことをお話しました。
Turtlebotの屋外の走行の速度検証(100P参照)
ROSのセキュリティ面の問題

かわいい生活支援ロボット達

IMG_20150503_114918
左はトヨタの生活支援/テレイグジスタンスロボット HSR、右はdemura.netチームのKenseiko chan 2 mobileです!

ロボカップジャパンオープンでの反省とロボカップ@ホームリーグ世界大会に向けて


demura.netチームの一員として7月のロボカップ世界大会に参加します!
ロボカップ世界大会では、ロバストな画像追跡を実現する予定です!

今回のロボカップジャパンオープンでの反省点は画像追跡/物体認識が正確ではないことです。
色情報だけでの追跡(CAMShift)では追跡対象の識別が正確ではないことがわかりました。追跡対象のズボンの色が青のとき、青色のカーテンの方を追跡してしまうことがありました。followmeの競技中は追跡対象の靴の色がユニークな色で、その靴の色情報を元に追跡したので正確に追跡対象を追跡してくれました。
色情報以外に追跡対象の位置情報、深さカメラの情報を利用して正確な画像追跡をする必要があることがわかりました。
世界大会までにこの問題を解決します!
これからもよろしくお願いします!

DCモーター実験キット(その1)

キットに電池ボックスとモーターをつなぐ
現在、好評発売中のDCモーター実験キットをArduino UNO R3で使う方法を紹介します。

準備

 DCモーター実験キットの内容

DCモーター実験キットの内容
DCモーター実験キット
  • モータードライバIC TA7291P x1
  • DCモーター FA-130RA-2270 x1
  • 抵抗10kオーム1/4w x2
  • ブレッドボード x1
  • ジャンパワイヤ(オス-オス) x10
  • 電池ボックス(単3×2本) x1
  •  

     キットの他に用意するもの

  • Arduino Uno R3
  • USBケーブル(A-B)
  • Windows/mac OS/Linux のいずれかを搭載したパソコン
  •  

    組み立て方

  • ブレッドボードにモータードライバIC(TA7291P)を挿します。写真の場所に指すことを前提で説明をするので、ブレッドボードの構造を知らない方は写真のとおりに挿してください。

    ブレッドボードとTA7291P
    ブレッドボードとTA7291P
  • ArduinoとTA7291Pがつながるようにブレッドボードにジャンパワイヤを挿します。

    ArduinoとTA7291P
    ArduinoとTA7291P
  • ArduinoのGNDとかかれたいずれかのピン(画像ではdigital13ピンの隣のGND)からTA7291Pの左から1番目のピンとをつなぎます。
  • ArduinoのDigitalOUT 9pinTA7291Pの左から5番目のピンをつなぎます。
  • ArduinoのDigitalOUT 11pinTA7291Pの左から6番目のピンをつなぎます。
  • Arduinoの5VとかかれたピンとTA7291Pの左から7番目のピンをつなぎます。
  • 抵抗(10kΩ × 2本)をTA7291Pの4番目のピンの手前に順に挿し、片方をとかかれた列に、もう片方をとかかれた列に挿します。

    TA7291Pに抵抗をつなぐ
    TA7291Pに抵抗をつなぐ
  • モーターと電池ボックスをTA7291Pにつながるようにブレッドボードに挿します。

    キットに電池ボックスとモーターをつなぐ
    キットに電池ボックスとモーターをつなぐ
  • モーターからつながっているワイヤの黄色い方TA7291Pの左から2番目のピンに挿します。
  • モーターからつながっているワイヤの青い方TA7291Pの一番右のピンに挿します。
  • 電池ボックスからつながっているワイヤの赤い方をブレッドボードのと書かれた列に挿します。
  • 電池ボックスからつながっているワイヤの黒い方をブレッドボードのと書かれた列に挿します。
  • TA7291Pの右から3番目のピンとブレッドボードのと書かれた列をジャンパワイヤでつなぎます。
  • 電池ボックスのスイッチがオフになっていることを確認したあと、電池を電池ボックスに入れます。
  • Arduino IDE を起動します。
  • 以下のスケッチを書き、任意の名前で保存します。
    [c]
    //サンプルスケッチ
    //モーターの正転逆転を繰り返す
    #define PWM0 9
    #define PWM1 11
    #define LED 13

    void setup() {
    pinMode(PWM0,OUTPUT);
    pinMode(PWM1,OUTPUT);
    pinMode(LED,OUTPUT);
    }

    void loop() {
    analogWrite(PWM0, 255);
    digitalWrite(LED,HIGH);
    delay(5000);

    analogWrite(PWM0, 0);
    digitalWrite(LED,LOW);
    delay(1000);

    analogWrite(PWM1, 255);
    digitalWrite(LED,HIGH);
    delay(5000);

    analogWrite(PWM1, 0);
    digitalWrite(LED,LOW);
    delay(1000);
    }
    //---ここまで---
    [/c]

  • メニューの「ツール」→「マイコンボード」 もしくは「ボード」*でArduino Unoを選択します。
  • メニューの  「ツール」→「シリアルポート」 もしくは「ポート」*で正しいポートを選択します。
  • 「マイコンボードに書き込む」のボタンでArduino Unoにサンプルスケッチをコンパイル・転送します。
  •   成功していた場合、電池ボックスのスイッチをオンにするとモーターが回りだします。5秒に一度止まり、回転が逆になります。
  • 次回以降、回路の説明・サンプルソースの解説・応用方法を紹介します!

    追記(2015.5.13)

    RT-KIT-DCM
    “DCモーター実験キット”はアイル店頭、WEBショップで販売中です。

    ↓ 画像クリックで商品購入ページヘジャンプします ↓ RT-KIT-DCM