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/PointIndices.h>
41 
43 
47 
49 void
51 {
52  // Call the super onInit ()
54 
55  // ---[ Mandatory parameters
56  double cluster_tolerance;
57  if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
58  {
59  NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ());
60  return;
61  }
62  int spatial_locator;
63  if (!pnh_->getParam ("spatial_locator", spatial_locator))
64  {
65  NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
66  return;
67  }
68 
69  //private_nh.getParam ("use_indices", use_indices_);
70  pnh_->getParam ("publish_indices", publish_indices_);
71 
72  if (publish_indices_)
73  pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
74  else
75  pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
76 
77  // Enable the dynamic reconfigure service
78  srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
79  dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
80  srv_->setCallback (f);
81 
82  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
83  " - max_queue_size : %d\n"
84  " - use_indices : %s\n"
85  " - cluster_tolerance : %f\n",
86  getName ().c_str (),
88  (use_indices_) ? "true" : "false", cluster_tolerance);
89 
90  // Set given parameters here
91  impl_.setClusterTolerance (cluster_tolerance);
92 
94 }
95 
97 void
99 {
100  // If we're supposed to look for PointIndices (indices)
101  if (use_indices_)
102  {
103  // Subscribe to the input using a filter
104  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
105  sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
106 
107  if (approximate_sync_)
108  {
109  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
110  sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
111  sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
112  }
113  else
114  {
115  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
116  sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
117  sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
118  }
119  }
120  else
121  // Subscribe in an old fashion to input only (no filters)
122  sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
123 }
124 
126 void
128 {
129  if (use_indices_)
130  {
131  sub_input_filter_.unsubscribe ();
132  sub_indices_filter_.unsubscribe ();
133  }
134  else
135  sub_input_.shutdown ();
136 }
137 
139 void
140 pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t /*level*/)
141 {
142  if (impl_.getClusterTolerance () != config.cluster_tolerance)
143  {
144  impl_.setClusterTolerance (config.cluster_tolerance);
145  NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
146  }
147  if (impl_.getMinClusterSize () != config.cluster_min_size)
148  {
149  impl_.setMinClusterSize (config.cluster_min_size);
150  NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
151  }
152  if (impl_.getMaxClusterSize () != config.cluster_max_size)
153  {
154  impl_.setMaxClusterSize (config.cluster_max_size);
155  NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
156  }
157  if (max_clusters_ != config.max_clusters)
158  {
159  max_clusters_ = config.max_clusters;
160  NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
161  }
162 }
163 
165 void
167  const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
168 {
169  // No subscribers, no work
170  if (pub_output_.getNumSubscribers () <= 0)
171  return;
172 
173  // If cloud is given, check if it's valid
174  if (!isValid (cloud))
175  {
176  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
177  return;
178  }
179  // If indices are given, check if they are valid
180  if (indices && !isValid (indices))
181  {
182  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
183  return;
184  }
185 
187  if (indices) {
188  std_msgs::Header cloud_header = fromPCL(cloud->header);
189  std_msgs::Header indices_header = indices->header;
190  NODELET_DEBUG ("[%s::input_indices_callback]\n"
191  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
192  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
193  getName ().c_str (),
194  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
195  indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
196  } else {
197  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 ());
198  }
200 
201  IndicesPtr indices_ptr;
202  if (indices)
203  indices_ptr.reset (new std::vector<int> (indices->indices));
204 
205  impl_.setInputCloud (pcl_ptr(cloud));
206  impl_.setIndices (indices_ptr);
207 
208  std::vector<pcl::PointIndices> clusters;
209  impl_.extract (clusters);
210 
211  if (publish_indices_)
212  {
213  for (size_t i = 0; i < clusters.size (); ++i)
214  {
215  if ((int)i >= max_clusters_)
216  break;
217  // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
218  pcl_msgs::PointIndices ros_pi;
219  moveFromPCL(clusters[i], ros_pi);
220  ros_pi.header.stamp += ros::Duration (i * 0.001);
221  pub_output_.publish (ros_pi);
222  }
223 
224  NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
225  }
226  else
227  {
228  for (size_t i = 0; i < clusters.size (); ++i)
229  {
230  if ((int)i >= max_clusters_)
231  break;
232  PointCloud output;
233  copyPointCloud (*cloud, clusters[i].indices, output);
234 
235  //PointCloud output_blob; // Convert from the templated output to the PointCloud blob
236  //pcl::toROSMsg (output, output_blob);
237  // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
238  std_msgs::Header header = fromPCL(output.header);
239  header.stamp += ros::Duration (i * 0.001);
240  toPCL(header, output.header);
241  // Publish a Boost shared ptr const data
242  pub_output_.publish (ros_ptr(output.makeShared ()));
243  NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
244  i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
245  }
246  }
247 }
248 
251 
pcl_ros::EuclideanClusterExtraction::publish_indices_
bool publish_indices_
Publish indices or convert to PointCloud clusters. Default: false.
Definition: extract_clusters.h:61
pcl_ros::EuclideanClusterExtraction
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
Definition: extract_clusters.h:57
extract_clusters.h
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet_topic_tools::NodeletLazy::pnh_
boost::shared_ptr< ros::NodeHandle > pnh_
pcl_ros::EuclideanClusterExtraction::onInit
void onInit()
Nodelet initialization routine.
Definition: extract_clusters.cpp:50
boost::shared_ptr
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
pcl_ros::EuclideanClusterExtraction::unsubscribe
void unsubscribe()
Definition: extract_clusters.cpp:127
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::PCLNodelet::use_indices_
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:95
pcl_conversions::toPCL
void toPCL(const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
pcl_ros::EuclideanClusterExtraction::impl_
pcl::EuclideanClusterExtraction< pcl::PointXYZ > impl_
The PCL implementation used.
Definition: extract_clusters.h:95
pcl_ros::EuclideanClusterExtraction::subscribe
void subscribe()
LazyNodelet connection routine.
Definition: extract_clusters.cpp:98
pcl_ros::EuclideanClusterExtraction::config_callback
void config_callback(EuclideanClusterExtractionConfig &config, uint32_t level)
Dynamic reconfigure callback.
Definition: extract_clusters.cpp:140
pcl_ros::PCLNodelet::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
f
f
pcl_ros::PCLNodelet::PointCloud
pcl::PointCloud< pcl::PointXYZ > PointCloud
Definition: pcl_nodelet.h:78
pcl_conversions::fromPCL
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
pcl_ros::PCLNodelet::max_queue_size_
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:128
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
pcl_ros::PCLNodelet::IndicesPtr
pcl::IndicesPtr IndicesPtr
Definition: pcl_nodelet.h:90
pcl_ros::EuclideanClusterExtraction::srv_
boost::shared_ptr< dynamic_reconfigure::Server< EuclideanClusterExtractionConfig > > srv_
Pointer to a dynamic reconfigure service.
Definition: extract_clusters.h:72
pcl_ros::PCLNodelet::pub_output_
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:125
pcl_ros::PCLNodelet::onInit
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:204
pcl_ros::EuclideanClusterExtraction::input_indices_callback
void input_indices_callback(const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
Input point cloud callback.
Definition: extract_clusters.cpp:166
nodelet::Nodelet
nodelet_topic_tools::NodeletLazy::onInitPostProcess
virtual void onInitPostProcess()
pcl::pcl_ptr
boost::shared_ptr< T > pcl_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:392
EuclideanClusterExtraction
pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction
Definition: extract_clusters.cpp:249
class_list_macros.hpp
nodelet::Nodelet::getName
const std::string & getName() const
header
const std::string header
ros::Duration
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
NODELET_DEBUG
#define NODELET_DEBUG(...)
pcl::ros_ptr
boost::shared_ptr< T > ros_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:354
pcl_conversions.h


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Fri Jul 12 2024 02:54:40