extract_clusters.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: extract_clusters.hpp 32052 2010-08-27 02:19:30Z rusu $
35  *
36  */
37 
39 #include <pcl/io/io.h>
40 #include <pcl/PointIndices.h>
42 
44 
48 
50 void
52 {
53  // Call the super onInit ()
55 
56  // ---[ Mandatory parameters
57  double cluster_tolerance;
58  if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
59  {
60  NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ());
61  return;
62  }
63  int spatial_locator;
64  if (!pnh_->getParam ("spatial_locator", spatial_locator))
65  {
66  NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
67  return;
68  }
69 
70  //private_nh.getParam ("use_indices", use_indices_);
71  pnh_->getParam ("publish_indices", publish_indices_);
72 
73  if (publish_indices_)
74  pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
75  else
76  pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
77 
78  // Enable the dynamic reconfigure service
79  srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
80  dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
81  srv_->setCallback (f);
82 
83  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
84  " - max_queue_size : %d\n"
85  " - use_indices : %s\n"
86  " - cluster_tolerance : %f\n",
87  getName ().c_str (),
89  (use_indices_) ? "true" : "false", cluster_tolerance);
90 
91  // Set given parameters here
92  impl_.setClusterTolerance (cluster_tolerance);
93 
95 }
96 
98 void
100 {
101  // If we're supposed to look for PointIndices (indices)
102  if (use_indices_)
103  {
104  // Subscribe to the input using a filter
107 
108  if (approximate_sync_)
109  {
110  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
112  sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
113  }
114  else
115  {
116  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
118  sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
119  }
120  }
121  else
122  // Subscribe in an old fashion to input only (no filters)
124 }
125 
127 void
129 {
130  if (use_indices_)
131  {
134  }
135  else
136  sub_input_.shutdown ();
137 }
138 
140 void
141 pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level)
142 {
143  if (impl_.getClusterTolerance () != config.cluster_tolerance)
144  {
145  impl_.setClusterTolerance (config.cluster_tolerance);
146  NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
147  }
148  if (impl_.getMinClusterSize () != config.cluster_min_size)
149  {
150  impl_.setMinClusterSize (config.cluster_min_size);
151  NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
152  }
153  if (impl_.getMaxClusterSize () != config.cluster_max_size)
154  {
155  impl_.setMaxClusterSize (config.cluster_max_size);
156  NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
157  }
158  if (max_clusters_ != config.max_clusters)
159  {
160  max_clusters_ = config.max_clusters;
161  NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
162  }
163 }
164 
166 void
168  const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
169 {
170  // No subscribers, no work
171  if (pub_output_.getNumSubscribers () <= 0)
172  return;
173 
174  // If cloud is given, check if it's valid
175  if (!isValid (cloud))
176  {
177  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
178  return;
179  }
180  // If indices are given, check if they are valid
181  if (indices && !isValid (indices))
182  {
183  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
184  return;
185  }
186 
188  if (indices) {
189  std_msgs::Header cloud_header = fromPCL(cloud->header);
190  std_msgs::Header indices_header = indices->header;
191  NODELET_DEBUG ("[%s::input_indices_callback]\n"
192  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
193  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
194  getName ().c_str (),
195  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
196  indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
197  } else {
198  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 ());
199  }
201 
202  IndicesPtr indices_ptr;
203  if (indices)
204  indices_ptr.reset (new std::vector<int> (indices->indices));
205 
206  impl_.setInputCloud (cloud);
207  impl_.setIndices (indices_ptr);
208 
209  std::vector<pcl::PointIndices> clusters;
210  impl_.extract (clusters);
211 
212  if (publish_indices_)
213  {
214  for (size_t i = 0; i < clusters.size (); ++i)
215  {
216  if ((int)i >= max_clusters_)
217  break;
218  // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
219  pcl_msgs::PointIndices ros_pi;
220  moveFromPCL(clusters[i], ros_pi);
221  ros_pi.header.stamp += ros::Duration (i * 0.001);
222  pub_output_.publish (ros_pi);
223  }
224 
225  NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
226  }
227  else
228  {
229  for (size_t i = 0; i < clusters.size (); ++i)
230  {
231  if ((int)i >= max_clusters_)
232  break;
233  PointCloud output;
234  copyPointCloud (*cloud, clusters[i].indices, output);
235 
236  //PointCloud output_blob; // Convert from the templated output to the PointCloud blob
237  //pcl::toROSMsg (output, output_blob);
238  // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
239  std_msgs::Header header = fromPCL(output.header);
240  header.stamp += ros::Duration (i * 0.001);
241  toPCL(header, output.header);
242  // Publish a Boost shared ptr const data
243  pub_output_.publish (output.makeShared ());
244  NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
245  i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
246  }
247  }
248 }
249 
252 
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
bool publish_indices_
Publish indices or convert to PointCloud clusters. Default: false.
#define NODELET_ERROR(...)
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:203
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< dynamic_reconfigure::Server< EuclideanClusterExtractionConfig > > srv_
Pointer to a dynamic reconfigure service.
f
const std::string & getName() const
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Definition: pcl_nodelet.h:121
void subscribe()
LazyNodelet connection routine.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud, PointIndices > > > sync_input_indices_a_
bool isValid(const PointCloud2::ConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
Definition: pcl_nodelet.h:140
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud, PointIndices > > > sync_input_indices_e_
Synchronized input, and indices.
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
boost::shared_ptr< ros::NodeHandle > pnh_
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
int max_clusters_
Maximum number of clusters to publish.
EuclideanClusterExtraction()
Empty constructor.
pcl::PointCloud< pcl::PointXYZ > PointCloud
Definition: pcl_nodelet.h:77
ros::Subscriber sub_input_
The input PointCloud subscriber.
PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet)
void onInit()
Nodelet initialization routine.
PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_nodelet.h:79
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Definition: pcl_nodelet.h:130
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:127
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:124
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
pcl::EuclideanClusterExtraction< pcl::PointXYZ > impl_
The PCL implementation used.
void input_indices_callback(const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
Input point cloud callback.
#define NODELET_DEBUG(...)
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:94
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:83
void config_callback(EuclideanClusterExtractionConfig &config, uint32_t level)
Dynamic reconfigure callback.
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.
Definition: pcl_nodelet.h:118


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Mon Jun 10 2019 14:19:18