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