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してくれます。

やっと動いた・・・。

続OpenRTM-aist

引き続きOpenRTMのサンプルを試してみました。
移動ロボットを仮想ジョイパッドで動かすことができます。




やっぱりかなりハマりました。
でも↓のサイトにだいたい書いてあります。
非常に参考になりました。
http://code.google.com/p/rtm-ros-robotics/wiki/RTM_2DSimulator_Example


とくに
$ sudo apt-get install tix
とか、実はバグがあるとか、ハマりどころを押さえていますね。

公式ドキュメントもアップデートしていただきたい所です。

2011年8月11日木曜日

OpenRTM-aistのインストール(自分用メモ)

日本で唯一OpenRTMではなくROS陣営と思われている(?)私ですが、
Electricがリリースされるまでネタがないので、
たまにはOpenRTMやってみようと思いました。
以下はメモ書きみたいなものです。

OpenRTMはまずこれやれ、っていうのがあったら教えてください。
ルンバをOpenRTMで動かせるか教えてください。


OpenRTM-aist 1.00(C++) on Ubuntu 10.04です。

http://www.openrtm.org/openrtm/ja/node/1001
に従いインストールしてみます。

OpenRTMはysugaさんが紹介していますね。

http://ysuga.net/robot/rtm

1.source.listの編集

/etc/apt/source.list.d/openrtm.list


deb http://www.openrtm.org/pub/Linux/ubuntu lucid main
を追加。

2.インストール

2.1.C++版インストール

以下のようにしてみました。成功したけど、これで動くかな。
$ sudo apt-get update

$ sudo apt-get install gcc g++ make uuid-dev
$ sudo apt-get install libomniorb4-dev omniidl4 ominiorb4-nameserver
$ sudo apt-get install openrtm-aist openrtm-aist-doc openrtm-aist-dev openrtm-aist-example
(公式どおりやると以下のようにエラーがでました。
$ apt-get install libomniorb4 libomniorb4-dev omniidl4 omniorb4-nameserver
パッケージリストを読み込んでいます... 完了
依存関係ツリーを作成しています                
状態情報を読み取っています... 完了
E: パッケージ libomniorb4 が見つかりません)
2.2.Python版インストール
Python版もいれてしまいましょう。
# apt-get install python-omniorb2-omg omniidl4-python
# apt-get install openrtm-aist-python openrtm-aist-python-example
2.3.Eclipseとツールのインストール
全部入りをダウンロードします。
$ tar zxvf eclipse342_rtmtools100release_linux_ja.tar.gz
$ cd eclipse
$ ./eclipse
で起動。
3.動作確認
次に何やればいいかわからなかったけどぐぐったらでました。
http://openrtm.org/openrtm/ja/content/%E5%8B%95%E4%BD%9C%E7%A2%BA%E8%AA%8D-linux%E7%B7%A8
3.1.ネームサーバの起動
$ rtm-naming
3.2 コンフィグ作成
$ mkdir ~/RTCwork
$ cd ~/RTCwork
して、以下の内容でrtc.confを作成.最初のnameserversのところはlocalhostでは動きませんでした。ipv6のせいだそうです。
corba.nameservers: 127.0.0.1
 naming.formats: %h.host_cxt/%n.rtc
 logger.enable: NO
 example.ConsoleOut.config_file: consout.conf
 example.ConsoleIn.config_file: consin.conf

3.3。サンプル実行

以下のようにしてインストールされているサンプルを実行

インプット側。
$ cd ~/RTCwork

$ /usr/share/OpenRTM-aist/examples/ConsoleInComp -f rtc.conf 

アウトプット側。
$ cd ~/RTCwork
$ /usr/share/OpenRTM-aist/examples/ConsoleOutComp -f rtc.conf 
(公式はexampsになっているので注意。)
また、必ずcd ~/RTCworkしてください。

あとはEclipseのSystemEditorから接続すればOK!
できたーーー!!

最後めっちゃ端折りましたが、SystemEditorが全く動かなくて困りました。
が、実はrtc.confにlocalhostと書いていたのが原因でした。

うーん疲れた。

PR2 SE発表

Willow Garageの新しいロボットPR2 SEが発表されました。

http://www.willowgarage.com/blog/2011/08/10/announcing-pr2se

新しい、といってもPR2のアームが1本になって安くなっただけです。
でもセンサも新しくなるようです。Kinect搭載かな?

気になるお値段は $285,000 = 約2200万円!
オープンソースへの貢献をすれば3割引なので、
1500万円で買えます。
たしかPR2は$400,000だから、まあまあお得ですね。
これは個人でも手が届く範囲ですね!

って全然無理だな。。。

研究機関ならがんばれば買えるのかな。。。