extract_clusters_on_table_server.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 #include "pcl/common/common.h"
00073 
00074 #include "pcl_cloud_tools/GetClusters.h"
00075 typedef pcl::PointXYZ Point;
00076 typedef pcl::PointCloud<Point> PointCloud;
00077 typedef PointCloud::Ptr PointCloudPtr;
00078 typedef PointCloud::ConstPtr PointCloudConstPtr;
00079 typedef pcl::search::KdTree<Point>::Ptr KdTreePtr;
00080 
00081 const tf::Vector3 wp_normal(1, 0, 0);
00082 const double wp_offset = -1.45;
00083 
00084 class ExtractClustersServer
00085 {
00086 public:
00088   ExtractClustersServer (const ros::NodeHandle &nh) : nh_ (nh)
00089     {
00090       nh_.param("sac_distance", sac_distance_, 0.03);
00091       nh_.param("z_min_limit", z_min_limit_, 0.3);
00092       nh_.param("z_max_limit", z_max_limit_, 1.5);
00093       nh_.param("max_iter", max_iter_, 500);
00094       nh_.param("normal_distance_weight", normal_distance_weight_, 0.1);
00095       nh_.param("eps_angle", eps_angle_, 20.0);
00096       nh_.param("seg_prob", seg_prob_, 0.99);
00097       nh_.param("normal_search_radius", normal_search_radius_, 0.05);
00098       //what area size of the table are we looking for?
00099       nh_.param("rot_table_frame", rot_table_frame_, std::string("rotating_table"));
00100       nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.03);
00101       //min 100 points
00102       nh_.param("object_cluster_min_size", object_cluster_min_size_, 100);
00103       nh_.param("k", k_, 10);
00104       nh_.param("base_link_head_tilt_link_angle", base_link_head_tilt_link_angle_, 0.8);
00105       nh_.param("min_table_inliers", min_table_inliers_, 100);
00106       nh_.param("cluster_min_height", cluster_min_height_, 0.02);
00107       nh_.param("cluster_max_height", cluster_max_height_, 0.4);
00108       nh_.param("nr_cluster", nr_cluster_, 1);
00109       nh_.param("downsample", downsample_, true);
00110       nh_.param("voxel_size", voxel_size_, 0.01);
00111       nh_.param("save_to_files", save_to_files_, false);
00112       nh_.param("point_cloud_topic", point_cloud_topic, std::string("/narrow_stereo_textured/points2"));
00113       nh_.param("publish_token", publish_token_, false);
00114       nh_.param("padding", padding_, 0.85);
00115       nh_.param("target_frame", target_frame_, std::string("base_link"));
00116 
00117       service_ = nh_.advertiseService("cluster_tracking", &ExtractClustersServer::clustersCallback, this);
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       //plane prior (for seg_.setAxis)
00145       //      base_link_head_tilt_link_angle_ = 0.9;
00146     }
00147 
00149   virtual ~ExtractClustersServer () 
00150   {
00151     for (size_t i = 0; i < table_coeffs_.size (); ++i) 
00152       delete table_coeffs_[i];
00153   }    
00154 
00155 private:
00157   bool 
00158   clustersCallback (pcl_cloud_tools::GetClusters::Request & req,
00159                     pcl_cloud_tools::GetClusters::Response & res)
00160     {
00161       //sensor_msgs::PointCloud2ConstPtr cloud_in;
00162           sensor_msgs::PointCloud2 cloud_in;
00163       cloud_in = req.input_cloud;
00164       //ros::topic::waitForMessage<sensor_msgs::PointCloud2>(point_cloud_topic,
00165                                                      //  ros::Duration(5.0));
00166       if (cloud_in.width == 0)
00167         {
00168           res.result = false;
00169           return false;
00170         }
00171 
00172       //get the transform between base_link and cloud frame
00173       bool found_transform = tf_listener_.waitForTransform(cloud_in.header.frame_id.c_str(), target_frame_,
00174                                                            cloud_in.header.stamp, ros::Duration(1.0));
00175       tf::StampedTransform transform;
00176       if (found_transform)
00177       {
00178         tf_listener_.lookupTransform(cloud_in.header.frame_id.c_str(), target_frame_, cloud_in.header.stamp, transform);
00179         double yaw, pitch, roll;
00180         transform.getBasis().getEulerZYX(yaw, pitch, roll);
00181         ROS_INFO("[ExtractCluster:] Transform X: %f Y: %f Z: %f R: %f P: %f Y: %f", transform.getOrigin().getX(), 
00182                  transform.getOrigin().getY(), transform.getOrigin().getZ(), roll, pitch, yaw);
00183         base_link_head_tilt_link_angle_ = pitch;
00184       }
00185       else
00186       {
00187         ROS_WARN("[ExtractCluster:] No transform found between %s and %s", cloud_in.header.frame_id.c_str(), target_frame_.c_str());
00188         ROS_WARN("[ExtractCluster:] Taking the following base_link_head_tilt_link_angle: %f", base_link_head_tilt_link_angle_);
00189       }
00190       
00191         ROS_INFO_STREAM ("[" << getName ().c_str () << "] Received cloud: in frame " << cloud_in.header.frame_id);
00192       
00193         // Downsample + filter the input dataser
00194         PointCloud cloud_raw, cloud;
00195         pcl::fromROSMsg (cloud_in, cloud_raw);
00196         vgrid_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
00197         vgrid_.filter (cloud);
00198         //cloud_pub_.publish(cloud);
00199         //return;
00200       
00201         // Fit a plane (the table)
00202         pcl::ModelCoefficients table_coeff;
00203         pcl::PointIndices table_inliers;
00204         PointCloud cloud_projected;
00205         pcl::PointCloud<Point> cloud_hull;
00206         // ---[ Estimate the point normals
00207         pcl::PointCloud<pcl::Normal> cloud_normals;
00208         n3d_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00209         n3d_.compute (cloud_normals);
00210         //cloud_pub_.publish(cloud_normals);
00211         //return;
00212         cloud_normals_.reset (new pcl::PointCloud<pcl::Normal> (cloud_normals));
00213       
00214         seg_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00215         seg_.setInputNormals (cloud_normals_);
00216         //z axis in Kinect frame
00217         btVector3 axis(0.0, 0.0, 1.0);
00218         //rotate axis around x in Kinect frame for an angle between base_link and head_tilt_link + 90deg
00219         //todo: get angle automatically
00220         btVector3 axis2 = axis.rotate(btVector3(1.0, 0.0, 0.0), btScalar(base_link_head_tilt_link_angle_ + pcl::deg2rad(90.0)));
00221         //std::cerr << "axis: " << fabs(axis2.getX()) << " " << fabs(axis2.getY()) << " " << fabs(axis2.getZ()) << std::endl;
00222         seg_.setAxis (Eigen::Vector3f(fabs(axis2.getX()), fabs(axis2.getY()), fabs(axis2.getZ())));
00223         // seg_.setIndices (boost::make_shared<pcl::PointIndices> (selection));
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], (int)table_inliers.indices.size ());
00227         if ((int)table_inliers.indices.size () <= min_table_inliers_)
00228         {
00229           ROS_ERROR ("table has to few inliers");
00230           res.result = false;
00231           return false;
00232         }
00233         // Project the table inliers using the planar model coefficients    
00234         proj_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00235         proj_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
00236         proj_.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients> (table_coeff));
00237         proj_.filter (cloud_projected);
00238         //cloud_pub_.publish (cloud_projected);
00239       
00240         // Create a Convex Hull representation of the projected inliers
00241         chull_.setInputCloud (boost::make_shared<PointCloud> (cloud_projected));
00242         chull_.reconstruct (cloud_hull);
00243         ROS_INFO ("Convex hull has: %d data points.", (int)cloud_hull.points.size ());
00244         cloud_pub_.publish (cloud_hull);
00245        
00246         //pcl::PointCloud<Point> cloud_hull_padded;
00247         //add_remove_padding_hull(cloud_hull, cloud_hull_padded, padding_);
00248         //ROS_INFO ("New Convex hull has: %d data points.", (int)cloud_hull_padded.points.size ());
00249         //sleep(2);
00250         //cloud_pub_.publish (cloud_hull_padded);
00251 
00252         // ---[ Get the objects on top of the table
00253         pcl::PointIndices cloud_object_indices;
00254         prism_.setHeightLimits (cluster_min_height_, cluster_max_height_);
00255         prism_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
00256         prism_.setInputPlanarHull (boost::make_shared<PointCloud>(cloud_hull));
00257         prism_.segment (cloud_object_indices);
00258         //ROS_INFO ("[%s] Number of object point indices: %d.", getName ().c_str (), (int)cloud_object_indices.indices.size ());
00259       
00260         //pcl::PointCloud<Point> cloud_object;
00261         //pcl::ExtractIndices<Point> extract_object_indices;
00262         //extract_object_indices.setInputCloud (boost::make_shared<PointCloud> (cloud));
00263         //extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00264         //extract_object_indices.filter (cloud_object);
00265         //ROS_INFO ("[%s ] Publishing number of object point candidates: %d.", getName ().c_str (), 
00266         //        (int)cloud_objects.points.size ());
00267       
00268       
00269         std::vector<pcl::PointIndices> clusters;
00270         cluster_.setInputCloud (boost::make_shared<PointCloud>(cloud_raw));
00271         cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00272         cluster_.setClusterTolerance (object_cluster_tolerance_);
00273         cluster_.setMinClusterSize (object_cluster_min_size_);
00274         cluster_.setSearchMethod (clusters_tree_);
00275         cluster_.extract (clusters);
00276 
00277         res.clusters_indices.clear();
00278 
00279         if (clusters.size() > 0)
00280         {
00281                         res.clusters_indices = clusters;
00282                         res.result = true;
00283         }
00284         else
00285         {
00286                 res.result = false;
00287         }
00288 
00289 //        pcl::PointCloud<Point> cloud_object_clustered;
00290 //        if (int(clusters.size()) >= 0)
00291 //        {
00292 //          for (int i = 0; i < clusters.size(); i++)
00293 //          {
00294 //            pcl::copyPointCloud (cloud_object, clusters[i], cloud_object_clustered);
00295 //            cloud_objects_pub_.publish (cloud_object_clustered);
00296 //            sensor_msgs::PointCloud2 cluster;
00297 //            pcl::toROSMsg (cloud_object_clustered, cluster);
00298 //            res.clusters.push_back(cluster);
00299 //          }
00300 //          res.result = true;
00301 //        }
00302 //        else
00303 //        {
00304 //          ROS_ERROR("Only %ld clusters found with size > %d points",
00305 //                    clusters.size(), object_cluster_min_size_);
00306 //          res.result = false;
00307 //        }
00308         return true;
00309     }
00310 
00311   ros::NodeHandle nh_;  // Do we need to keep it?
00312   tf::TransformBroadcaster transform_broadcaster_;
00313   tf::TransformListener tf_listener_;
00314   bool save_to_files_, downsample_, publish_token_, got_cluster_, action_called_;
00315 
00316   double normal_search_radius_;
00317   double voxel_size_;
00318   double padding_;
00319 
00320   std::string rot_table_frame_, object_name_, point_cloud_topic, target_frame_;
00321   double object_cluster_tolerance_,  cluster_min_height_, cluster_max_height_;
00322   int object_cluster_min_size_, object_cluster_max_size_;
00323 
00324   pcl::PCDWriter pcd_writer_;
00325   double sac_distance_, normal_distance_weight_, z_min_limit_, z_max_limit_;
00326   double eps_angle_, seg_prob_, base_link_head_tilt_link_angle_;
00327   int k_, max_iter_, min_table_inliers_, nr_cluster_;
00328   
00329   ros::Subscriber point_cloud_sub_;
00330 
00331   std::vector<Eigen::Vector4d *> table_coeffs_;
00332 
00333   pcl_ros::Publisher<Point> cloud_pub_;
00334   //  pcl_ros::Publisher<Point> cloud_extracted_pub_;
00335   pcl_ros::Publisher<Point> cloud_objects_pub_;
00336   pcl_ros::Publisher<Point> token_pub_;
00337 
00338   // PCL objects
00339   //pcl::PassThrough<Point> vgrid_;                   // Filtering + downsampling object
00340   pcl::VoxelGrid<Point> vgrid_;                   // Filtering + downsampling object
00341   pcl::NormalEstimation<Point, pcl::Normal> n3d_;   //Normal estimation
00342   // The resultant estimated point cloud normals for \a cloud_filtered_
00343   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_;
00344   pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;               // Planar segmentation object
00345   pcl::ProjectInliers<Point> proj_;               // Inlier projection object
00346   pcl::ExtractIndices<Point> extract_;            // Extract (too) big tables
00347   pcl::ConvexHull<Point> chull_;  
00348   pcl::ExtractPolygonalPrismData<Point> prism_;
00349   pcl::PointCloud<Point> cloud_objects_;
00350   pcl::EuclideanClusterExtraction<Point> cluster_;
00351   KdTreePtr clusters_tree_, normals_tree_;
00352 
00354 
00355   std::string getName () const { return ("ExtractClustersServer"); }
00356 
00357   ros::ServiceServer service_;
00358 };
00359 
00360 /* ---[ */
00361 int
00362 main (int argc, char** argv)
00363 {
00364   ros::init (argc, argv, "extract_clusters_server");
00365   ros::NodeHandle nh("~");
00366   ExtractClustersServer clusters (nh);
00367   ros::spin ();
00368 }
00369 /* ]--- */
 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