drawer_cluster_detector.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00042 // ROS core
00043 #include <ros/ros.h>
00044 // Messages
00045 #include <visualization_msgs/Marker.h>
00046 #include <sensor_msgs/PointCloud2.h>
00047 // PCL stuff
00048 #include <pcl/point_types.h>
00049 #include <pcl/point_cloud.h>
00050 #include <pcl/ros/conversions.h>
00051 #include "pcl/sample_consensus/method_types.h"
00052 #include "pcl/sample_consensus/model_types.h"
00053 #include <pcl/segmentation/sac_segmentation.h>
00054 #include <pcl/filters/voxel_grid.h>
00055 #include <pcl/filters/passthrough.h>
00056 #include <pcl/filters/project_inliers.h>
00057 #include <pcl/surface/convex_hull.h>
00058 #include "pcl/filters/extract_indices.h"
00059 #include "pcl/segmentation/extract_polygonal_prism_data.h"
00060 #include "pcl/common/common.h"
00061 #include <pcl/io/pcd_io.h>
00062 #include "pcl/segmentation/extract_clusters.h"
00063 #include <pcl/features/normal_3d.h>
00064 #include <pcl/common/angles.h>
00065 #include "pcl/common/common.h"
00066 #include <pcl_ros/publisher.h>
00067 
00068 #include <tf/transform_broadcaster.h>
00069 #include <tf/transform_listener.h>
00070 #include <tf/message_filter.h>
00071 
00072 
00073 typedef pcl::PointXYZ Point;
00074 typedef pcl::PointCloud<Point> PointCloud;
00075 typedef PointCloud::Ptr PointCloudPtr;
00076 typedef PointCloud::Ptr PointCloudPtr;
00077 typedef PointCloud::ConstPtr PointCloudConstPtr;
00078 typedef pcl::KdTree<Point>::Ptr KdTreePtr;
00079 
00080 const tf::Vector3 wp_normal(1, 0, 0);
00081 const double wp_offset = -1.45;
00082 
00083 // Waits for a point cloud with pan-tilt close to (0,0) and deduces the position of ptu_base
00085 class DrawerHandlesDetector 
00086 {
00087 public:
00089   DrawerHandlesDetector (const ros::NodeHandle &nh) : nh_ (nh)
00090   {
00091     nh_.param("z_min_limit", z_min_limit_, 0.7);
00092     nh_.param("z_max_limit", z_max_limit_, 0.9);
00093     nh_.param("y_min_limit", y_min_limit_, -0.5);
00094     nh_.param("y_max_limit", y_max_limit_, 0.5);
00095     nh_.param("x_min_limit", x_min_limit_, 0.0);
00096     nh_.param("x_max_limit", x_max_limit_, 1.0);
00097     nh_.param("publish_largest_handle_pose", publish_largest_handle_pose_, false);
00098     
00099     nh_.param("k", k_, 30);
00100     normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
00101     n3d_.setKSearch (k_);
00102     n3d_.setSearchMethod (normals_tree_);
00103     
00104     nh_.param("sac_distance", sac_distance_, 0.02);
00105     nh_.param("normal_distance_weight", normal_distance_weight_, 0.05);
00106     nh_.param("max_iter", max_iter_, 500);
00107     nh_.param("eps_angle", eps_angle_, 20.0);
00108     nh_.param("seg_prob", seg_prob_, 0.99);
00109     seg_.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00110     seg_.setMethodType (pcl::SAC_RANSAC);
00111     seg_.setDistanceThreshold (sac_distance_);
00112     seg_.setNormalDistanceWeight (normal_distance_weight_);
00113     seg_.setOptimizeCoefficients (true);
00114     btVector3 axis(0.0, 0.0, 1.0);
00115     seg_.setAxis (Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()), fabs(axis.getZ())));
00116     seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
00117     seg_.setMaxIterations (max_iter_);
00118     seg_.setProbability (seg_prob_);
00119 
00120     nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.03);
00121     nh_.param("object_cluster_min_size", object_cluster_min_size_, 200);
00122     cluster_.setClusterTolerance (object_cluster_tolerance_);
00123     cluster_.setSpatialLocator(0);
00124     cluster_.setMinClusterSize (object_cluster_min_size_);
00125     //    cluster_.setMaxClusterSize (object_cluster_max_size_);
00126     clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
00127     clusters_tree_->setEpsilon (1);
00128     cluster_.setSearchMethod (clusters_tree_);
00129 
00130     nh_.param("nr_cluster", nr_cluster_, 1);
00131 
00132     nh_.param("cluster_min_height", cluster_min_height_, 0.03);
00133     nh_.param("cluster_max_height", cluster_max_height_, 0.1);
00134 
00135     nh_.param("handle_cluster_tolerance", handle_cluster_tolerance_, 0.02);
00136     nh_.param("handle_cluster_min_size", handle_cluster_min_size_,40);
00137     nh_.param("handle_cluster_max_size", handle_cluster_max_size_, 500);
00138     handle_cluster_.setClusterTolerance (handle_cluster_tolerance_);
00139     handle_cluster_.setSpatialLocator(0);
00140     handle_cluster_.setMinClusterSize (handle_cluster_min_size_);
00141     handle_cluster_.setMaxClusterSize (handle_cluster_max_size_);
00142     cluster_.setSearchMethod (clusters_tree_);
00143     
00144     seg_line_.setModelType (pcl::SACMODEL_LINE);
00145     seg_line_.setMethodType (pcl::SAC_RANSAC);
00146     seg_line_.setDistanceThreshold (0.05);
00147 //    seg_line_.setNormalDistanceWeight (0.0);
00148     seg_line_.setOptimizeCoefficients (true);
00149     seg_line_.setMaxIterations (max_iter_);
00150     seg_line_.setProbability (seg_prob_);
00151 
00152     nh_.param("min_table_inliers", min_table_inliers_, 100);
00153     nh_.param("voxel_size", voxel_size_, 0.01);
00154     nh_.param("point_cloud_topic", point_cloud_topic_, std::string("/shoulder_cloud2"));
00155     nh_.param("output_handle_topic", output_handle_topic_,
00156     std::string("handle_projected_inliers/output"));
00157     nh_.param("handle_pose_topic", handle_pose_topic_, std::string("handle_pose"));
00158     cloud_pub_.advertise (nh_, "debug_cloud", 1);
00159     //cloud_extracted_pub_.advertise (nh_, "cloud_extracted", 1);
00160     cloud_handle_pub_.advertise (nh_, output_handle_topic_, 10);
00161     handle_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped> (handle_pose_topic_, 1);
00162   }
00163 
00165   virtual ~DrawerHandlesDetector () 
00166   {
00167   }
00168 
00169     
00170   void 
00171   init ()  // tolerance: how close to (0,0) is good enough?
00172   {
00173     ROS_INFO ("[DrawerHandlesDetector:] Listening for incoming data on topic %s", nh_.resolveName (point_cloud_topic_).c_str ());
00174     point_cloud_sub_ = nh_.subscribe (point_cloud_topic_, 1,  &DrawerHandlesDetector::ptuFinderCallback, this);
00175     //object_name_ = object_name;
00176   }
00177     
00178 private:
00180   void 
00181   ptuFinderCallback (const sensor_msgs::PointCloud2ConstPtr &cloud_in)
00182     {
00183       ROS_INFO_STREAM ("[" << getName ().c_str () << "] Received cloud: cloud time " << cloud_in->header.stamp);
00184       PointCloud cloud_raw, cloud;
00185       // Downsample + filter the input dataser
00186       pcl::fromROSMsg (*cloud_in, cloud_raw);
00187 
00188       PointCloudPtr cloud_raw_ptr (new PointCloud(cloud_raw));
00189       PointCloudPtr cloud_x_ptr (new PointCloud());
00190       PointCloudPtr cloud_y_ptr (new PointCloud());
00191       PointCloudPtr cloud_z_ptr (new PointCloud());
00192 
00193       //Downsample
00194       vgrid_.setInputCloud (cloud_raw_ptr);
00195       vgrid_.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
00196       //Filter x
00197       vgrid_.setFilterFieldName ("x");
00198       vgrid_.setFilterLimits (x_min_limit_, x_max_limit_);
00199       vgrid_.filter (*cloud_x_ptr);
00200       //Filter y
00201       vgrid_.setInputCloud (cloud_x_ptr);
00202       vgrid_.setFilterFieldName ("y");
00203       vgrid_.setFilterLimits (y_min_limit_, y_max_limit_);
00204       vgrid_.filter (*cloud_y_ptr);
00205       //Filter z
00206       vgrid_.setInputCloud (cloud_y_ptr);
00207       vgrid_.setFilterFieldName ("z");
00208       vgrid_.setFilterLimits (z_min_limit_, z_max_limit_);
00209       vgrid_.filter (*cloud_z_ptr);
00210       //For Debug
00211       //cloud_pub_.publish(cloud_z_ptr);
00212       //return;
00213 
00214       //Estimate Point Normals
00215       pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
00216       n3d_.setInputCloud (cloud_z_ptr);
00217       n3d_.compute (*cloud_normals);
00218       
00219       //Segment the biggest furniture_face plane
00220       pcl::ModelCoefficients::Ptr table_coeff (new pcl::ModelCoefficients ());
00221       pcl::PointIndices::Ptr table_inliers (new pcl::PointIndices ());
00222       seg_.setInputCloud (cloud_z_ptr);
00223       seg_.setInputNormals (cloud_normals);
00224       seg_.segment (*table_inliers, *table_coeff);
00225       ROS_INFO ("[%s] Table model: [%f, %f, %f, %f] with %d inliers.", getName ().c_str (), 
00226                 table_coeff->values[0], table_coeff->values[1], table_coeff->values[2], table_coeff->values[3], 
00227                 (int)table_inliers->indices.size ());
00228 
00229       if ((int)table_inliers->indices.size () <= min_table_inliers_)
00230       {
00231         ROS_ERROR ("table has to few inliers");
00232         return;
00233       }
00234 
00235       //Extract the biggest cluster correponding to above inliers
00236       std::vector<pcl::PointIndices> clusters;
00237       cluster_.setInputCloud (cloud_z_ptr);
00238       cluster_.setIndices(table_inliers);
00239       cluster_.extract (clusters);
00240 
00241       
00242       PointCloudPtr biggest_face (new PointCloud());
00243       if (int(clusters.size()) >= nr_cluster_)
00244       {
00245         for (int i = 0; i < nr_cluster_; i++)
00246         {
00247           pcl::copyPointCloud (*cloud_z_ptr, clusters[i], *biggest_face);
00248         }
00249       }
00250       else
00251       {
00252         ROS_ERROR("Only %ld clusters found with size > %d points", clusters.size(), object_cluster_min_size_);
00253         return;
00254       }
00255       ROS_INFO("Found biggest face with %ld points", biggest_face->points.size());
00256       //For Debug
00257       //cloud_pub_.publish(*biggest_face);
00258       //return;
00259 
00260       //Project Points into the Perfect plane
00261       PointCloudPtr cloud_projected (new PointCloud());
00262       proj_.setInputCloud (biggest_face);
00263       proj_.setModelCoefficients (table_coeff);
00264       proj_.setModelType (pcl::SACMODEL_PARALLEL_PLANE);
00265       proj_.filter (*cloud_projected);
00266       //For Debug
00267       //cloud_pub_.publish(*cloud_projected);
00268       //return;
00269 
00270       PointCloudPtr cloud_hull (new PointCloud());
00271       // Create a Convex Hull representation of the projected inliers
00272       chull_.setInputCloud (cloud_projected);
00273       chull_.reconstruct (*cloud_hull);
00274       ROS_INFO ("Convex hull has: %d data points.", (int)cloud_hull->points.size ());
00275       if ((int)cloud_hull->points.size () == 0)
00276       {
00277         ROS_WARN("Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ());
00278         return;
00279       }
00280       //For Debug
00281       cloud_pub_.publish(*cloud_hull);
00282       pcl::PointXYZ hull_min;
00283       pcl::PointXYZ hull_max;
00284       pcl::PointXYZ hull_center;
00285       pcl::getMinMax3D (*cloud_hull, hull_min, hull_max);
00286       hull_center.x = (hull_max.x + hull_min.x)/2;
00287       hull_center.y = (hull_max.y + hull_min.y)/2;
00288       hull_center.z = (hull_max.z + hull_min.z)/2;
00289       //return;
00290 
00291       // Extract the handle clusters using a polygonal prism 
00292       pcl::PointIndices::Ptr handles_indices (new pcl::PointIndices ());
00293       ROS_INFO_STREAM("min, max height" << cluster_min_height_ << " " << cluster_max_height_);
00294       prism_.setHeightLimits (cluster_min_height_, cluster_max_height_);
00295       //prism_.setInputCloud (cloud_z_ptr);
00296       prism_.setInputCloud (cloud_raw_ptr);
00297       prism_.setInputPlanarHull (cloud_hull);
00298       prism_.segment (*handles_indices);
00299       ROS_INFO ("[%s] Number of handle point indices: %d.", getName ().c_str (), (int)handles_indices->indices.size ());
00300 
00301       //Cluster handles
00302       PointCloudPtr handles (new PointCloud());
00303       pcl::copyPointCloud (*cloud_raw_ptr, *handles_indices, *handles);
00304       //For Debug
00305       //cloud_pub_.publish(*handles);
00306       //return;
00307 
00308       std::vector<pcl::PointIndices> handle_clusters;
00309       handle_cluster_.setInputCloud (handles);
00310       handle_cluster_.extract (handle_clusters);
00311       ROS_INFO ("[%s] Found handle clusters: %d.", getName ().c_str (), (int)handle_clusters.size ());
00312       if ((int)handle_clusters.size () == 0)
00313         return;
00314 
00315       PointCloudPtr handle_final (new PointCloud());
00316       PointCloudPtr handle_final_final (new PointCloud());
00317       pcl::ModelCoefficients::Ptr line_coeff (new pcl::ModelCoefficients ());
00318       pcl::PointIndices::Ptr line_inliers (new pcl::PointIndices ());
00319 
00320       uint handle_clusters_size;
00321       // if (publish_largest_handle_pose_)
00322       //   handle_clusters_size = 1;
00323       // else
00324       handle_clusters_size = handle_clusters.size();
00325 
00326       double dist = 1000.0;
00327       geometry_msgs::PoseStamped handle_pose_final;
00328       //fit lines, project points into perfect lines
00329       for (uint i = 0; i < handle_clusters_size; i++)
00330       {
00331         pcl::copyPointCloud (*handles, handle_clusters[i], *handle_final);
00332         // seg_line_.setInputCloud (handle_final);
00333         // seg_line_.segment (*line_inliers, *line_coeff);
00334         // ROS_INFO("line_inliers %ld", line_inliers->indices.size());
00335         // //Project Points into the Perfect plane
00336         // PointCloudPtr line_projected (new PointCloud());
00337         // proj_.setInputCloud (handle_final);
00338         // proj_.setModelCoefficients (line_coeff);
00339         // proj_.setModelType (pcl::SACMODEL_LINE);
00340         // proj_.filter (*line_projected);
00341         //For Debug
00342         
00343         //TODO: get centroid, get orientation and publish pose
00344         //stamped, change topic name
00345         pcl::PointXYZ point_center;
00346         geometry_msgs::PoseStamped handle_pose;
00347         std::string frame (cloud_in->header.frame_id);
00348         getHandlePose(handle_final, table_coeff, frame,
00349                        handle_pose);
00350         //      handle_pose_pub_.publish(handle_pose);
00351         double dist_tmp = sqrt((hull_center.x - handle_pose.pose.position.x)*(hull_center.x - handle_pose.pose.position.x) + 
00352           (hull_center.y - handle_pose.pose.position.y)*(hull_center.y - handle_pose.pose.position.y));
00353         std::cerr << "dist: " << i << " " << dist_tmp << "cluster size: " << handle_final->points.size () << std::endl;
00354         if (dist_tmp < dist)
00355           {
00356             dist = dist_tmp;
00357             handle_pose_final = handle_pose;
00358             *handle_final_final = *handle_final;
00359           }
00360       //   ROS_INFO("Handle pose published: x %f, y %f, z %f, ox %f, oy 
00361       // %f, oz %f, ow %f", handle_pose.pose.position.x,
00362       // handle_pose.pose.position.y, handle_pose.pose.position.z,
00363       // handle_pose.pose.orientation.x, handle_pose.pose.orientation.y,
00364       // handle_pose.pose.orientation.z, handle_pose.pose.orientation.w);
00365       }
00366       if (dist < 999.0)
00367         {
00368           handle_pose_pub_.publish(handle_pose_final);
00369           cloud_handle_pub_.publish(*handle_final_final);
00370         }
00371     }
00372 
00373   void getHandlePose(pcl::PointCloud<Point>::Ptr line_projected,
00374                      pcl::ModelCoefficients::Ptr table_coeff,
00375                      std::string & frame,
00376                      geometry_msgs::PoseStamped & pose)
00377   {
00378     //Calculate the centroid of the line
00379     pcl::PointXYZ point_min;
00380     pcl::PointXYZ point_max;
00381     pcl::getMinMax3D (*line_projected, point_min, point_max);
00382     // point_min = line_projected->points[line_projected->points.size() - 1];
00383     // point_max = line_projected->points[0];
00384     pose.pose.position.x = (point_max.x + point_min.x)/2;
00385     pose.pose.position.y = (point_max.y + point_min.y)/2;
00386     pose.pose.position.z = (point_max.z + point_min.z)/2;
00387 
00388     ROS_INFO("min  %f %f %f", point_min.x, point_min.y, point_min.z);
00389     ROS_INFO("max  %f %f %f", point_max.x, point_max.y, point_max.z);
00390 
00391     //Calculate orientation of the line
00392     //x = orientation of the normal of the plane
00393     //y = cross product of x and y
00394     //z = orientation of the handle line
00395     // btVector3 x_axis(table_coeff->values[0],  table_coeff->values[1],  table_coeff->values[2]);
00396     // btVector3 z_axis(point_max.x - point_min.x, point_max.y - point_min.y, point_max.z - point_min.z);
00397     // x_axis.normalize();
00398     // z_axis.normalize();
00399     // btVector3 y_axis = (z_axis.cross(x_axis)).normalize();
00400     // x_axis =  (y_axis.cross(z_axis)).normalize();
00401     // z_axis =  (x_axis.cross(y_axis)).normalize();
00402     // y_axis = (z_axis.cross(x_axis)).normalize();
00403 
00404     // ROS_DEBUG("x_AXIS %f %f %f len %f", x_axis.x(), x_axis.y(), x_axis.z(), x_axis.length());
00405     // ROS_DEBUG("y_AXIS %f %f %f len %f", y_axis.x(), y_axis.y(), y_axis.z(), y_axis.length());
00406     // ROS_DEBUG("z_AXIS %f %f %f len %f", z_axis.x(), z_axis.y(), z_axis.z(), z_axis.length());
00407   
00408     // btMatrix3x3 rot(x_axis.x(), y_axis.x(), z_axis.x(),
00409     //                   x_axis.y(), y_axis.y(), z_axis.y(),
00410     //                 x_axis.z(), y_axis.z(), z_axis.z());
00411     // btQuaternion rot_quat;
00412     // rot.getRotation(rot_quat);
00413     pose.pose.orientation.x = 0.0;
00414     pose.pose.orientation.y = 0.0;
00415     pose.pose.orientation.z = 0.0;
00416     pose.pose.orientation.w = 1.0;
00417     pose.header.frame_id = frame;
00418     pose.header.stamp = ros::Time::now();
00419     pose.header.seq = 0;
00420   }
00421 
00422   ros::NodeHandle nh_;
00423   double voxel_size_;
00424 
00425   std::string point_cloud_topic_, output_handle_topic_, handle_pose_topic_;
00426   double object_cluster_tolerance_, handle_cluster_tolerance_, cluster_min_height_, cluster_max_height_;
00427   int object_cluster_min_size_, object_cluster_max_size_, handle_cluster_min_size_, handle_cluster_max_size_;
00428 
00429   double sac_distance_, normal_distance_weight_, z_min_limit_, z_max_limit_;
00430   double y_min_limit_, y_max_limit_, x_min_limit_, x_max_limit_;
00431   double eps_angle_, seg_prob_;
00432   int k_, max_iter_, min_table_inliers_, nr_cluster_;
00433   //whether to publish the pose of the largest handle found or all of them
00434   bool publish_largest_handle_pose_;
00435 
00436   ros::Subscriber point_cloud_sub_;
00437   pcl_ros::Publisher<Point> cloud_pub_;
00438   pcl_ros::Publisher<Point> cloud_handle_pub_;
00439   ros::Publisher handle_pose_pub_;
00440 
00441   // PCL objects
00442   //pcl::PassThrough<Point> vgrid_;                   // Filtering + downsampling object
00443   pcl::VoxelGrid<Point> vgrid_;                   // Filtering + downsampling object
00444   pcl::NormalEstimation<Point, pcl::Normal> n3d_;   //Normal estimation
00445   // The resultant estimated point cloud normals for \a cloud_filtered_
00446   pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_;
00447   pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;               // Planar segmentation object
00448   pcl::SACSegmentation<Point> seg_line_;               // Planar segmentation object
00449   pcl::ProjectInliers<Point> proj_;               // Inlier projection object
00450   pcl::ExtractIndices<Point> extract_;            // Extract (too) big tables
00451   pcl::ConvexHull<Point> chull_;  
00452   pcl::ExtractPolygonalPrismData<Point> prism_;
00453   pcl::PointCloud<Point> cloud_objects_;
00454   pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_;
00455   KdTreePtr clusters_tree_, normals_tree_;
00456 
00458 
00459   std::string getName () const { return ("DrawerHandlesDetector"); }
00460 };
00461 
00462 /* ---[ */
00463 int
00464 main (int argc, char** argv)
00465 {
00466   ros::init (argc, argv, "drawer_cluster_detector");
00467   ros::NodeHandle nh("~");
00468   DrawerHandlesDetector dhd (nh);
00469   dhd.init ();  // 5 degrees tolerance
00470   ros::spin ();
00471 }
00472 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


handle_detection
Author(s): Nico Blodow
autogenerated on Thu May 23 2013 06:56:41