2011年12月31日土曜日

よいお年を!


あと2時間ほどで2011年もおしまいです。

2010年ほどは記事を書けませんで、ここの情報も古いものとなりつつありますが、
来年もほそぼそと続けていくつもりですのでよろしくお願いします。

少なくとも以下のリリース時には記事を書きたいですね〜。

来年もよろしくお願いします!!

Fuerte Turtle (Mar 2012) (フォルテタートル)

  • Ubuntu Lucid
  • Ubuntu Oneiric
  • Ubuntu Precise
  • C++03
  • Boost 1.40
  • Lisp SBCL 1.0.x
  • Python 2.6

Groovy Galapagos (Aug 2012) (グルーヴィーガラパゴス)

  • Ubuntu Oneiric
  • Ubuntu P
  • System libraries TBD

2011年12月23日金曜日

RTミドルウェアコンテスト行ってきた

本日(2011年12月23日)に行われたRTミドルウェアコンテストに
完全に僭越ながら審査員として行ってきました!

みんなスーツの中、私は仕事じゃないので私服で審査してきました。
結果は公式に任せるとして、非常に楽しくすごさせていただきました。

RTミドルウェアとROSはいわばライバル関係で、
日本人でありながら何故かその敵側を応援?しちゃっている私ですが、
みなさん暖かく迎えてくださいました。

全体的な印象としては

  • ysugaさんすげー
  • 暖かく、いい雰囲気(使ってくれてありがとう、という人が多い)
  • 教育や学生の発表が多い
  • みんなWindowsが好き
  • ドキュメントが超丁寧


このブログらしきことを書くと、いくつかROSに似たコンセプトのものがありました。




「移動ロボットのソフトウェア開発のための屋内環境シミュレータRTC」
www.openrtm.org/openrtm/ja/project/RTMContest2011_1115
これはstageっぽい。人もシミュレーションに入れられるところが違う。

ysugaさんの
「RTno (汎用マイコンボードarduinoでRTコンポーネント対応デバイスを作るためのライブラリ)」
これも同じようなコンセプトのものがROSにあります。


「初心者向けシステム開発支援用RTC」
これはまるっきりrxplotです。
データをオンラインでグラフ化してくれます。

RTMはとっつきにくい、といっている方が何人かいました。
ROSの方がシンプルなので始めるのは少しは簡単かもしれないと思いました。

RTMの審査員をやってみて思ったのは、ROSとの比較で言うと、
  • 依存関係を考慮した配布・ビルド方法がもう少し楽になるといいと思う。rospack・rosmake・rosdepは非常に強力。
  • 自動生成したソースやら、すべてを1つのフォルダに入れて配っている人がいる気がする。もう少しソースの配置とかのルールを決めたほうがいいと思う。
  • RTC以外でライブラリを配る文化がもっと欲しい。PCLも最初はROSだったけど今はROSを切り離した。
  • ROSはドキュメントはプロむけ。RTMは親切過ぎないか心配。インストールや使い方はむつかしすぎるのをドキュメントでカバーしようとし過ぎなのかも?




なんにせよ、日本のロボット界ではソースを配る文化がなかったから、それを大幅に改善したRTミドルウェアプロジェクトの貢献は大きいと思った。

以上偉そうに好き勝手書きました。
誤解・間違い等あると思いますので、コメントください。


2011年12月9日金曜日

テレプレゼンスロボットの作り方

Makeに持っていったロボットの中身を軽く紹介しようと思います。

まず全体構成は↓のようになっています。
基本的にいままでつくってきたノードをrosbridgeでブラウザから動かしています。

roombaは前に自作したotl_roombaパッケージのもの、lv2_armはアーム用に作ったノード、usb_camはboshのもの、battery_checkerはこの前紹介したノードでPCのバッテリーチェックをします。jtalk_nodeは日本語をしゃべるノードです。
いずれも公開済みです。

ブラウザでアクセスすると以下のように表示されます。
ぐるぐるのところでルンバの速度を出力、スライダやボタンでロボットのカメラ操作、
バッテリ残量を右上のアイコンで表示(@tera_kojiさんに作ってもらいました)、
テキストボックスに文字を書くとロボットがしゃべってくれます。
画面の上には大きくカメラ動画が流れます。
javascript/HTML5で動的なページにしています。
HTMLやjavascriptにはまったく詳しくないので苦労しました。


rosbridgeはsocketやwebsocketでROSプロトコルをしゃべってくれるサーバーで、javascriptからROSを簡単に使えます。
rosbridgeはsubversionの最新版がおすすめです。安定版だとfirefoxだとうまく動きません。

画像を動画で表示する方法は、

HTMLに以下のように書いて、


  <img src="img/camera_dummy.jpg" name="camera" width="320" height="240"/>


javascriptに以下のような感じで書きます。


    connection.addHandler('/usb_cam/image_raw',
                          function(msg) {
                            document.camera.src=msg.uri;
                          });

これだけで/usb_cam/image_rawがガンガン送られてくれば動画になります。

まあ、そんな感じです。


例によって以下のリポジトリにおいておきました。整理もしていないので見せられる状態にないですが、ジョイスティックの部分とか、出せるように整理したいです。

2011年12月8日木曜日

Make Tokyo Meeting 07ありがとう

Make Tokyo Meeting 07通称MTM07の出展が無事終わりました。

このblogの読者に直にあえてうれしかったです。
PCの前からロボットを動かしてくださったみなさん、ありがとうございました。
(完成度が低くて申し訳ないです)

FRISK Roombaも買っていただきありがとうございました。
(快く一緒に参加してくださった@longjie0723さんありがとうございました!)
http://orientalrobotics.blogspot.com/2011/12/mtm07.html

紹介記事がありますので紹介します。
http://ascii.jp/elem/000/000/654/654252/index-6.html

Makeは本当に面白いプロジェクトがいっぱいで、すごく刺激になり、楽しい2日間でした。
ゆっくり見てまわるのもいいけど、また出たいな。

2011年12月3日土曜日

MTM07参加中!

Make Tokyo Meeting 参加中!!
にてロボット遠隔操作&Makeの体験ができるぞ!

2011年11月27日日曜日

MTM07参加します!

MTM07(http://www.oreilly.co.jp/mtm/07/)に出展します。

出展名はOTL++で、Oriental Robotics (http://orientalrobotics.blogspot.com/)さんといっしょにやります。
タイトルはDIYロボティクスとかにした気がします。

目玉はFRISK Roomba。フリスクケースにルンバ操作用Bluetoothモジュールが入っています。
http://orientalrobotics.blogspot.com/2011/02/friskroomba.html

こいつとKinectがあればROSの自律移動も試せるし、いろいろ遊べます。
私がこれまでに上げた動画でもこれを使っています。


Kinectでもなんでもつながります。

パソコンからプログラム余裕です。



あとはテレプレゼンスロボットを持って行きますので、「東京の会場までいけないよ」という人は自宅からMTM07に参加できちゃいます!
ブラウザから操作可能にしていますので後ほどアドレスを書きますので乞うご期待。


他にもいろいろ持っていくので、ロボット好きなひとは是非遊びに来てくださいね。

2011年11月20日日曜日

PC (Linux)のバッテリ残量を調べる

PC(Linux)のバッテリ容量をプログラムから知りたいことがあったのでPythonで作ってみました。
例によって参考程度になればと貼っておきます。
他にもっといい方法あれば教えてください。

一応ノードも作りました。

  1. #! /usr/bin/env python  
  2.   
  3. import re  
  4.   
  5. class AcpiChecker():  
  6.     """read /proc/acpi/battery/*** files for check the PC's battery"""  
  7.     def __init__(self, battery_name):  
  8.         self._info_path = "/proc/acpi/battery/" + battery_name + "/info"  
  9.         self._state_path = "/proc/acpi/battery/" + battery_name + "/state"  
  10.     def get_capacity(self):  
  11.         f = open(self._info_path)  
  12.         for line in f:  
  13.             match = re.search(r'^last full capacity:\s*(\d+)', line)  
  14.             if match:  
  15.                 capacity = match.groups()[0]  
  16.         f.close()  
  17.         return capacity  
  18.   
  19.     def get_state(self):  
  20.         f = open(self._state_path)  
  21.         for line in f:  
  22.             match = re.search(r'^remaining capacity:\s*(\d+)', line)  
  23.             if match:  
  24.                 state = match.groups()[0]  
  25.         f.close()  
  26.         return state  
  27.   
  28.     def get_rate(self):  
  29.         return float(self.get_state()) / float(self.get_capacity())  
  30.   
  31. if __name__=='__main__':  
  32.     checker = AcpiChecker('BAT0')  
  33.     print checker.get_rate()  

ノードはこんな感じでてきとーに作ってみました。


  1. #! /usr/bin/env python  
  2.   
  3. import roslib  
  4. roslib.load_manifest('otl_battery_checker')  
  5.   
  6. import rospy  
  7. from std_msgs.msg import Float64  
  8. from battery_check import AcpiChecker  
  9.   
  10. class BatteryCheckerNode():  
  11.     """check battery and publish the charge rate"""  
  12.     def __init__(self, battery_name='BAT0'):  
  13.         self._checker = AcpiChecker(battery_name)  
  14.         self._pub = rospy.Publisher('/pc/battery_rate', Float64)  
  15.     def proc(self):  
  16.         msg = Float64()  
  17.         msg.data = self._checker.get_rate()  
  18.         self._pub.publish(msg)  
  19.   
  20. if __name__=='__main__':  
  21.     rospy.init_node('pc_battery_checker')  
  22.     battery_name = 'BAT0'  
  23.     if rospy.has_param('~device'):  
  24.         battery_name = rospy.get_param('~device')  
  25.     check = BatteryCheckerNode(battery_name)  
  26.     rate = rospy.Rate(1)  
  27.     while not rospy.is_shutdown():  
  28.         check.proc()  
  29.         rate.sleep()  


参考にしたサイト

2011年11月19日土曜日

RT ミドルウエアコンテスト 2011 参加(ただしお金だけ)

突然ですが、RTミドルウェアコンテストに参加することにしました。

ただし、ミドルウェアはまじめに書いたこと無いので協賛のほうです。(こんなんで審査していいんだろうか)
http://www.openrtm.org/rt/RTMcontest/2011/award.html
勢いでつい本名で出してしまいました。

一応このブログに関係あるっぽくしようと思って、「グローバルスタンダード賞」としました。
ROSに負けない、国際的に認められそうなモジュールがあるといいなぁ。

2011年11月6日日曜日

キーポン届いた!

アメリカ在住の @kunikku92 さんからMy Keeponが届いたよ!

開封の儀式!



キターーー!!!

うしろはこんな感じ。


上から
さあ出てらっしゃい。

おおおおお!!!


  説明書もおしゃれ


ちょっと動かして見ました。
単三電池8本必要です。
構造がひどくて入れるの超大変でした。
動画で後半手拍子していますが、音楽に合わせて踊ります。


Willowグッズももらっちゃった!!やったー!!!
ありがとう!!!
下の光ってる奴はPR2チョコですね。なんと。。。


ありがとう @kunikku92 !!!

2011年10月12日水曜日

トランスフォーマー作ってみた

今日はROSでも僕の作品でもないものの紹介です。
会社のイベントにアイデアコンテストというものがありまして、
今年のテーマが「未来の車」でした。

私たちのチームでいろいろ考えた結果できた車がこれです。

  1. ロボットアームを収納し変身
  2. ヘリを収納し発射
  3. アームでヘリをキャッチしてドッキング!
というオモシロロボットカーです。

アームでは、車から降りないで駐車券を取ったり、ドライブスルーを受け取ったりできます。
ヘリでは渋滞のときに先を見に行ったり、ヘリのカメラで本当のアラウンドビューができます。

ドッキングは男の夢です。
社内コンテストは優勝はできませんでしたが、結構満足行くものが出来ました。
自慢です。

2011年9月8日木曜日

ROS Electricのインストール(PCL1.1との共存)

さそっくelectricをインストールしようとしたのですが、依存関係が満たせないとのエラーになりました。(PCL1.1をスタンドアロンで入れたUbuntu 10.04で、ROS diamondbackが入った状態です。)

しぼっていくと以下のエラーが原因のようです。

依存: libeigen3-dev (= 3.0.1-1+ros4~lucid) しかし、3.0.2-1lucid3 はインストールされようとしています


これはスタンドアローンのPCLとの競合が原因のようです。
まずPCLを削除します。(ROS側で同じバージョンの1.1が入るので削除してもいい??でしょう)

$ sudo apt-get remove libpcl-1.1 libeigen3-dev

次に以下のファイルを削除するか、拡張子を変更するか、中身を"#"でコメントアウトします。
/etc/apt/sources.list.d/v-launchpad-jochen-sprickerhof-de-pcl-lucid.list

そして
$ sudo apt-get update
$ sudo apt-get install ros-electric-desktop-full

で入るはずです。
競合してしまうのは非常に残念ですね。

ROS Electric on OSX (Lion)

ROS ElectricをMac(Lion)に入れようとしましたが、rosconsoleのビルドでこけています。

調べたら以下のパッチがあったのでこれを使ってみたらうまく行きました。
https://code.ros.org/trac/ros/attachment/ticket/3626/ros_comm-1.6.2-rosconsole_clang_osx.patch

以上メモでした。

iheart roboticsさんからTシャツもらった

iheart roboticsさんからROS Tシャツとiheartroboticsグッズを貰いました。

エアメールで頂きました。

みなさんもblogを一度は見たことがあると思います。
Tシャツちょっと長いけどうれしい!

さっそくみんな自慢しましたがここでも自慢させてください。
周りには通じる人少ないんで。


お返しに折り紙の本でも贈ろうかと思っています。

2011年9月2日金曜日

joint_state_publisherメモ(再)

以前joint_state_publisherを紹介しました。
http://ros-robot.blogspot.com/2010/11/jointstatepublisher.html

http://www.ros.org/wiki/joint_state_publisher

詳しいやり方を書かなかったので自分のためのメモです。

まず、joint_state_publisherをゲットします。
そのためにはmercurialというソフトを必要としますので、いれましょう。

$ sudo apt-get install mercurial

次に以下のコマンドでjoint_state_publisherを取得します。

$ hg clone https://kforge.ros.org/robotmodel/visualization

そうしたら、hoge_descriptionというパッケージにurdf/hoge.xmlというurdfファイルを作ったとすると、以下のようなlaunchファイルを用意して、
パラメータにファイルを読み込ませ、joint_state_publisherを駆動し、robot_state_publisherを動かし、rvizで表示します。
joint_state_publisherがsensor_msgs/JointStateを発行し、それをrobot_state_publisherがtfに変換します。それをrvizが表示するのです。

  1. <launch>  
  2.   <param name="robot_description" textfile="$(find hoge_description)/urdf/hoge.xml"/>  
  3.   <param name="use_gui" value="true"/>  
  4.   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />  
  5.   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />  
  6.   <node name="rviz" pkg="rviz" type="rviz"/>  
  7. </launch>  


2011年9月1日木曜日

ROS Electric Emys release

Electric がリリースされました。
http://www.ros.org/wiki/electric

Diamondbackからの変更点のうち私が気になったものをざっと見てみましょう。


  • arm navigation (アームのプランニング系)1.0。
    • やっとアーム系がまともに使えるように
  • Interactive markers (rvizで物体に触れるようになる)
    • rvizでインタラクティブに操作できるように
  • PCL1.1 
    • 最新版が使えるようになりました
  • Android
    • javaによる実装のrosjavaがはいりました
  • tf
    • 最大100倍早くなったそうです。
  • sensor_msgs/Joy
    • joy/Joyメッセージがsensor_msgsに格上げです
  • eigen, opencv2, yaml_cppなどがUbuntu nativeのものを利用するように
  • Unary stackの導入
    • スタックかつパッケージというUnary stackができた!
    • リリース単位のstackと機能単位のpackageという矛盾を解消するようです
というくらいですかね。
まだインストールはしていません。

2011年8月28日日曜日

m3pi robotを買った

@longjie0723 さんがm3pi robotというものを買うと言っていたので
きっと面白いものだろうと思って買ってみました。
(@longjie0723さんありがとうございます。)
http://www.pololu.com/catalog/product/2153

流行りの(ちょっと古い?)mbedというマイコンが載った小型ロボットです。
mbedの特徴はブラウザでプログラムが書ける、Ether使える、USBホスト機能ありなどですかね。
センサはライントレース用の光センサがついています。
いろいろ拡張ができそうです。

手のひらサイズでものすごく速く動きます。
とりあえず試しにLCDにOTLと表示したり、LEDつけて暴れさせたりして遊んでみました。


プログラムもC++で書けて非常に簡単です。

既存のロボットだと7万円〜30万円くらいしますから(もちろんセンサ数とか全然違うかもしれませんが)
プログラマブルな小型ロボットとしてはいい感じじゃないでしょうか?

2011年8月25日木曜日

Point Cloud Libraryを試す(その5:ユークリッドクラスター抽出)

いい加減PCLも飽きてきましたが、これもやっちゃいましょう。
クラスタリングです。
画像処理でもラベリングは結構使うと思います。
それの3D版です。

Euclidean Cluster Extraction
http://pointclouds.org/documentation/tutorials/cluster_extraction.php#cluster-extraction

こちらのチュートリアルのほうがシンプルですが、私は可視化したかったので↓のように少し改造してビューワを付けました。



$ wget http://dev.pointclouds.org/attachments/download/157/table_scene_lms400.pcd


して、以下をcluster_extraction.cppとして保存です。


  1. #include <pcl/ModelCoefficients.h>  
  2. #include <pcl/point_types.h>  
  3. #include <pcl/io/pcd_io.h>  
  4. #include <pcl/features/normal_3d.h>  
  5. #include <pcl/filters/extract_indices.h>  
  6. #include <pcl/filters/voxel_grid.h>  
  7. #include <pcl/kdtree/kdtree.h>  
  8. #include <pcl/sample_consensus/method_types.h>  
  9. #include <pcl/sample_consensus/model_types.h>  
  10. #include <pcl/segmentation/sac_segmentation.h>  
  11. #include <pcl/segmentation/extract_clusters.h>  
  12. #include <pcl/visualization/cloud_viewer.h>  
  13.   
  14.   
  15. int  
  16. main (int argc, char** argv)  
  17. {  
  18.   // Read in the cloud data  
  19.   pcl::PCDReader reader;  
  20.   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);  
  21.   reader.read ("table_scene_lms400.pcd", *cloud);  
  22.   std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*  
  23.   
  24.   // Create the filtering object: downsample the dataset using a leaf size of 1cm  
  25.   pcl::VoxelGrid<pcl::PointXYZ> vg;  
  26.   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);  
  27.   vg.setInputCloud (cloud);  
  28.   vg.setLeafSize (0.01, 0.01, 0.01);  
  29.   vg.filter (*cloud_filtered);  
  30.   std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*  
  31.   
  32.   // Create the segmentation object for the planar model and set all the parameters  
  33.   pcl::SACSegmentation<pcl::PointXYZ> seg;  
  34.   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);  
  35.   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  
  36.   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());  
  37.   pcl::PCDWriter writer;  
  38.   seg.setOptimizeCoefficients (true);  
  39.   seg.setModelType (pcl::SACMODEL_PLANE);  
  40.   seg.setMethodType (pcl::SAC_RANSAC);  
  41.   seg.setMaxIterations (100);  
  42.   seg.setDistanceThreshold (0.02);  
  43.   
  44.   int i=0, nr_points = cloud_filtered->points.size ();  
  45.   while (cloud_filtered->points.size () > 0.3 * nr_points)  
  46.     {  
  47.       // Segment the largest planar component from the remaining cloud  
  48.       seg.setInputCloud(cloud_filtered);  
  49.       seg.segment (*inliers, *coefficients); //*  
  50.       if (inliers->indices.size () == 0)  
  51. <span class="Apple-tab-span" style="white-space: pre;"> </span>{  
  52. <span class="Apple-tab-span" style="white-space: pre;"> </span>  std::cout << "Could not estimate a planar model for the given dataset." << std::endl;  
  53. <span class="Apple-tab-span" style="white-space: pre;"> </span>  break;  
  54. <span class="Apple-tab-span" style="white-space: pre;"> </span>}  
  55.   
  56.       // Extract the planar inliers from the input cloud  
  57.       pcl::ExtractIndices<pcl::PointXYZ> extract;  
  58.       extract.setInputCloud (cloud_filtered);  
  59.       extract.setIndices (inliers);  
  60.       extract.setNegative (false);  
  61.   
  62.       // Write the planar inliers to disk  
  63.       extract.filter (*cloud_plane); //*  
  64.       std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;  
  65.   
  66.       // Remove the planar inliers, extract the rest  
  67.       extract.setNegative (true);  
  68.       extract.filter (*cloud_filtered); //*  
  69.     }  
  70.   
  71.   // Creating the KdTree object for the search method of the extraction  
  72.   pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);  
  73.   tree->setInputCloud (cloud_filtered);  
  74.   
  75.   std::vector<pcl::PointIndices> cluster_indices;  
  76.   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;  
  77.   ec.setClusterTolerance (0.02); // 2cm  
  78.   ec.setMinClusterSize (100);  
  79.   ec.setMaxClusterSize (25000);  
  80.   ec.setSearchMethod (tree);  
  81.   ec.setInputCloud( cloud_filtered);  
  82.   ec.extract (cluster_indices);  
  83.   
  84.   int j = 0;  
  85.   float colors[6][3] ={{255, 0, 0}, {0,255,0}, {0,0,255}, {255,255,0}, {0,255,255}, {255,0,255}};  
  86.   pcl::visualization::CloudViewer viewer("cluster viewer");  
  87.   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);  
  88.   pcl::copyPointCloud(*cloud_filtered, *cloud_cluster);  
  89.   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)  
  90.     {  
  91.       for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) {  
  92. <span class="Apple-tab-span" style="white-space: pre;"> </span>cloud_cluster->points[*pit].r = colors[j%6][0];  
  93. <span class="Apple-tab-span" style="white-space: pre;"> </span>cloud_cluster->points[*pit].g = colors[j%6][1];  
  94. <span class="Apple-tab-span" style="white-space: pre;"> </span>cloud_cluster->points[*pit].b = colors[j%6][2];  
  95.       }  
  96.       std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;  
  97.       std::stringstream ss;  
  98.       ss << "cloud_cluster_" << j << ".pcd";  
  99.       writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false); //*  
  100.       j++;  
  101.     }  
  102.   viewer.showCloud (cloud_cluster);   
  103.   while (!viewer.wasStopped())  
  104.     {  
  105.       sleep (1);  
  106.     }  
  107.   
  108.   
  109.   return (0);  
  110. }  

するとこんな感じです。


ちなみに元データはこんな感じ。テーブル上面と床が削除されています。


少しだけコードを読むと、
最初にポイントクラウドをファイルから読み込み、pcl::VexelGridを使ってダウンサンプリングしています。
ダウンサンプリングのチュートリアルもありますが簡単なので飛ばしました。
setLeafSizeで10mm刻みのグリッドでサンプリングしていますね。

次に前回やった平面検出で平面を削除しています。
複数の平面検出をどうやるのかと思ったら、検出した領域を削除して、削除したもに対してもう一度検出することでやっているみたいです。
while(...)のところです。

で、今回のキモはpcl::KdTreeを使ってEuclideanClusterEtractionしているところです。
setClusterToleranceでマージする最大距離を指定して、最小、最大のパーティクル数を指定しているようです。

CMakeListx.txtはいつもと同じです。


cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(cluster_extraction)

find_package(PCL 1.1 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (cluster_extraction cluster_extraction.cpp)
target_link_libraries (cluster_extraction ${PCL_LIBRARIES})

ここまでで、机の上にある物体をラベリングして認識することができるようになりました。
あとはフィルタのチュートリアルでもやればひと通り終わりかな?

大体感じはつかめたのでPCL本体はこれくらいにしておきましょう。

これで少しは面白いことができるかもしれません。

次はROSとのからみを見てみます。

2011年8月24日水曜日

Point Cloud Libraryを試す(その4:平面抽出)

今回はポイントクラウドから平面を抽出します。

その前に、前回使ったPointXYZRGBの色の部分がfloat rgbとなっていて気になったので調べてみました。

// pack r/g/b into rgb
 uint8_t r = 255, g = 0, b = 0;    // Example: Red color
 uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
 p.rgb = *reinterpret_cast<float*>(&rgb);
↑のような感じで使うみたいです。
悪名高きreinterpret_castか!と思ったら、
ver.1.1.0からはr,g,b変数も使えるみたいで、
point.r = 255;
point.g = 100;
point.b = 10;

みたいに指定できるようです。


PCLのAPIリファレンスはこちらです。
Point Cloud Library (PCL): PCL API Documentation

という小ネタを挟みまして、今回は平面検出です。

↓のようにPointCloudから平面っぽいところを切りだしてくれるはずです。

チュートリアルはこちら

チュートリアルにあるコードは簡単なんですが、ビューワが付いていないので、ビューワを付けてみました。
以下をplanar_segmentation.cppとして保存してください。

  1. #include <iostream>  
  2. #include <pcl/ModelCoefficients.h>  
  3. #include <pcl/io/pcd_io.h>  
  4. #include <pcl/point_types.h>  
  5. #include <pcl/sample_consensus/method_types.h>  
  6. #include <pcl/sample_consensus/model_types.h>  
  7. #include <pcl/segmentation/sac_segmentation.h>  
  8. #include <pcl/visualization/cloud_viewer.h>  
  9.   
  10. int  
  11. main (int argc, char** argv)  
  12. {  
  13.   pcl::PointCloud<pcl::PointXYZRGB> cloud;  
  14.   
  15.   // Fill in the cloud data  
  16.   cloud.width  = 15;  
  17.   cloud.height = 10;  
  18.   cloud.points.resize (cloud.width * cloud.height);  
  19.   
  20.   // Generate the data  
  21.   size_t i;  
  22.   for (i = 0; i < cloud.points.size ()/2; ++i)  
  23.     {  
  24.       // 平面におく  
  25.       cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);  
  26.       cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);  
  27.       cloud.points[i].z = 0.1 * rand () / (RAND_MAX + 1.0);  
  28.       cloud.points[i].r = 255;  
  29.       cloud.points[i].g = 255;  
  30.       cloud.points[i].b = 255;  
  31.     }  
  32.   
  33.   for (; i < cloud.points.size (); ++i)  
  34.     {  
  35.       // ちらばらせる  
  36.       cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);  
  37.       cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);  
  38.       cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);  
  39.       cloud.points[i].r = 255;  
  40.       cloud.points[i].g = 255;  
  41.       cloud.points[i].b = 255;  
  42.     }  
  43.   
  44.   // Set a few outliers  
  45.   cloud.points[0].z = 2.0;  
  46.   cloud.points[3].z = -2.0;  
  47.   cloud.points[6].z = 4.0;  
  48.   
  49.   std::cerr << "Point cloud data: " << cloud.points.size () << " points" << std::endl;  
  50.   for (size_t i = 0; i < cloud.points.size (); ++i)  
  51.     std::cerr << "    " << cloud.points[i].x << " "  
  52.      << cloud.points[i].y << " "  
  53.      << cloud.points[i].z << std::endl;  
  54.   
  55.   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  
  56.   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);  
  57.   // Create the segmentation object  
  58.   pcl::SACSegmentation<pcl::PointXYZRGB> seg;  
  59.   // Optional  
  60.   seg.setOptimizeCoefficients (true);  
  61.   // Mandatory  
  62.   seg.setModelType (pcl::SACMODEL_PLANE);  
  63.   seg.setMethodType (pcl::SAC_RANSAC);  
  64.   seg.setDistanceThreshold (0.1);  
  65.   
  66.   seg.setInputCloud (cloud.makeShared ());  
  67.   seg.segment (*inliers, *coefficients);  
  68.   
  69.   if (inliers->indices.size () == 0)  
  70.     {  
  71.       PCL_ERROR ("Could not estimate a planar model for the given dataset.");  
  72.       return (-1);  
  73.     }  
  74.   
  75.   std::cerr << "Model coefficients: " << coefficients->values[0] << " "  
  76.    << coefficients->values[1] << " "  
  77.    << coefficients->values[2] << " "  
  78.    << coefficients->values[3] << std::endl;  
  79.   
  80.   std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;  
  81.   for (size_t i = 0; i < inliers->indices.size (); ++i) {  
  82.     std::cerr << inliers->indices[i] << "    " << cloud.points[inliers->indices[i]].x << " "  
  83.      << cloud.points[inliers->indices[i]].y << " "  
  84.      << cloud.points[inliers->indices[i]].z << std::endl;  
  85.     cloud.points[inliers->indices[i]].r = 255;  
  86.     cloud.points[inliers->indices[i]].g = 0;  
  87.     cloud.points[inliers->indices[i]].b = 0;  
  88.   }  
  89.   pcl::visualization::CloudViewer viewer("Cloud Viewer");  
  90.   
  91.   viewer.showCloud(cloud.makeShared());  
  92.   
  93.   while (!viewer.wasStopped ())  
  94.     {  
  95.        
  96.     }  
  97.   
  98.   return (0);  
  99. }  
するとこんな感じで平面っぽいところにあるポイントが赤く表示されました。

CMakeLists.txtはいつもと同じです。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(planar_segmentation)

find_package(PCL 1.1 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (planar_segmentation planar_segmentation.cpp)
target_link_libraries (planar_segmentation ${PCL_LIBRARIES})
$ cmake .
$ make
でビルドして、
$ ./planar_segmentation
で実行してみます。


SACSegmentationというClassが今回の本質のようです。ここにSACMODEL_PLANE=平面モデルを指定したり、
setDistanceThresholdでモデルとの誤差の閾値を指定しています。setMethodTypeで計算方法を選択(今回はSAC_RANSAC)しています。RANSACは確率的なモデル推定だったきがします。
coefficients->valuesに入っているのは推定されたモデル
ax+by+cz+d=0の平面の式になります。

これで平面上にあるポイントが検出できました。

さらにKinectでやってみました。前回のKinectのキャプチャサンプルに今回のを合体させてみました。
一番広い平面が真っ赤にそまります。これは楽しい!

一応ソースコードです。適当です。
  1. #include <iostream>  
  2. #include <pcl/ModelCoefficients.h>  
  3. #include <pcl/io/openni_grabber.h>  
  4. #include <pcl/io/pcd_io.h>  
  5. #include <pcl/point_types.h>  
  6. #include <pcl/sample_consensus/method_types.h>  
  7. #include <pcl/sample_consensus/model_types.h>  
  8. #include <pcl/segmentation/sac_segmentation.h>  
  9. #include <pcl/visualization/cloud_viewer.h>  
  10.   
  11. void segmentate(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double threshould) {  
  12.   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  
  13.   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);  
  14.   // Create the segmentation object  
  15.   pcl::SACSegmentation<pcl::PointXYZRGB> seg;  
  16.   // Optional  
  17.   seg.setOptimizeCoefficients (true);  
  18.   // Mandatory  
  19.   seg.setModelType (pcl::SACMODEL_PLANE);  
  20.   seg.setMethodType (pcl::SAC_RANSAC);  
  21.   seg.setDistanceThreshold (threshould);  
  22.   
  23.   seg.setInputCloud (cloud.makeShared ());  
  24.   seg.segment (*inliers, *coefficients);  
  25.   
  26.   for (size_t i = 0; i < inliers->indices.size (); ++i) {  
  27.     cloud.points[inliers->indices[i]].r = 255;  
  28.     cloud.points[inliers->indices[i]].g = 0;  
  29.     cloud.points[inliers->indices[i]].b = 0;  
  30.   }  
  31. }  
  32.   
  33.  class SimpleOpenNIViewer  
  34.  {  
  35.    public:  
  36.      SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {}  
  37.   
  38.      void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)  
  39.      {  
  40.        if (!viewer.wasStopped()) {  
  41.   pcl::PointCloud<pcl::PointXYZRGB> segmented_cloud(*cloud);  
  42.   segmentate(segmented_cloud, 0.01);  
  43.          viewer.showCloud (segmented_cloud.makeShared());  
  44.        }  
  45.      }  
  46.   
  47.      void run ()  
  48.      {  
  49.        pcl::Grabber* interface = new pcl::OpenNIGrabber();  
  50.   
  51.        boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =  
  52.          boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);  
  53.   
  54.        interface->registerCallback (f);  
  55.        interface->start ();  
  56.        while (!viewer.wasStopped())  
  57.        {  
  58.          sleep (1);  
  59.        }  
  60.   
  61.        interface->stop ();  
  62.      }  
  63.   
  64.      pcl::visualization::CloudViewer viewer;  
  65.  };  
  66.   
  67.  int main ()  
  68.  {  
  69.    SimpleOpenNIViewer v;  
  70.    v.run ();  
  71.    return 0;  
  72.  }  

2011年8月22日月曜日

Point Cloud Libraryを試す(その3:Kinectからデータ取得)

いよいよKinectを使ってデータを取得してみます。
http://pointclouds.org/documentation/tutorials/openni_grabber.php#openni-grabber

データが取得できれば、これまでやった、ファイルへの保存、ビューワでの表示、がとりあえずできるようになるわけです。
公式のサンプルコードは動かないので↓のように修正しました。

  1. #include <pcl/io/openni_grabber.h>  
  2. #include <pcl/visualization/cloud_viewer.h>  
  3.   
  4.  class SimpleOpenNIViewer  
  5.  {  
  6.    public:  
  7.      SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {}  
  8.   
  9.      void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)  
  10.      {  
  11.        if (!viewer.wasStopped())  
  12.          viewer.showCloud (cloud);  
  13.      }  
  14.   
  15.      void run ()  
  16.      {  
  17.        pcl::Grabber* interface = new pcl::OpenNIGrabber();  
  18.   
  19.        boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =  
  20.          boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);  
  21.   
  22.        interface->registerCallback (f);  
  23.   
  24.        interface->start ();  
  25.   
  26.        while (!viewer.wasStopped())  
  27.        {  
  28.          sleep (1);  
  29.        }  
  30.   
  31.        interface->stop ();  
  32.      }  
  33.   
  34.      pcl::visualization::CloudViewer viewer;  
  35.  };  
  36.   
  37.  int main ()  
  38.  {  
  39.    SimpleOpenNIViewer v;  
  40.    v.run ();  
  41.    return 0;  
  42.  }  

ビューワで表示されます。




さらに
#include<pcl/io/pcd_io.h>
して、cloud_cb_の中に
pcl::io::savePCDFileBinary("kinect.pcd", *cloud);
を追加すればファイルに保存もできますね。
マイフレーム保存してもけっこう軽いです。

次回は平面抽出をやります。

2011年8月21日日曜日

Point Cloud Libraryを試す(その2:ビューワ編)

前回ファイルの書き込みをしました。
今回は読み込みを飛ばして可視化をします。
可視化できないと何をやっているのかさっぱりなので。

http://pointclouds.org/documentation/tutorials/cloud_viewer.php#cloud-viewer


前回のコードを少し書き換えて、ビューワのコードをいれてみました。

  1. #include <iostream>  
  2. #include <pcl/io/pcd_io.h>  
  3. #include <pcl/point_types.h>  
  4. #include <pcl/visualization/cloud_viewer.h> // 追加  
  5.   
  6. int  
  7.   main (int argc, char** argv)  
  8. {  
  9.   pcl::PointCloud<pcl::PointXYZ> cloud;  
  10.     
  11.   // Fill in the cloud data  
  12.   cloud.width    = 5;  
  13.   cloud.height   = 1;  
  14.   cloud.is_dense = false;  
  15.   cloud.points.resize (cloud.width * cloud.height);  
  16.   
  17.   for (size_t i = 0; i < cloud.points.size (); ++i)  
  18.   {  
  19.     cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);  
  20.     cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);  
  21.     cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);  
  22.   }  
  23.   // 追加  
  24.   pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");  
  25.   viewer.showCloud (cloud.makeShared());  
  26.   while (!viewer.wasStopped ()) {  
  27.   }  
  28.   
  29.   return (0);  
  30. }  

CMakeLists.txtは以下のような感じ。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(pcd_write)

find_package(PCL 1.1 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (pcd_write pcd_write.cpp)
add_executable (pcl_view_test pcl_view_test.cpp)

target_link_libraries (pcd_write ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})
target_link_libraries (pcl_view_test ${PCL_LIBRARIES})

$ ./pcl_view_test
とすると以下のようなビューワが立ち上がります。
マウスを駆使してうまく表示します。ホイールで縮小するとよさそうです。


見えないでしょうがちっちゃい5つの白い点が作成したポイントクラウドです。

ためしに点を100x100くらいにしてみます。widthとheightを100にしてみます。

おー、ポイントクラウドって感じですね。

公式のチュートリアルはこちらです。
もう少し詳しいので必要になったら参照したいと思います。
http://pointclouds.org/documentation/tutorials/cloud_viewer.php#cloud-viewer

2011年8月20日土曜日

Point Cloud Libraryを試す(その1:インストール&コンパイルテスト編)

ROSを使わない前提でのインストールを説明します。

(ROSでのPCLのインストールは簡単です。
$ sudo apt-get install ros-diamondback-point-cloud-perception
だけ。ROSでは/usr/とかにインストールしないので、以下のインストール方法と競合はしません。)

相変わらず環境はUbuntu 10.04です。
http://pointclouds.org/downloads/linux.html


sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
sudo apt-get update
sudo apt-get install libpcl-1.1-dev

以上でおしまいです。
簡単ですね。

ではまずこれをやります。

Writing Point Cloud data to PCD files (ポイントクラウドデータをPCDファイルに書きこむ)

まず以下の内容をpcd_write.cppというファイル名で保存します。



#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

int
  main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ> cloud;

  // Fill in the cloud data
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.points.resize (cloud.width * cloud.height);

  for (size_t i = 0; i < cloud.points.size (); ++i)
  {
    cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
    cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
    cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
  }

  pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);
  std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;

  for (size_t i = 0; i < cloud.points.size (); ++i)
    std::cerr << "    " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;

  return (0);
}



で、次のようなCMakeLists.txtを作成します。


cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(pcd_write)

find_package(PCL 1.1 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (pcd_write pcd_write.cpp)
target_link_libraries (pcd_write ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})


$ cmake .
$ make
とするとpcd_writeという実行ファイルができるはずです。
$ ./pcd_write
して、以下のように表示されればOK

$ ./pcd_write
Saved 5 data points to test_pcd.pcd.
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762

test_pcd.pcdというファイルができているはずです。
中身は

# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 5
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 5
DATA ascii
0.35222197 -0.15188313 -0.10639524
-0.3974061 -0.47310591 0.29260206
-0.73189831 0.66710472 0.44130373
-0.73476553 0.85458088 -0.036173344
-0.46070004 -0.2774682 -0.91676188
のようになっています。

pcl::PointXYZの定義は

// \brief A point structure representing Euclidean xyz coordinates.
struct PointXYZ
{
  float x;
  float y;
  float z;
};
となっています。
たんなる3次元座標ですね。この他にも色情報を含んだPointXYZRGBなど、さまざまなタイプがpcl/point_types.hに定義されています。

pcl::PointCloudクラスはwidthやheightを持ちます。これは画像のアナロジーになっているみたいです。
Kinectでは距離画像(画素が距離情報の画像)を取れるので親和性が高いのでしょう。

is_denseはTrueの場合にNaNやInfが値として入っていないことを保証するようです。

あとstd_msgs::Header型のheaderという変数も持ちます。これはROSのものと同じ型です。
ROSに依存していないのになぜこれが使えるかというと、
/usr/include/pcl-1.1/の下にstd_msgsとsensor_msgsがインストールされているからです。
非常に気持ち悪いですね。

あとは
pcl::io::savePCDFileASCII関数でファイルに出力しています。非常に簡単ですね。

2011年8月19日金曜日

Point Cloud Libraryを試す(その0)

かなり遅くなりましたが今後このブログでPoint Cloud Library (PCL)を試したいと思います。

PCLはKinectやレーザーレンジファインダで獲得できる3次元の点群を使った認識を行うためのライブラリです。(たぶん)
(including filtering, feature estimation, surface reconstruction, registration, model fitting and segmentation)とあります。


HPを見るとスポンサーとして、intel, nvidia, willowなどが並び、日本でも産総研、東大、トヨタが付いているようですね。


私はPCLはかなり前から注目していましたが、正直何に使っていいか分かりませんでした。
Kinectが発売され、誰でも使えるようになり、さらに注目度が増していたわけですが、
やはりどうロボットにいれていいかわからず手がでませんでした。
6軸以上のアームがあれば物体認識に使えるんでしょうけど、
そんなもの持っていませんし、作るにもお金とスキルと時間が必要です。

また、なんとか私が持っているロボット(移動台車、Kinect、2軸アーム)での利用法をさがそうとしていましたが、
やはり思いつきませんでした。
(1つ考えていますが、あまり面白くないです。誰かネタがあったら教えてください。)

でも勉強していくうちに思いつくかもしれない、と思って見切り発車でやっていこうと思います。

PCLはROS依存とROS依存じゃないところをちゃんと意識して作っているっぽいので、ROSじ
ゃない方面からやるか、ROSでやるか、という2つのやり方があると思います。
ドキュメントの再利用性を考えて、ROS非依存で進めて、最後にROSからだとこうなる、という感じで進めようかと思います。
最後にルンバにKinectを載せてなんかやるでしょう。

ではでは。

2011年8月16日火曜日

OpenRTM-asit ROSパッチ(ros port)を使ってルンバを操縦する

前回OpenRTM-aistからROSを使えるようにしました。
次はお決まりですがルンバを動かします。

今回はOpenRTM-aistの仮想ジョイスティックコンポーネントを利用してルンバを動かします。

構成としては

TkJoyStick(RTC) -> JoyRTM2velROS(今回作ったコンポーネント)(RTC/ROS) -> いつもの自作ルンバノード(ROS)

という感じです。
TkJoyStickはOpenRTM-aistのサンプルに入っていて、ジョイスティックっぽいGUIです。

面倒なのでビデオ解説です。



JoyRTM2velROSのソースはTkJoyStickからの入力(TimedFloatSeq)をROSのgeometry_msgs/Twist型に変換しているだけです。
やはりOpenRTMとROSは非常に似ていますね。

OpenRTM-aistのコードの自動生成は便利だけど修正がめんどくさいです。
とくにRosPortのように標準ツールでサポートされていないと大変です。
なにかいい方法があれば教えてください。

参考程度にcppのソースファイルだけ貼りつけておきます。
いつも通り適当です。

一応リポジトリに
otl-ros-pkg/otl_nav/JoyRTM2velROS
としておいておきました。

【参考サイト】
http://code.google.com/p/rtm-ros-robotics/wiki/rosport_Example
http://code.google.com/p/rtm-ros-robotics/wiki/RTM_HelloWorldSample



ヘッダファイル

  1. // -*- C++ -*-  
  2. /*! 
  3.  * @file  JoyRTM2velROS.h 
  4.  * @brief convert Joystick sample port to ROS twist msg 
  5.  * @date  $Date$ 
  6.  * 
  7.  * $Id$ 
  8.  */  
  9.   
  10. #ifndef JOYRTM2VELROS_H  
  11. #define JOYRTM2VELROS_H  
  12.   
  13. #include <rtm/Manager.h>  
  14. #include <rtm/DataFlowComponentBase.h>  
  15. #include <rtm/CorbaPort.h>  
  16. #include <rtm/DataInPort.h>  
  17. #include <rtm/DataOutPort.h>  
  18. #include <rtm/idl/BasicDataTypeSkel.h>  
  19. #include <rtm/idl/ExtendedDataTypesSkel.h>  
  20. #include <rtm/idl/InterfaceDataTypesSkel.h>  
  21. #include <rtm/RosOutPort.h>  
  22. #include "geometry_msgs/Twist.h"  
  23.   
  24. using namespace RTC;  
  25.   
  26. /*! 
  27.  * @class JoyRTM2velROS 
  28.  * @brief convert Joystick sample port to ROS twist msg 
  29.  * 
  30.  */  
  31. class JoyRTM2velROS  
  32.   : public RTC::DataFlowComponentBase  
  33. {  
  34.  public:  
  35.   /*! 
  36.    * @brief constructor 
  37.    * @param manager Maneger Object 
  38.    */  
  39.   JoyRTM2velROS(RTC::Manager* manager);  
  40.   
  41.   /*! 
  42.    * @brief destructor 
  43.    */  
  44.   ~JoyRTM2velROS();  
  45.   
  46.   /*** 
  47.    * 
  48.    * The initialize action (on CREATED->ALIVE transition) 
  49.    * formaer rtc_init_entry()  
  50.    * 
  51.    * @return RTC::ReturnCode_t 
  52.    *  
  53.    *  
  54.    */  
  55.    virtual RTC::ReturnCode_t onInitialize();  
  56.   
  57.   /*** 
  58.    * 
  59.    * The execution action that is invoked periodically 
  60.    * former rtc_active_do() 
  61.    * 
  62.    * @param ec_id target ExecutionContext Id 
  63.    * 
  64.    * @return RTC::ReturnCode_t 
  65.    *  
  66.    *  
  67.    */  
  68.    virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);  
  69.   
  70.  protected:  
  71.   
  72.   // Configuration variable declaration  
  73.   // <rtc-template block="config_declare">  
  74.   /*! 
  75.    *  
  76.    * - Name:  max_linear_velocity 
  77.    * - DefaultValue: 0.1 
  78.    */  
  79.   double m_max_linear_velocity;  
  80.   /*! 
  81.    *  
  82.    * - Name:  max_angular_velocity 
  83.    * - DefaultValue: 0.2 
  84.    */  
  85.   double m_max_angular_velocity;  
  86.   // </rtc-template>  
  87.   
  88.   // DataInPort declaration  
  89.   // <rtc-template block="inport_declare">  
  90.   TimedFloatSeq m_joyInput;  
  91.   /*! 
  92.    */  
  93.   InPort<timedfloatseq> m_joyInputIn;  
  94.     
  95.   // ROS Port  
  96.   geometry_msgs::Twist m_rosVel;  
  97.   RosOutPort<geometry_msgs::twist> m_rosVelOut;  
  98.   
  99.  private:  
  100.   
  101. };  
  102.   
  103.   
  104. extern "C"  
  105. {  
  106.   DLL_EXPORT void JoyRTM2velROSInit(RTC::Manager* manager);  
  107. };  
  108.   
  109. #endif // JOYRTM2VELROS_H  
  110. </geometry_msgs::twist></timedfloatseq></rtc-template>  

実体(.cpp)

  1. // -*- C++ -*-  
  2. /*! 
  3.  * @file  JoyRTM2velROS.cpp 
  4.  * @brief convert Joystick sample port to ROS twist msg 
  5.  * @date $Date$ 
  6.  * 
  7.  * $Id$ 
  8.  */  
  9.   
  10. #include "JoyRTM2velROS.h"  
  11.   
  12. // Module specification  
  13. // <rtc-template block="module_spec">  
  14. static const char* joyrtm2velros_spec[] =  
  15.   {  
  16.     "implementation_id""JoyRTM2velROS",  
  17.     "type_name",         "JoyRTM2velROS",  
  18.     "description",       "convert Joystick sample port to ROS twist msg",  
  19.     "version",           "1.0.0",  
  20.     "vendor",            "OTL",  
  21.     "category",          "Sample",  
  22.     "activity_type",     "PERIODIC",  
  23.     "kind",              "DataFlowComponent",  
  24.     "max_instance",      "1",  
  25.     "language",          "C++",  
  26.     "lang_type",         "compile",  
  27.     // Configuration variables  
  28.     "conf.default.max_linear_velocity""0.2",  
  29.     "conf.default.max_angular_velocity""0.4",  
  30.     // Widget  
  31.     "conf.__widget__.max_linear_velocity""slider",  
  32.     "conf.__widget__.max_angular_velocity""slider",  
  33.     // Constraints  
  34.     ""  
  35.   };  
  36. // </rtc-template>  
  37.   
  38. /*! 
  39.  * @brief constructor 
  40.  * @param manager Maneger Object 
  41.  */  
  42. JoyRTM2velROS::JoyRTM2velROS(RTC::Manager* manager)  
  43.     // <rtc-template block="initializer">  
  44.   : RTC::DataFlowComponentBase(manager),  
  45.     m_joyInputIn("joy_input", m_joyInput)  
  46.   
  47.     // </rtc-template>  
  48.     ,  
  49.     m_rosVelOut("cmd_vel""rtm2ros", m_rosVel)  
  50. {  
  51. }  
  52.   
  53. /*! 
  54.  * @brief destructor 
  55.  */  
  56. JoyRTM2velROS::~JoyRTM2velROS()  
  57. {  
  58. }  
  59.   
  60.   
  61.   
  62. RTC::ReturnCode_t JoyRTM2velROS::onInitialize()  
  63. {  
  64.   // Registration: InPort/OutPort/Service  
  65.   // <rtc-template block="registration">  
  66.   // Set InPort buffers  
  67.   addInPort("joy_input", m_joyInputIn);  
  68.   
  69.   // Set OutPort buffer  
  70.   
  71.   // Set service provider to Ports  
  72.   
  73.   // Set service consumers to Ports  
  74.   
  75.   // Set CORBA Service Ports  
  76.   
  77.   // </rtc-template>  
  78.   
  79.   // <rtc-template block="bind_config">  
  80.   // Bind variables and configuration variable  
  81.   bindParameter("max_linear_velocity", m_max_linear_velocity, "0.2");  
  82.   bindParameter("max_angular_velocity", m_max_angular_velocity, "0.4");  
  83.   // </rtc-template>  
  84.   
  85.   addRosOutPort("ros_velocity", m_rosVelOut);  
  86.   
  87.   return RTC::RTC_OK;  
  88. }  
  89.   
  90.   
  91. RTC::ReturnCode_t JoyRTM2velROS::onExecute(RTC::UniqueId ec_id)  
  92. {  
  93.   if(m_joyInputIn.isNew()) {  
  94.     m_joyInputIn.read();  
  95.   
  96.     m_rosVel.linear.x = m_joyInput.data[1] * 0.005;  
  97.     m_rosVel.angular.z = -m_joyInput.data[0] * 0.010;  
  98.   
  99.     if ( m_max_linear_velocity < m_rosVel.linear.x) {  
  100.       m_rosVel.linear.x = m_max_linear_velocity;  
  101.     } else if (-m_max_linear_velocity > m_rosVel.linear.x) {  
  102.       m_rosVel.linear.x = -m_max_linear_velocity;  
  103.     } else if (fabs(m_rosVel.linear.x) < 0.01) {  
  104.       m_rosVel.linear.x = 0.0;  
  105.     }  
  106.     if ( m_max_angular_velocity < m_rosVel.angular.z) {  
  107.       m_rosVel.angular.z = m_max_angular_velocity;  
  108.     } else if (-m_max_angular_velocity > m_rosVel.angular.z) {  
  109.       m_rosVel.angular.z = -m_max_angular_velocity;  
  110.     } else if (fabs(m_rosVel.angular.z) < 0.01) {  
  111.       m_rosVel.angular.z = 0.0;  
  112.     }  
  113.   
  114.     m_rosVelOut.write();  
  115.   }  
  116.   return RTC::RTC_OK;  
  117. }  
  118.   
  119.   
  120.   
  121. extern "C"  
  122. {  
  123.   
  124.   void JoyRTM2velROSInit(RTC::Manager* manager)  
  125.   {  
  126.     coil::Properties profile(joyrtm2velros_spec);  
  127.     manager->registerFactory(profile,  
  128.                              RTC::Create<JoyRTM2velROS>,  
  129.                              RTC::Delete<JoyRTM2velROS>);  
  130.   }  
  131.   
  132. };  

OpenRTM-asit ROSパッチを試す(インストール編)

OpenRTMにROS通信を追加するパッチがあります。
これを使ってみます。

環境はUbuntu10.04、ROSはdiamondbackです。
かなり苦労しました。

http://www.openrtm.org/OpenRTM-aist/html/E3839EE3838BE383A5E382A2E383AB2Frosport.html

ソースからOpenRTM-aistをビルドする必要ああるのでソースをゲットします。
1.0.0です。

$ wget http://www.openrtm.org/pub/OpenRTM-aist/cxx/1.0.0/OpenRTM-aist-1.0.0-RELEASE.tar.bz2

パッチもゲット
$ wget http://www.openrtm.org/pub/OpenRTM-aist/cxx/1.0.0/ros_transport.patch-1.0.1.tar.gz


aptでいれたOpenRTM-aistは削除しておきます。
$ sudo apt-get remove openrtm-aist-example openrtm-aist openrtm-aist-dev

パッケージの展開
$ tar jxvf OpenRTM-aist-1.0.0-RELEASE.tar.bz2
$ tar zxvf ros_transport.patch-1.0.1.tar.gz
$ cd OpenRTM-aist-1.0.0

パッチを当てます。

$ patch -p0 < ../ros_transport.patch

そしてautoconfができるようにlibtool.m4を現在使っているシステムのものと入れ替えます(ここ重要)
$ find . -name libtool.m4 -exec cp /usr/share/aclocal/libtool.m4 {} \;

さらにsrc/lib/rtm/Makefile.amの以下の変数を次のように書き換えます。(ここも重要)
(cturtleを使っている場合はこれなくても動くようです)

ros_cflags=`rospack export --lang=cpp --attrib=cflags roscpp`
ros_libs=`rospack export --lang=cpp --attrib=lflags roscpp`

そしたらautoreconfして、あとはお決まりの流れです。

$ autoreconf -ifv
$ ./configure
$ make

インストール
$ sudo make install 
これで/usr/local/以下にインストールされます。



@hyaguchijskさんに協力をいただきました。ありがとうございました。

次にサンプルをmakeします。
$ wget http://www.openrtm.org/pub/OpenRTM-aist/cxx/1.0.0/examples.ros-1.0.0.tar.gz
$ tar zxvf examples.ros-1.0.0.tar.gz
$ cd examples.ros
$ cd publisher
$ make

$ cd ../subscriber
$ make

この状態でpublisherCompとsubscriberCompを動かしてActivateすると
それぞれPub/Subしてくれます。

やっと動いた・・・。