00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pcl_ros/segmentation/sac_segmentation.h"
00040 #include <pcl/io/io.h>
00041
00043 void
00044 pcl_ros::SACSegmentation::onInit ()
00045 {
00046
00047 PCLNodelet::onInit ();
00048
00049
00050 if (use_indices_)
00051 {
00052
00053 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00054 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00055
00056
00057
00058
00059
00060 if (latched_indices_)
00061 {
00062
00063 sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
00064
00065 sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
00066
00067
00068
00069 sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
00070 sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
00071 sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
00072 }
00073
00074 else
00075 {
00076 if (approximate_sync_)
00077 {
00078 sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
00079 sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00080 sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
00081 }
00082 else
00083 {
00084 sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
00085 sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00086 sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
00087 }
00088 }
00089 }
00090 else
00091
00092 sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
00093
00094
00095 pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
00096 pub_model_ = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
00097
00098
00099 int model_type;
00100 if (!pnh_->getParam ("model_type", model_type))
00101 {
00102 NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!");
00103 return;
00104 }
00105 double threshold;
00106 if (!pnh_->getParam ("distance_threshold", threshold))
00107 {
00108 NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
00109 return;
00110 }
00111
00112
00113 int method_type = 0;
00114 pnh_->getParam ("method_type", method_type);
00115
00116 XmlRpc::XmlRpcValue axis_param;
00117 pnh_->getParam ("axis", axis_param);
00118 Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
00119
00120 switch (axis_param.getType ())
00121 {
00122 case XmlRpc::XmlRpcValue::TypeArray:
00123 {
00124 if (axis_param.size () != 3)
00125 {
00126 NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
00127 return;
00128 }
00129 for (int i = 0; i < 3; ++i)
00130 {
00131 if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
00132 {
00133 NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
00134 return;
00135 }
00136 double value = axis_param[i]; axis[i] = value;
00137 }
00138 break;
00139 }
00140 default:
00141 {
00142 break;
00143 }
00144 }
00145
00146
00147 srand (time (0));
00148
00149
00150 srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig> >(*pnh_);
00151 dynamic_reconfigure::Server<SACSegmentationConfig>::CallbackType f = boost::bind (&SACSegmentation::config_callback, this, _1, _2);
00152 srv_->setCallback (f);
00153
00154 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00155 " - model_type : %d\n"
00156 " - method_type : %d\n"
00157 " - model_threshold : %f\n"
00158 " - axis : [%f, %f, %f]\n",
00159 getName ().c_str (), model_type, method_type, threshold,
00160 axis[0], axis[1], axis[2]);
00161
00162
00163 impl_.setModelType (model_type);
00164 impl_.setMethodType (method_type);
00165 impl_.setAxis (axis);
00166 }
00167
00169 void
00170 pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t level)
00171 {
00172 boost::mutex::scoped_lock lock (mutex_);
00173
00174 if (impl_.getDistanceThreshold () != config.distance_threshold)
00175 {
00176
00177 impl_.setDistanceThreshold (config.distance_threshold);
00178 NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
00179 }
00180
00181 if (impl_.getEpsAngle () != config.eps_angle)
00182 {
00183 impl_.setEpsAngle (config.eps_angle);
00184 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);
00185 }
00186
00187
00188 if (min_inliers_ != config.min_inliers)
00189 {
00190 min_inliers_ = config.min_inliers;
00191 NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_);
00192 }
00193
00194 if (impl_.getMaxIterations () != config.max_iterations)
00195 {
00196
00197 impl_.setMaxIterations (config.max_iterations);
00198 NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
00199 }
00200 if (impl_.getProbability () != config.probability)
00201 {
00202
00203 impl_.setProbability (config.probability);
00204 NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
00205 }
00206 if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
00207 {
00208 impl_.setOptimizeCoefficients (config.optimize_coefficients);
00209 NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
00210 }
00211
00212 double radius_min, radius_max;
00213 impl_.getRadiusLimits (radius_min, radius_max);
00214 if (radius_min != config.radius_min)
00215 {
00216 radius_min = config.radius_min;
00217 NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min);
00218 impl_.setRadiusLimits (radius_min, radius_max);
00219 }
00220 if (radius_max != config.radius_max)
00221 {
00222 radius_max = config.radius_max;
00223 NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max);
00224 impl_.setRadiusLimits (radius_min, radius_max);
00225 }
00226
00227 if (tf_input_frame_ != config.input_frame)
00228 {
00229 tf_input_frame_ = config.input_frame;
00230 NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
00231 NODELET_WARN ("input_frame TF not implemented yet!");
00232 }
00233 if (tf_output_frame_ != config.output_frame)
00234 {
00235 tf_output_frame_ = config.output_frame;
00236 NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
00237 NODELET_WARN ("output_frame TF not implemented yet!");
00238 }
00239 }
00240
00242 void
00243 pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud,
00244 const PointIndicesConstPtr &indices)
00245 {
00246 boost::mutex::scoped_lock lock (mutex_);
00247
00248
00249 if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
00250 return;
00251
00252 PointIndices inliers;
00253 ModelCoefficients model;
00254
00255 inliers.header = model.header = cloud->header;
00256
00257
00258 if (!isValid (cloud))
00259 {
00260 NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00261 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00262 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00263 return;
00264 }
00265
00266 if (indices && !isValid (indices))
00267 {
00268 NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00269 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00270 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00271 return;
00272 }
00273
00275 if (indices && !indices->header.frame_id.empty ())
00276 NODELET_DEBUG ("[%s::input_indices_callback]\n"
00277 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00278 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00279 getName ().c_str (),
00280 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00281 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00282 else
00283 NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
00284 getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
00286
00287
00288 tf_input_orig_frame_ = cloud->header.frame_id;
00289 PointCloudConstPtr cloud_tf;
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301 cloud_tf = cloud;
00302
00303 IndicesPtr indices_ptr;
00304 if (indices && !indices->header.frame_id.empty ())
00305 indices_ptr.reset (new std::vector<int> (indices->indices));
00306
00307 impl_.setInputCloud (cloud_tf);
00308 impl_.setIndices (indices_ptr);
00309
00310
00311 if (!cloud->points.empty ())
00312 impl_.segment (inliers, model);
00313
00314
00315
00316
00317 if ((int)inliers.indices.size () <= min_inliers_)
00318 {
00319 inliers.indices.clear ();
00320 model.values.clear ();
00321 }
00322
00323
00324 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00325 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00326 NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
00327 getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
00328 model.values.size (), pnh_->resolveName ("model").c_str ());
00329
00330 if (inliers.indices.empty ())
00331 NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
00332 }
00333
00335 void
00336 pcl_ros::SACSegmentationFromNormals::onInit ()
00337 {
00338
00339 PCLNodelet::onInit ();
00340
00341
00342 srv_ = boost::make_shared <dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > (*pnh_);
00343 dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2);
00344 srv_->setCallback (f);
00345
00346
00347 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00348 sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
00349
00350
00351 sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this);
00352
00353 if (approximate_sync_)
00354 sync_input_normals_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
00355 else
00356 sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
00357
00358
00359 if (use_indices_)
00360 {
00361
00362 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00363
00364 if (approximate_sync_)
00365 sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
00366 else
00367 sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
00368 }
00369 else
00370 {
00371
00372 sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1));
00373
00374 if (approximate_sync_)
00375 sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
00376 else
00377 sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
00378 }
00379
00380 if (approximate_sync_)
00381 sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
00382 else
00383 sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
00384
00385
00386 pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
00387 pub_model_ = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
00388
00389
00390 int model_type;
00391 if (!pnh_->getParam ("model_type", model_type))
00392 {
00393 NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
00394 return;
00395 }
00396 double threshold;
00397 if (!pnh_->getParam ("distance_threshold", threshold))
00398 {
00399 NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ());
00400 return;
00401 }
00402
00403
00404 int method_type = 0;
00405 pnh_->getParam ("method_type", method_type);
00406
00407 XmlRpc::XmlRpcValue axis_param;
00408 pnh_->getParam ("axis", axis_param);
00409 Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
00410
00411 switch (axis_param.getType ())
00412 {
00413 case XmlRpc::XmlRpcValue::TypeArray:
00414 {
00415 if (axis_param.size () != 3)
00416 {
00417 NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
00418 return;
00419 }
00420 for (int i = 0; i < 3; ++i)
00421 {
00422 if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
00423 {
00424 NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
00425 return;
00426 }
00427 double value = axis_param[i]; axis[i] = value;
00428 }
00429 break;
00430 }
00431 default:
00432 {
00433 break;
00434 }
00435 }
00436
00437
00438 srand (time (0));
00439
00440 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00441 " - model_type : %d\n"
00442 " - method_type : %d\n"
00443 " - model_threshold : %f\n"
00444 " - axis : [%f, %f, %f]\n",
00445 getName ().c_str (), model_type, method_type, threshold,
00446 axis[0], axis[1], axis[2]);
00447
00448
00449 impl_.setModelType (model_type);
00450 impl_.setMethodType (method_type);
00451 impl_.setAxis (axis);
00452 }
00453
00455 void
00456 pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model)
00457 {
00458 boost::mutex::scoped_lock lock (mutex_);
00459
00460 if (model->values.size () < 3)
00461 {
00462 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 ());
00463 return;
00464 }
00465 NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]);
00466
00467 Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]);
00468 impl_.setAxis (axis);
00469 }
00470
00472 void
00473 pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level)
00474 {
00475 boost::mutex::scoped_lock lock (mutex_);
00476
00477 if (impl_.getDistanceThreshold () != config.distance_threshold)
00478 {
00479 impl_.setDistanceThreshold (config.distance_threshold);
00480 NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
00481 }
00482
00483 if (impl_.getEpsAngle () != config.eps_angle)
00484 {
00485 impl_.setEpsAngle (config.eps_angle);
00486 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);
00487 }
00488
00489 if (impl_.getMaxIterations () != config.max_iterations)
00490 {
00491 impl_.setMaxIterations (config.max_iterations);
00492 NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
00493 }
00494
00495
00496 if (min_inliers_ != config.min_inliers)
00497 {
00498 min_inliers_ = config.min_inliers;
00499 NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_);
00500 }
00501
00502
00503 if (impl_.getProbability () != config.probability)
00504 {
00505 impl_.setProbability (config.probability);
00506 NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
00507 }
00508
00509 if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
00510 {
00511 impl_.setOptimizeCoefficients (config.optimize_coefficients);
00512 NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
00513 }
00514
00515 if (impl_.getNormalDistanceWeight () != config.normal_distance_weight)
00516 {
00517 impl_.setNormalDistanceWeight (config.normal_distance_weight);
00518 NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight);
00519 }
00520
00521 double radius_min, radius_max;
00522 impl_.getRadiusLimits (radius_min, radius_max);
00523 if (radius_min != config.radius_min)
00524 {
00525 radius_min = config.radius_min;
00526 NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min);
00527 impl_.setRadiusLimits (radius_min, radius_max);
00528 }
00529 if (radius_max != config.radius_max)
00530 {
00531 radius_max = config.radius_max;
00532 NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max);
00533 impl_.setRadiusLimits (radius_min, radius_max);
00534 }
00535 }
00536
00538 void
00539 pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
00540 const PointCloudConstPtr &cloud,
00541 const PointCloudNConstPtr &cloud_normals,
00542 const PointIndicesConstPtr &indices
00543 )
00544 {
00545 boost::mutex::scoped_lock lock (mutex_);
00546
00547
00548 if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
00549 return;
00550
00551 PointIndices inliers;
00552 ModelCoefficients model;
00553
00554 inliers.header = model.header = cloud->header;
00555
00556 if (impl_.getModelType () < 0)
00557 {
00558 NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ());
00559 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00560 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00561 return;
00562 }
00563
00564 if (!isValid (cloud))
00565 {
00566 NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ());
00567 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00568 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00569 return;
00570 }
00571
00572 if (indices && !isValid (indices))
00573 {
00574 NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ());
00575 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00576 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00577 return;
00578 }
00579
00581 if (indices && !indices->header.frame_id.empty ())
00582 NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
00583 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00584 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00585 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00586 getName ().c_str (),
00587 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00588 cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
00589 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00590 else
00591 NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
00592 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00593 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00594 getName ().c_str (),
00595 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00596 cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
00598
00599
00600
00601 int cloud_nr_points = cloud->width * cloud->height;
00602 int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
00603 if (cloud_nr_points != cloud_normals_nr_points)
00604 {
00605 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);
00606 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00607 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00608 return;
00609 }
00610
00611 impl_.setInputCloud (cloud);
00612 impl_.setInputNormals (cloud_normals);
00613
00614 IndicesPtr indices_ptr;
00615 if (indices && !indices->header.frame_id.empty ())
00616 indices_ptr.reset (new std::vector<int> (indices->indices));
00617
00618 impl_.setIndices (indices_ptr);
00619
00620
00621 if (!cloud->points.empty ())
00622 impl_.segment (inliers, model);
00623
00624
00625 if ((int)inliers.indices.size () <= min_inliers_)
00626 {
00627 inliers.indices.clear ();
00628 model.values.clear ();
00629 }
00630
00631
00632 pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00633 pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00634 NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
00635 getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
00636 model.values.size (), pnh_->resolveName ("model").c_str ());
00637 if (inliers.indices.empty ())
00638 NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
00639 }
00640
00641 typedef pcl_ros::SACSegmentation SACSegmentation;
00642 typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals;
00643 PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentation, SACSegmentation, nodelet::Nodelet);
00644 PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentationFromNormals, SACSegmentationFromNormals, nodelet::Nodelet);
00645