extract_clusters.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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  * $Id: extract_clusters.hpp 32052 2010-08-27 02:19:30Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include <pcl/io/io.h>
00040 #include <pcl/PointIndices.h>
00041 #include "pcl_ros/segmentation/extract_clusters.h"
00042 
00043 #include <pcl_conversions/pcl_conversions.h>
00044 
00045 using pcl_conversions::fromPCL;
00046 using pcl_conversions::moveFromPCL;
00047 using pcl_conversions::toPCL;
00048 
00050 void
00051 pcl_ros::EuclideanClusterExtraction::onInit ()
00052 {
00053   // Call the super onInit ()
00054   PCLNodelet::onInit ();
00055 
00056   // ---[ Mandatory parameters
00057   double cluster_tolerance;
00058   if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
00059   {
00060     NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ()); 
00061     return;
00062   }
00063   int spatial_locator;
00064   if (!pnh_->getParam ("spatial_locator", spatial_locator))
00065   {
00066     NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00067     return;
00068   }
00069 
00070   //private_nh.getParam ("use_indices", use_indices_);
00071   pnh_->getParam ("publish_indices", publish_indices_);
00072 
00073   if (publish_indices_)
00074     pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
00075   else
00076     pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
00077 
00078   // Enable the dynamic reconfigure service
00079   srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
00080   dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f =  boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
00081   srv_->setCallback (f);
00082 
00083   // If we're supposed to look for PointIndices (indices)
00084   if (use_indices_)
00085   {
00086     // Subscribe to the input using a filter
00087     sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00088     sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00089 
00090     if (approximate_sync_)
00091     {
00092       sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
00093       sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00094       sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
00095     }
00096     else
00097     {
00098       sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
00099       sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00100       sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
00101     }
00102   }
00103   else
00104     // Subscribe in an old fashion to input only (no filters)
00105     sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
00106 
00107   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00108                  " - max_queue_size    : %d\n"
00109                  " - use_indices       : %s\n"
00110                  " - cluster_tolerance : %f\n",
00111                  getName ().c_str (),
00112                  max_queue_size_,
00113                  (use_indices_) ? "true" : "false", cluster_tolerance);
00114 
00115   // Set given parameters here
00116   impl_.setClusterTolerance (cluster_tolerance);
00117 }
00118 
00120 void
00121 pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level)
00122 {
00123   if (impl_.getClusterTolerance () != config.cluster_tolerance)
00124   {
00125     impl_.setClusterTolerance (config.cluster_tolerance);
00126     NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
00127   }
00128   if (impl_.getMinClusterSize () != config.cluster_min_size)
00129   {
00130     impl_.setMinClusterSize (config.cluster_min_size);
00131     NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
00132   }
00133   if (impl_.getMaxClusterSize () != config.cluster_max_size)
00134   {
00135     impl_.setMaxClusterSize (config.cluster_max_size);
00136     NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
00137   }
00138   if (max_clusters_ != config.max_clusters)
00139   {
00140     max_clusters_ = config.max_clusters;
00141     NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
00142   }
00143 }
00144 
00146 void
00147 pcl_ros::EuclideanClusterExtraction::input_indices_callback (
00148       const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
00149 {
00150   // No subscribers, no work
00151   if (pub_output_.getNumSubscribers () <= 0)
00152     return;
00153 
00154   // If cloud is given, check if it's valid
00155   if (!isValid (cloud))
00156   {
00157     NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00158     return;
00159   }
00160   // If indices are given, check if they are valid
00161   if (indices && !isValid (indices))
00162   {
00163     NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00164     return;
00165   }
00166 
00168   if (indices) {
00169     std_msgs::Header cloud_header = fromPCL(cloud->header);
00170     std_msgs::Header indices_header = indices->header;
00171     NODELET_DEBUG ("[%s::input_indices_callback]\n"
00172                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00173                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00174                    getName ().c_str (),
00175                    cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00176                    indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00177   } else {
00178     NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
00179   }
00181 
00182   IndicesPtr indices_ptr;
00183   if (indices)
00184     indices_ptr.reset (new std::vector<int> (indices->indices));
00185 
00186   impl_.setInputCloud (cloud);
00187   impl_.setIndices (indices_ptr);
00188 
00189   std::vector<pcl::PointIndices> clusters;
00190   impl_.extract (clusters);
00191 
00192   if (publish_indices_)
00193   {
00194     for (size_t i = 0; i < clusters.size (); ++i)
00195     {
00196       if ((int)i >= max_clusters_)
00197         break;
00198       // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
00199       pcl_msgs::PointIndices ros_pi;
00200       moveFromPCL(clusters[i], ros_pi);
00201       ros_pi.header.stamp += ros::Duration (i * 0.001);
00202       pub_output_.publish (ros_pi);
00203     }
00204 
00205     NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
00206   }
00207   else
00208   {
00209     for (size_t i = 0; i < clusters.size (); ++i)
00210     {
00211       if ((int)i >= max_clusters_)
00212         break;
00213       PointCloud output;
00214       copyPointCloud (*cloud, clusters[i].indices, output);
00215 
00216       //PointCloud output_blob;     // Convert from the templated output to the PointCloud blob
00217       //pcl::toROSMsg (output, output_blob);
00218       // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
00219       std_msgs::Header header = fromPCL(output.header);
00220       header.stamp += ros::Duration (i * 0.001);
00221       toPCL(header, output.header);
00222       // Publish a Boost shared ptr const data
00223       pub_output_.publish (output.makeShared ());
00224       NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
00225                      i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
00226     }
00227   }
00228 }
00229 
00230 typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction;
00231 PLUGINLIB_DECLARE_CLASS (pcl, EuclideanClusterExtraction, EuclideanClusterExtraction, nodelet::Nodelet);
00232 


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Aug 26 2015 15:25:30