sac_segmentation.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, 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: sac_segmentation.hpp 33195 2010-10-10 14:12:19Z marton $
35  *
36  */
37 
40 
42 
44 
46 void
48 {
49  // Call the super onInit ()
51 
52 
53  // Advertise the output topics
54  pub_indices_ = advertise<PointIndices> (*pnh_, "inliers", max_queue_size_);
55  pub_model_ = advertise<ModelCoefficients> (*pnh_, "model", max_queue_size_);
56 
57  // ---[ Mandatory parameters
58  int model_type;
59  if (!pnh_->getParam ("model_type", model_type))
60  {
61  NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!");
62  return;
63  }
64  double threshold; // unused - set via dynamic reconfigure in the callback
65  if (!pnh_->getParam ("distance_threshold", threshold))
66  {
67  NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
68  return;
69  }
70 
71  // ---[ Optional parameters
72  int method_type = 0;
73  pnh_->getParam ("method_type", method_type);
74 
75  XmlRpc::XmlRpcValue axis_param;
76  pnh_->getParam ("axis", axis_param);
77  Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
78 
79  switch (axis_param.getType ())
80  {
82  {
83  if (axis_param.size () != 3)
84  {
85  NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
86  return;
87  }
88  for (int i = 0; i < 3; ++i)
89  {
90  if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
91  {
92  NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
93  return;
94  }
95  double value = axis_param[i]; axis[i] = value;
96  }
97  break;
98  }
99  default:
100  {
101  break;
102  }
103  }
104 
105  // Initialize the random number generator
106  srand (time (0));
107 
108  // Enable the dynamic reconfigure service
109  srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig> >(*pnh_);
110  dynamic_reconfigure::Server<SACSegmentationConfig>::CallbackType f = boost::bind (&SACSegmentation::config_callback, this, _1, _2);
111  srv_->setCallback (f);
112 
113  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
114  " - model_type : %d\n"
115  " - method_type : %d\n"
116  " - model_threshold : %f\n"
117  " - axis : [%f, %f, %f]\n",
118  getName ().c_str (), model_type, method_type, threshold,
119  axis[0], axis[1], axis[2]);
120 
121  // Set given parameters here
122  impl_.setModelType (model_type);
123  impl_.setMethodType (method_type);
124  impl_.setAxis (axis);
125 
127 }
128 
130 void
132 {
133  // If we're supposed to look for PointIndices (indices)
134  if (use_indices_)
135  {
136  // Subscribe to the input using a filter
139 
140  // when "use_indices" is set to true, and "latched_indices" is set to true,
141  // we'll subscribe and get a separate callback for PointIndices that will
142  // save the indices internally, and a PointCloud + PointIndices callback
143  // will take care of meshing the new PointClouds with the old saved indices.
144  if (latched_indices_)
145  {
146  // Subscribe to a callback that saves the indices
148  // Subscribe to a callback that sets the header of the saved indices to the cloud header
150 
151  // Synchronize the two topics. No need for an approximate synchronizer here, as we'll
152  // match the timestamps exactly
153  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
155  sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
156  }
157  // "latched_indices" not set, proceed with regular <input,indices> pairs
158  else
159  {
160  if (approximate_sync_)
161  {
162  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
164  sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
165  }
166  else
167  {
168  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
170  sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
171  }
172  }
173  }
174  else
175  // Subscribe in an old fashion to input only (no filters)
177 }
178 
180 void
182 {
183  if (use_indices_)
184  {
187  }
188  else
189  sub_input_.shutdown ();
190 }
191 
193 void
194 pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t /*level*/)
195 {
196  boost::mutex::scoped_lock lock (mutex_);
197 
198  if (impl_.getDistanceThreshold () != config.distance_threshold)
199  {
200  //sac_->setDistanceThreshold (threshold_); - done in initSAC
201  impl_.setDistanceThreshold (config.distance_threshold);
202  NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
203  }
204  // The maximum allowed difference between the model normal and the given axis _in radians_
205  if (impl_.getEpsAngle () != config.eps_angle)
206  {
207  impl_.setEpsAngle (config.eps_angle);
208  NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
209  }
210 
211  // Number of inliers
212  if (min_inliers_ != config.min_inliers)
213  {
214  min_inliers_ = config.min_inliers;
215  NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_);
216  }
217 
218  if (impl_.getMaxIterations () != config.max_iterations)
219  {
220  //sac_->setMaxIterations (max_iterations_); - done in initSAC
221  impl_.setMaxIterations (config.max_iterations);
222  NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
223  }
224  if (impl_.getProbability () != config.probability)
225  {
226  //sac_->setProbability (probability_); - done in initSAC
227  impl_.setProbability (config.probability);
228  NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
229  }
230  if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
231  {
232  impl_.setOptimizeCoefficients (config.optimize_coefficients);
233  NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
234  }
235 
236  double radius_min, radius_max;
237  impl_.getRadiusLimits (radius_min, radius_max);
238  if (radius_min != config.radius_min)
239  {
240  radius_min = config.radius_min;
241  NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min);
242  impl_.setRadiusLimits (radius_min, radius_max);
243  }
244  if (radius_max != config.radius_max)
245  {
246  radius_max = config.radius_max;
247  NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max);
248  impl_.setRadiusLimits (radius_min, radius_max);
249  }
250 
251  if (tf_input_frame_ != config.input_frame)
252  {
253  tf_input_frame_ = config.input_frame;
254  NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
255  NODELET_WARN ("input_frame TF not implemented yet!");
256  }
257  if (tf_output_frame_ != config.output_frame)
258  {
259  tf_output_frame_ = config.output_frame;
260  NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
261  NODELET_WARN ("output_frame TF not implemented yet!");
262  }
263 }
264 
266 void
268  const PointIndicesConstPtr &indices)
269 {
270  boost::mutex::scoped_lock lock (mutex_);
271 
272  pcl_msgs::PointIndices inliers;
273  pcl_msgs::ModelCoefficients model;
274  // Enforce that the TF frame and the timestamp are copied
275  inliers.header = model.header = fromPCL(cloud->header);
276 
277  // If cloud is given, check if it's valid
278  if (!isValid (cloud))
279  {
280  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
281  pub_indices_.publish (inliers);
282  pub_model_.publish (model);
283  return;
284  }
285  // If indices are given, check if they are valid
286  if (indices && !isValid (indices))
287  {
288  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
289  pub_indices_.publish (inliers);
290  pub_model_.publish (model);
291  return;
292  }
293 
295  if (indices && !indices->header.frame_id.empty ())
296  NODELET_DEBUG ("[%s::input_indices_callback]\n"
297  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
298  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
299  getName ().c_str (),
300  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
301  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
302  else
303  NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
304  getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
306 
307  // Check whether the user has given a different input TF frame
308  tf_input_orig_frame_ = cloud->header.frame_id;
309  PointCloudConstPtr cloud_tf;
310 /* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
311  {
312  NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
313  // Save the original frame ID
314  // Convert the cloud into the different frame
315  PointCloud cloud_transformed;
316  if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_))
317  return;
318  cloud_tf.reset (new PointCloud (cloud_transformed));
319  }
320  else*/
321  cloud_tf = cloud;
322 
323  IndicesPtr indices_ptr;
324  if (indices && !indices->header.frame_id.empty ())
325  indices_ptr.reset (new std::vector<int> (indices->indices));
326 
327  impl_.setInputCloud (pcl_ptr(cloud_tf));
328  impl_.setIndices (indices_ptr);
329 
330  // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
331  if (!cloud->points.empty ()) {
332  pcl::PointIndices pcl_inliers;
333  pcl::ModelCoefficients pcl_model;
334  pcl_conversions::moveToPCL(inliers, pcl_inliers);
335  pcl_conversions::moveToPCL(model, pcl_model);
336  impl_.segment (pcl_inliers, pcl_model);
337  pcl_conversions::moveFromPCL(pcl_inliers, inliers);
338  pcl_conversions::moveFromPCL(pcl_model, model);
339  }
340 
341  // Probably need to transform the model of the plane here
342 
343  // Check if we have enough inliers, clear inliers + model if not
344  if ((int)inliers.indices.size () <= min_inliers_)
345  {
346  inliers.indices.clear ();
347  model.values.clear ();
348  }
349 
350  // Publish
351  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
352  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
353  NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
354  getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
355  model.values.size (), pnh_->resolveName ("model").c_str ());
356 
357  if (inliers.indices.empty ())
358  NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
359 }
360 
362 void
364 {
365  // Call the super onInit ()
367 
368  // Enable the dynamic reconfigure service
369  srv_ = boost::make_shared <dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > (*pnh_);
370  dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2);
371  srv_->setCallback (f);
372 
373  // Advertise the output topics
374  pub_indices_ = advertise<PointIndices> (*pnh_, "inliers", max_queue_size_);
375  pub_model_ = advertise<ModelCoefficients> (*pnh_, "model", max_queue_size_);
376 
377  // ---[ Mandatory parameters
378  int model_type;
379  if (!pnh_->getParam ("model_type", model_type))
380  {
381  NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
382  return;
383  }
384  double threshold; // unused - set via dynamic reconfigure in the callback
385  if (!pnh_->getParam ("distance_threshold", threshold))
386  {
387  NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ());
388  return;
389  }
390 
391  // ---[ Optional parameters
392  int method_type = 0;
393  pnh_->getParam ("method_type", method_type);
394 
395  XmlRpc::XmlRpcValue axis_param;
396  pnh_->getParam ("axis", axis_param);
397  Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
398 
399  switch (axis_param.getType ())
400  {
402  {
403  if (axis_param.size () != 3)
404  {
405  NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
406  return;
407  }
408  for (int i = 0; i < 3; ++i)
409  {
410  if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
411  {
412  NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
413  return;
414  }
415  double value = axis_param[i]; axis[i] = value;
416  }
417  break;
418  }
419  default:
420  {
421  break;
422  }
423  }
424 
425  // Initialize the random number generator
426  srand (time (0));
427 
428  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
429  " - model_type : %d\n"
430  " - method_type : %d\n"
431  " - model_threshold : %f\n"
432  " - axis : [%f, %f, %f]\n",
433  getName ().c_str (), model_type, method_type, threshold,
434  axis[0], axis[1], axis[2]);
435 
436  // Set given parameters here
437  impl_.setModelType (model_type);
438  impl_.setMethodType (method_type);
439  impl_.setAxis (axis);
440 
442 }
443 
445 void
447 {
448  // Subscribe to the input and normals using filters
450  sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
451 
452  // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked)
453  sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this);
454 
455  if (approximate_sync_)
456  sync_input_normals_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
457  else
458  sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
459 
460  // If we're supposed to look for PointIndices (indices)
461  if (use_indices_)
462  {
463  // Subscribe to the input using a filter
465 
466  if (approximate_sync_)
467  sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
468  else
469  sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
470  }
471  else
472  {
473  // Create a different callback for copying over the timestamp to fake indices
475 
476  if (approximate_sync_)
477  sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
478  else
479  sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
480  }
481 
482  if (approximate_sync_)
483  sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
484  else
485  sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
486 }
487 
489 void
491 {
493  sub_normals_filter_.unsubscribe ();
494 
495  sub_axis_.shutdown ();
496 
497  if (use_indices_)
499 }
500 
502 void
503 pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model)
504 {
505  boost::mutex::scoped_lock lock (mutex_);
506 
507  if (model->values.size () < 3)
508  {
509  NODELET_ERROR ("[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", getName ().c_str (), model->values.size (), pnh_->resolveName ("axis").c_str ());
510  return;
511  }
512  NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]);
513 
514  Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]);
515  impl_.setAxis (axis);
516 }
517 
519 void
520 pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t /*level*/)
521 {
522  boost::mutex::scoped_lock lock (mutex_);
523 
524  if (impl_.getDistanceThreshold () != config.distance_threshold)
525  {
526  impl_.setDistanceThreshold (config.distance_threshold);
527  NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
528  }
529  // The maximum allowed difference between the model normal and the given axis _in radians_
530  if (impl_.getEpsAngle () != config.eps_angle)
531  {
532  impl_.setEpsAngle (config.eps_angle);
533  NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
534  }
535 
536  if (impl_.getMaxIterations () != config.max_iterations)
537  {
538  impl_.setMaxIterations (config.max_iterations);
539  NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
540  }
541 
542  // Number of inliers
543  if (min_inliers_ != config.min_inliers)
544  {
545  min_inliers_ = config.min_inliers;
546  NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_);
547  }
548 
549 
550  if (impl_.getProbability () != config.probability)
551  {
552  impl_.setProbability (config.probability);
553  NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
554  }
555 
556  if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
557  {
558  impl_.setOptimizeCoefficients (config.optimize_coefficients);
559  NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
560  }
561 
562  if (impl_.getNormalDistanceWeight () != config.normal_distance_weight)
563  {
564  impl_.setNormalDistanceWeight (config.normal_distance_weight);
565  NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight);
566  }
567 
568  double radius_min, radius_max;
569  impl_.getRadiusLimits (radius_min, radius_max);
570  if (radius_min != config.radius_min)
571  {
572  radius_min = config.radius_min;
573  NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min);
574  impl_.setRadiusLimits (radius_min, radius_max);
575  }
576  if (radius_max != config.radius_max)
577  {
578  radius_max = config.radius_max;
579  NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max);
580  impl_.setRadiusLimits (radius_min, radius_max);
581  }
582 }
583 
585 void
587  const PointCloudConstPtr &cloud,
588  const PointCloudNConstPtr &cloud_normals,
589  const PointIndicesConstPtr &indices
590  )
591 {
592  boost::mutex::scoped_lock lock (mutex_);
593 
594  PointIndices inliers;
595  ModelCoefficients model;
596  // Enforce that the TF frame and the timestamp are copied
597  inliers.header = model.header = fromPCL(cloud->header);
598 
599  if (impl_.getModelType () < 0)
600  {
601  NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ());
602  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
603  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
604  return;
605  }
606 
607  if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
608  {
609  NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ());
610  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
611  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
612  return;
613  }
614  // If indices are given, check if they are valid
615  if (indices && !isValid (indices))
616  {
617  NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ());
618  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
619  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
620  return;
621  }
622 
624  if (indices && !indices->header.frame_id.empty ())
625  NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
626  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
627  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
628  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
629  getName ().c_str (),
630  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
631  cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
632  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
633  else
634  NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
635  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
636  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
637  getName ().c_str (),
638  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
639  cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
641 
642 
643  // Extra checks for safety
644  int cloud_nr_points = cloud->width * cloud->height;
645  int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
646  if (cloud_nr_points != cloud_normals_nr_points)
647  {
648  NODELET_ERROR ("[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", getName ().c_str (), cloud_nr_points, cloud_normals_nr_points);
649  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
650  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
651  return;
652  }
653 
654  impl_.setInputCloud (pcl_ptr(cloud));
655  impl_.setInputNormals (pcl_ptr(cloud_normals));
656 
657  IndicesPtr indices_ptr;
658  if (indices && !indices->header.frame_id.empty ())
659  indices_ptr.reset (new std::vector<int> (indices->indices));
660 
661  impl_.setIndices (indices_ptr);
662 
663  // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
664  if (!cloud->points.empty ()) {
665  pcl::PointIndices pcl_inliers;
666  pcl::ModelCoefficients pcl_model;
667  pcl_conversions::moveToPCL(inliers, pcl_inliers);
668  pcl_conversions::moveToPCL(model, pcl_model);
669  impl_.segment (pcl_inliers, pcl_model);
670  pcl_conversions::moveFromPCL(pcl_inliers, inliers);
671  pcl_conversions::moveFromPCL(pcl_model, model);
672  }
673 
674  // Check if we have enough inliers, clear inliers + model if not
675  if ((int)inliers.indices.size () <= min_inliers_)
676  {
677  inliers.indices.clear ();
678  model.values.clear ();
679  }
680 
681  // Publish
682  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
683  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
684  NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
685  getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
686  model.values.size (), pnh_->resolveName ("model").c_str ());
687  if (inliers.indices.empty ())
688  NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
689 }
690 
695 
ros::Publisher pub_model_
The output ModelCoefficients publisher.
pcl_msgs::ModelCoefficients ModelCoefficients
Definition: pcl_nodelet.h:86
virtual void subscribe()
LazyNodelet connection routine.
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
boost::mutex mutex_
Internal mutex.
#define NODELET_ERROR(...)
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:204
#define NODELET_WARN(...)
virtual void subscribe()
LazyNodelet connection routine.
void input_callback(const PointCloudConstPtr &input)
Input callback. Used when latched_indices_ is set.
void config_callback(SACSegmentationConfig &config, uint32_t level)
Dynamic reconfigure callback.
f
std::string tf_output_frame_
The output TF frame the data should be transformed into, if input.header.frame_id is different...
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud, PointIndices > > > sync_input_indices_a_
pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals
void input_callback(const PointCloudConstPtr &cloud)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Definition: pcl_nodelet.h:122
bool latched_indices_
Set to true if the indices topic is latched.
Definition: pcl_nodelet.h:116
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:141
const std::string & getName() const
ros::Subscriber sub_input_
The input PointCloud subscriber.
Type const & getType() const
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< ros::NodeHandle > pnh_
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud, PointIndices > > > sync_input_indices_e_
Synchronized input, and indices.
pcl_msgs::PointIndices PointIndices
Definition: pcl_nodelet.h:82
virtual void onInit()
Nodelet initialization routine.
pcl::PointCloud< pcl::PointXYZ > PointCloud
Definition: pcl_nodelet.h:78
void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr &model)
Model callback.
SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods...
message_filters::PassThrough< pcl_msgs::PointIndices > nf_pi_
Null passthrough filter, used for pushing empty elements in the synchronizer.
ros::Publisher pub_indices_
The output PointIndices publisher.
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)
SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models...
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Definition: pcl_nodelet.h:131
std::string tf_input_orig_frame_
The original data input TF frame.
pcl::IndicesPtr IndicesPtr
Definition: pcl_nodelet.h:90
pcl::SACSegmentation< pcl::PointXYZ > impl_
The PCL implementation used.
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:128
void indices_callback(const PointIndicesConstPtr &indices)
Indices callback. Used when latched_indices_ is set.
virtual void onInit()
Nodelet initialization routine.
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
SACSegmentation()
Constructor.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
boost::shared_ptr< T > pcl_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:392
void input_indices_callback(const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
Input point cloud callback. Used when use_indices is set.
#define NODELET_DEBUG(...)
std::string tf_input_frame_
The input TF frame the data should be transformed into, if input.header.frame_id is different...
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:95
void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
boost::shared_ptr< dynamic_reconfigure::Server< SACSegmentationConfig > > srv_
Pointer to a dynamic reconfigure service.
void config_callback(SACSegmentationFromNormalsConfig &config, uint32_t level)
Dynamic reconfigure callback.
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.
Definition: pcl_nodelet.h:119
Connection registerCallback(const C &callback)
void input_normals_indices_callback(const PointCloudConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, const PointIndicesConstPtr &indices)
Input point cloud callback.


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu Feb 16 2023 03:08:02