extract_clusters_on_table.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 
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 //for actionlib
00073 #include <actionlib/server/simple_action_server.h>
00074 #include <pcl_cloud_tools/GetClustersAction.h>
00075 
00076 #include "pcl/common/common.h"
00077 typedef pcl::PointXYZ Point;
00078 typedef pcl::PointCloud<Point> PointCloud;
00079 typedef PointCloud::Ptr PointCloudPtr;
00080 typedef PointCloud::ConstPtr PointCloudConstPtr;
00081 typedef pcl::search::KdTree<Point>::Ptr KdTreePtr;
00082 
00083 const tf::Vector3 wp_normal(1, 0, 0);
00084 const double wp_offset = -1.45;
00085 
00086 class ExtractClusters 
00087 {
00088 public:
00090   ExtractClusters (const ros::NodeHandle &nh) : nh_ (nh), 
00091                                                 as_(nh_, "get_clusters", boost::bind(&ExtractClusters::executeCB, this, _1), false)
00092     {
00093       nh_.param("sac_distance", sac_distance_, 0.03);
00094       nh_.param("z_min_limit", z_min_limit_, 0.3);
00095       nh_.param("z_max_limit", z_max_limit_, 1.5);
00096       nh_.param("max_iter", max_iter_, 500);
00097       nh_.param("normal_distance_weight", normal_distance_weight_, 0.1);
00098       nh_.param("eps_angle", eps_angle_, 20.0);
00099       nh_.param("seg_prob", seg_prob_, 0.99);
00100       nh_.param("normal_search_radius", normal_search_radius_, 0.05);
00101       //what area size of the table are we looking for?
00102       nh_.param("rot_table_frame", rot_table_frame_, std::string("rotating_table"));
00103       nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.03);
00104       //min 100 points
00105       nh_.param("object_cluster_min_size", object_cluster_min_size_, 100);
00106       nh_.param("k", k_, 10);
00107 //      nh_.param("base_link_head_tilt_link_angle", base_link_head_tilt_link_angle_, 0.8);
00108       nh_.param("min_table_inliers", min_table_inliers_, 100);
00109       nh_.param("cluster_min_height", cluster_min_height_, 0.02);
00110       nh_.param("cluster_max_height", cluster_max_height_, 0.4);
00111       nh_.param("nr_cluster", nr_cluster_, 1);
00112       nh_.param("downsample", downsample_, true);
00113       nh_.param("voxel_size", voxel_size_, 0.01);
00114       nh_.param("save_to_files", save_to_files_, false);
00115       nh_.param("point_cloud_topic", point_cloud_topic, std::string("/narrow_stereo_textured/points2"));
00116       nh_.param("publish_token", publish_token_, false);
00117       nh_.param("padding", padding_, 0.85);
00118 
00119       cloud_pub_.advertise (nh_, "table_inliers", 1);
00120       //      cloud_extracted_pub_.advertise (nh_, "cloud_extracted", 1);
00121       cloud_objects_pub_.advertise (nh_, "cloud_objects", 10);
00122 
00123       vgrid_.setFilterFieldName ("z");
00124       vgrid_.setFilterLimits (z_min_limit_, z_max_limit_);
00125       if (downsample_)
00126         vgrid_.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
00127 
00128       seg_.setDistanceThreshold (sac_distance_);
00129       seg_.setMaxIterations (max_iter_);
00130       seg_.setNormalDistanceWeight (normal_distance_weight_);
00131       seg_.setOptimizeCoefficients (true);
00132       seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00133       seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
00134       seg_.setMethodType (pcl::SAC_RANSAC);
00135       seg_.setProbability (seg_prob_);
00136 
00137       proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00138       clusters_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
00139       clusters_tree_->setEpsilon (1);
00140       normals_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
00141 
00142       n3d_.setKSearch (k_);
00143       n3d_.setSearchMethod (normals_tree_);
00144       //action server
00145       as_.start();
00146       //needed for the synchronization of callbacks
00147       got_cluster_ = action_called_ = false;
00148       //plane prior (for seg_.setAxis)
00149       base_link_head_tilt_link_angle_ = 0.9;
00150     }
00151 
00153   virtual ~ExtractClusters () 
00154   {
00155     for (size_t i = 0; i < table_coeffs_.size (); ++i) 
00156       delete table_coeffs_[i];
00157   }    
00158 
00159   void 
00160   init (double tolerance, std::string object_name)  // tolerance: how close to (0,0) is good enough?
00161     {
00162       ROS_INFO ("[ExtractCluster:] Listening for incoming data on topic %s", nh_.resolveName (point_cloud_topic).c_str ());
00163       point_cloud_sub_ = nh_.subscribe (point_cloud_topic, 1,  &ExtractClusters::clustersCallback, this);
00164       object_name_ = object_name;
00165     }
00166   
00167   void add_remove_padding_hull (pcl::PointCloud<Point> &hull_in, pcl::PointCloud<Point> &hull_out, double padding)
00168     {
00169       //      hull_out.points.resize(hull_in.points.size());
00170       hull_out = hull_in;
00171       Point point_max, point_min, point_center;
00172       getMinMax3D (hull_in, point_min, point_max);
00173       //Calculate the centroid of the hull
00174       point_center.x = (point_max.x + point_min.x)/2;
00175       point_center.y = (point_max.y + point_min.y)/2;
00176       point_center.z = (point_max.z + point_min.z)/2;
00177 
00178       for (unsigned long i = 0; i < hull_in.points.size(); i++)
00179       {
00180         double dist_to_center = sqrt((point_center.x - hull_in.points[i].x) * (point_center.x - hull_in.points[i].x) +
00181                                      (point_center.y - hull_in.points[i].y) * (point_center.y - hull_in.points[i].y));
00182         ROS_INFO("[%s] Dist to center: %lf", getName().c_str (), dist_to_center);
00183         double angle;
00184         angle= atan2((hull_in.points[i].y - point_center.y), (hull_in.points[i].x - point_center.x));
00185         double new_dist_to_center = padding * dist_to_center;
00186         hull_out.points[i].y = point_center.y + sin(angle) * new_dist_to_center;
00187         hull_out.points[i].x = point_center.x + cos(angle) * new_dist_to_center;
00188       }
00189     }
00190     
00191   void executeCB(const pcl_cloud_tools::GetClustersGoalConstPtr &goal)
00192     {
00193       action_called_ = true;
00194       got_cluster_ = false;
00195       while (!got_cluster_)
00196       {
00197         ROS_INFO("[%s: ] Waiting to get cluster", getName().c_str ());
00198         sleep(1);    
00199       }
00200       bool success = true;
00201       // publish info to the console for the user
00202       ROS_INFO("[%s: ] Getting the cluster through action", getName().c_str ());
00203 
00204       // start executing the action
00205       // check that preempt has not been requested by the client
00206       if (as_.isPreemptRequested() || !ros::ok())
00207       {
00208         ROS_INFO("%s: Preempted", getName().c_str ());
00209         // set the action state to preempted
00210         as_.setPreempted();
00211         success = false;
00212       }
00213       
00214       pcl::PointXYZ point_min;
00215       pcl::PointXYZ point_max;
00216       ias_table_msgs::TableCluster table_cluster;
00217       result_.clusters.clear();
00218       for (uint i = 0; i < cloud_object_clustered_array_.size(); i++)
00219       {
00220         pcl::getMinMax3D (cloud_object_clustered_array_[i], point_min, point_max);
00221         
00222         //get the cluster
00223         table_cluster.header.stamp = ros::Time::now();
00224         table_cluster.header.frame_id = cloud_object_clustered_array_[i].header.frame_id;
00225         table_cluster.header.seq = 0;
00226 
00227         table_cluster.center.x = (point_max.x + point_min.x)/2;
00228         table_cluster.center.y = (point_max.y + point_min.y)/2;
00229         table_cluster.center.z = (point_max.z + point_min.z)/2;
00230         
00231         table_cluster.min_bound.x = point_min.x;
00232         table_cluster.min_bound.y = point_min.y;
00233         table_cluster.min_bound.z = point_min.z;
00234         
00235         table_cluster.max_bound.x = point_max.x;
00236         table_cluster.max_bound.y = point_max.y;
00237         table_cluster.max_bound.z = point_max.z;
00238         result_.clusters.push_back(table_cluster);
00239       // publish the feedback
00240       //as_.publishFeedback(feedback_);
00241       }
00242       if(success)
00243       {
00244 
00245         ROS_INFO("%s: Action Succeeded", getName().c_str ());
00246       // set the action state to succeeded
00247         as_.setSucceeded(result_);
00248       }
00249       got_cluster_ = false;
00250       action_called_ = false;
00251       cloud_object_clustered_array_.clear();
00252     }
00253 
00254 private:
00256   void 
00257   clustersCallback (const sensor_msgs::PointCloud2ConstPtr &cloud_in)
00258     {
00259       //get the transform between base_link and cloud frame
00260       bool found_transform = tf_listener_.waitForTransform("head_tilt_link", "base_link",
00261                                                            cloud_in->header.stamp, ros::Duration(1.0));
00262       tf::StampedTransform transform;
00263       if (found_transform)
00264       {
00265         tf_listener_.lookupTransform("head_tilt_link", "base_link", cloud_in->header.stamp, transform);
00266         double yaw, pitch, roll;
00267         transform.getBasis().getEulerZYX(yaw, pitch, roll);
00268         ROS_INFO("[ExtractCluster:] Transform X: %f Y: %f Z: %f R: %f P: %f Y: %f", transform.getOrigin().getX(), 
00269                  transform.getOrigin().getY(), transform.getOrigin().getZ(), roll, pitch, yaw);
00270         base_link_head_tilt_link_angle_ = pitch;
00271       }
00272       else
00273       {
00274         ROS_INFO("[ExtractCluster:] No transform found between %s and base_link", cloud_in->header.frame_id.c_str());
00275         return;
00276       }
00277       
00278       if (!got_cluster_ || !action_called_)
00279       {
00280         ROS_INFO_STREAM ("[" << getName ().c_str () << "] Received cloud: in frame " << cloud_in->header.frame_id);
00281       
00282         // Downsample + filter the input dataser
00283         PointCloud cloud_raw, cloud;
00284         pcl::fromROSMsg (*cloud_in, cloud_raw);
00285         vgrid_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
00286         vgrid_.filter (cloud);
00287         //cloud_pub_.publish(cloud);
00288         //return;
00289       
00290         // Fit a plane (the table)
00291         pcl::ModelCoefficients table_coeff;
00292         pcl::PointIndices table_inliers;
00293         PointCloud cloud_projected;
00294         pcl::PointCloud<Point> cloud_hull;
00295         // ---[ Estimate the point normals
00296         pcl::PointCloud<pcl::Normal> cloud_normals;
00297         n3d_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00298         n3d_.compute (cloud_normals);
00299         //cloud_pub_.publish(cloud_normals);
00300         //return;
00301         cloud_normals_.reset (new pcl::PointCloud<pcl::Normal> (cloud_normals));
00302       
00303         seg_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00304         seg_.setInputNormals (cloud_normals_);
00305         //z axis in Kinect frame
00306         btVector3 axis(0.0, 0.0, 1.0);
00307         //rotate axis around x in Kinect frame for an angle between base_link and head_tilt_link + 90deg
00308         //todo: get angle automatically
00309         btVector3 axis2 = axis.rotate(btVector3(1.0, 0.0, 0.0), btScalar(base_link_head_tilt_link_angle_ + pcl::deg2rad(90.0)));
00310         //std::cerr << "axis: " << fabs(axis2.getX()) << " " << fabs(axis2.getY()) << " " << fabs(axis2.getZ()) << std::endl;
00311         seg_.setAxis (Eigen::Vector3f(fabs(axis2.getX()), fabs(axis2.getY()), fabs(axis2.getZ())));
00312         // seg_.setIndices (boost::make_shared<pcl::PointIndices> (selection));
00313         seg_.segment (table_inliers, table_coeff);
00314         ROS_INFO ("[%s] Table model: [%f, %f, %f, %f] with %d inliers.", getName ().c_str (), 
00315                   table_coeff.values[0], table_coeff.values[1], table_coeff.values[2], table_coeff.values[3], (int)table_inliers.indices.size ());
00316         if ((int)table_inliers.indices.size () <= min_table_inliers_)
00317         {
00318           ROS_ERROR ("table has to few inliers");
00319           return;
00320         }
00321         // Project the table inliers using the planar model coefficients    
00322         proj_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00323         proj_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
00324         proj_.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients> (table_coeff));
00325         proj_.filter (cloud_projected);
00326         //cloud_pub_.publish (cloud_projected);
00327       
00328         // Create a Convex Hull representation of the projected inliers
00329         chull_.setInputCloud (boost::make_shared<PointCloud> (cloud_projected));
00330         chull_.reconstruct (cloud_hull);
00331         ROS_INFO ("Convex hull has: %d data points.", (int)cloud_hull.points.size ());
00332         cloud_pub_.publish (cloud_hull);
00333        
00334         //pcl::PointCloud<Point> cloud_hull_padded;
00335         //add_remove_padding_hull(cloud_hull, cloud_hull_padded, padding_);
00336         //ROS_INFO ("New Convex hull has: %d data points.", (int)cloud_hull_padded.points.size ());
00337         //sleep(2);
00338         //cloud_pub_.publish (cloud_hull_padded);
00339 
00340         // ---[ Get the objects on top of the table
00341         pcl::PointIndices cloud_object_indices;
00342         prism_.setHeightLimits (cluster_min_height_, cluster_max_height_);
00343         prism_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00344         prism_.setInputPlanarHull (boost::make_shared<PointCloud>(cloud_hull));
00345         prism_.segment (cloud_object_indices);
00346         //ROS_INFO ("[%s] Number of object point indices: %d.", getName ().c_str (), (int)cloud_object_indices.indices.size ());
00347       
00348         pcl::PointCloud<Point> cloud_object;
00349         pcl::ExtractIndices<Point> extract_object_indices;
00350         extract_object_indices.setInputCloud (boost::make_shared<PointCloud> (cloud));
00351         extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00352         extract_object_indices.filter (cloud_object);
00353         //ROS_INFO ("[%s ] Publishing number of object point candidates: %d.", getName ().c_str (), 
00354         //        (int)cloud_objects.points.size ());
00355       
00356       
00357         std::vector<pcl::PointIndices> clusters;
00358         cluster_.setInputCloud (boost::make_shared<PointCloud>(cloud_object));
00359         cluster_.setClusterTolerance (object_cluster_tolerance_);
00360         cluster_.setMinClusterSize (object_cluster_min_size_);
00361         cluster_.setSearchMethod (clusters_tree_);
00362         cluster_.extract (clusters);
00363         cloud_object_clustered_array_.clear();
00364         pcl::PointCloud<Point> cloud_object_clustered;
00365         if (int(clusters.size()) >= nr_cluster_)
00366         {
00367           for (int i = 0; i < nr_cluster_; i++)
00368           {
00369             pcl::copyPointCloud (cloud_object, clusters[i], cloud_object_clustered);
00370             char object_name_angle[100];
00371             if (save_to_files_)
00372             {
00373               sprintf (object_name_angle, "%04d",  i);
00374 //              pcd_writer_.write (object_name_ + "_" +  object_name_angle + ".pcd", cloud_object_clustered, true);
00375               std::stringstream ss;
00376               ss << cloud_in->header.stamp;
00377               ROS_INFO("Saving cluster to: %s_%s.pcd", object_name_.c_str(), ss.str().c_str());
00378               pcd_writer_.write (object_name_ + "_" +  ss.str () + ".pcd", cloud_object_clustered, true);
00379             }
00380             cloud_objects_pub_.publish (cloud_object_clustered);
00381             cloud_object_clustered_array_.push_back(cloud_object_clustered);
00382           }
00383         
00384         
00385           ROS_INFO("Published %d clusters in frame: %s", nr_cluster_, cloud_object_clustered.header.frame_id.c_str());
00386           // cloud_objects_pub_.publish (cloud_object_clustered);
00387           pcl::PointCloud<Point> token;
00388           Point p0;
00389           p0.x = p0.y = p0.z  = 100.0;
00390           token.width = 1;
00391           token.height = 1;
00392           token.is_dense = false;
00393           token.points.resize(token.width * token.height);
00394           token.points[0] = p0;
00395           token.header.frame_id = cloud_object_clustered.header.frame_id;
00396           token.header.stamp = ros::Time::now();
00397           if (publish_token_)
00398           {
00399             cloud_objects_pub_.publish (token);
00400             ROS_INFO("Published token cluster with size %d.", token.width * token.height);
00401           }
00402         }
00403         else
00404         {
00405           ROS_ERROR("Only %ld clusters found with size > %d points", clusters.size(), object_cluster_min_size_);
00406         }
00407         got_cluster_ = true;
00408       }
00409     }
00410 
00412 
00416   double
00417   compute2DPolygonalArea (PointCloud &points, const std::vector<double> &normal)
00418     {
00419       int k0, k1, k2;
00420     
00421       // Find axis with largest normal component and project onto perpendicular plane
00422       k0 = (fabs (normal.at (0) ) > fabs (normal.at (1))) ? 0  : 1;
00423       k0 = (fabs (normal.at (k0)) > fabs (normal.at (2))) ? k0 : 2;
00424       k1 = (k0 + 1) % 3;
00425       k2 = (k0 + 2) % 3;
00426  
00427       // cos(theta), where theta is the angle between the polygon and the projected plane
00428       double ct = fabs ( normal.at (k0) );
00429  
00430       double area = 0;
00431       float p_i[3], p_j[3];
00432 
00433       for (unsigned int i = 0; i < points.points.size (); i++)
00434       {
00435         p_i[0] = points.points[i].x; p_i[1] = points.points[i].y; p_i[2] = points.points[i].z;
00436         int j = (i + 1) % points.points.size ();
00437         p_j[0] = points.points[j].x; p_j[1] = points.points[j].y; p_j[2] = points.points[j].z;
00438 
00439         area += p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];
00440       }
00441       area = fabs (area) / (2 * ct);
00442 
00443       return (area);
00444     }
00445 
00446   ros::NodeHandle nh_;  // Do we need to keep it?
00447   tf::TransformBroadcaster transform_broadcaster_;
00448   tf::TransformListener tf_listener_;
00449   bool save_to_files_, downsample_, publish_token_, got_cluster_, action_called_;
00450 
00451   double normal_search_radius_;
00452   double voxel_size_;
00453   double padding_;
00454 
00455   std::string rot_table_frame_, object_name_, point_cloud_topic;
00456   double object_cluster_tolerance_,  cluster_min_height_, cluster_max_height_;
00457   int object_cluster_min_size_, object_cluster_max_size_;
00458 
00459   pcl::PCDWriter pcd_writer_;
00460   double sac_distance_, normal_distance_weight_, z_min_limit_, z_max_limit_;
00461   double eps_angle_, seg_prob_, base_link_head_tilt_link_angle_;
00462   int k_, max_iter_, min_table_inliers_, nr_cluster_;
00463   
00464   ros::Subscriber point_cloud_sub_;
00465 
00466   std::vector<Eigen::Vector4d *> table_coeffs_;
00467 
00468   pcl_ros::Publisher<Point> cloud_pub_;
00469   //  pcl_ros::Publisher<Point> cloud_extracted_pub_;
00470   pcl_ros::Publisher<Point> cloud_objects_pub_;
00471   pcl_ros::Publisher<Point> token_pub_;
00472 
00473   // PCL objects
00474   //pcl::PassThrough<Point> vgrid_;                   // Filtering + downsampling object
00475   pcl::VoxelGrid<Point> vgrid_;                   // Filtering + downsampling object
00476   pcl::NormalEstimation<Point, pcl::Normal> n3d_;   //Normal estimation
00477   // The resultant estimated point cloud normals for \a cloud_filtered_
00478   pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_;
00479   pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;               // Planar segmentation object
00480   pcl::ProjectInliers<Point> proj_;               // Inlier projection object
00481   pcl::ExtractIndices<Point> extract_;            // Extract (too) big tables
00482   pcl::ConvexHull<Point> chull_;  
00483   pcl::ExtractPolygonalPrismData<Point> prism_;
00484   pcl::PointCloud<Point> cloud_objects_;
00485   pcl::EuclideanClusterExtraction<Point> cluster_;
00486   KdTreePtr clusters_tree_, normals_tree_;
00487   std::vector<pcl::PointCloud<Point> >cloud_object_clustered_array_;
00488 
00490 
00491   std::string getName () const { return ("ExtractClusters"); }
00492 
00493   //action related objects
00494   actionlib::SimpleActionServer<pcl_cloud_tools::GetClustersAction> as_;
00495   std::string action_name_;
00496   // create messages that are used to published feedback/result
00497   pcl_cloud_tools::GetClustersFeedback feedback_;
00498   pcl_cloud_tools::GetClustersResult result_;
00499 };
00500 
00501 /* ---[ */
00502 int
00503 main (int argc, char** argv)
00504 {
00505   ros::init (argc, argv, "extract_clusters");
00506   ros::NodeHandle nh("~");
00507   if (argc < 2)
00508   {
00509     ROS_ERROR ("usage %s <object_name>", argv[0]);
00510     exit(2);
00511   }
00512   std::string object_name = argv[1];
00513   ExtractClusters clusters (nh);
00514   clusters.init (5, object_name);  // 5 degrees tolerance
00515   ros::spin ();
00516 }
00517 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_tools
Author(s): Nico Blodow, Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Thu May 23 2013 17:11:36