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