59 if (!pnh_->getParam (
"model_type", model_type))
61 NODELET_ERROR (
"[onInit] Need a 'model_type' parameter to be set before continuing!");
65 if (!pnh_->getParam (
"distance_threshold", threshold))
67 NODELET_ERROR (
"[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
73 pnh_->getParam (
"method_type", method_type);
76 pnh_->getParam (
"axis", axis_param);
77 Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
83 if (axis_param.
size () != 3)
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 ());
88 for (
int i = 0; i < 3; ++i)
92 NODELET_ERROR (
"[%s::onInit] Need floating point values for 'axis' parameter.",
getName ().c_str ());
95 double value = axis_param[i]; axis[i] = value;
109 srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig> >(*pnh_);
111 srv_->setCallback (f);
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]);
122 impl_.setModelType (model_type);
123 impl_.setMethodType (method_type);
124 impl_.setAxis (axis);
196 boost::mutex::scoped_lock lock (
mutex_);
198 if (
impl_.getDistanceThreshold () != config.distance_threshold)
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);
205 if (
impl_.getEpsAngle () != config.eps_angle)
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);
218 if (
impl_.getMaxIterations () != config.max_iterations)
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);
224 if (
impl_.getProbability () != config.probability)
227 impl_.setProbability (config.probability);
228 NODELET_DEBUG (
"[%s::config_callback] Setting new probability to: %f.",
getName ().c_str (), config.probability);
230 if (
impl_.getOptimizeCoefficients () != config.optimize_coefficients)
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");
236 double radius_min, radius_max;
237 impl_.getRadiusLimits (radius_min, radius_max);
238 if (radius_min != config.radius_min)
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);
244 if (radius_max != config.radius_max)
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);
270 boost::mutex::scoped_lock lock (
mutex_);
272 pcl_msgs::PointIndices inliers;
273 pcl_msgs::ModelCoefficients model;
275 inliers.header = model.header =
fromPCL(cloud->header);
286 if (indices && !
isValid (indices))
295 if (indices && !indices->header.frame_id.empty ())
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.",
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 ());
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 ());
324 if (indices && !indices->header.frame_id.empty ())
325 indices_ptr.reset (
new std::vector<int> (indices->indices));
328 impl_.setIndices (indices_ptr);
331 if (!cloud->points.empty ()) {
332 pcl::PointIndices pcl_inliers;
333 pcl::ModelCoefficients pcl_model;
336 impl_.segment (pcl_inliers, pcl_model);
346 inliers.indices.clear ();
347 model.values.clear ();
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 ());
357 if (inliers.indices.empty ())
369 srv_ = boost::make_shared <dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > (*pnh_);
371 srv_->setCallback (f);
379 if (!pnh_->getParam (
"model_type", model_type))
381 NODELET_ERROR (
"[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
getName ().c_str ());
385 if (!pnh_->getParam (
"distance_threshold", threshold))
387 NODELET_ERROR (
"[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!",
getName ().c_str ());
393 pnh_->getParam (
"method_type", method_type);
396 pnh_->getParam (
"axis", axis_param);
397 Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
403 if (axis_param.
size () != 3)
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 ());
408 for (
int i = 0; i < 3; ++i)
412 NODELET_ERROR (
"[%s::onInit] Need floating point values for 'axis' parameter.",
getName ().c_str ());
415 double value = axis_param[i]; axis[i] = value;
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]);
437 impl_.setModelType (model_type);
438 impl_.setMethodType (method_type);
439 impl_.setAxis (axis);
456 sync_input_normals_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > (
max_queue_size_);
458 sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (
max_queue_size_);
477 sync_input_normals_indices_a_->connectInput (
sub_input_filter_, sub_normals_filter_, nf_);
479 sync_input_normals_indices_e_->connectInput (
sub_input_filter_, sub_normals_filter_, nf_);
493 sub_normals_filter_.unsubscribe ();
495 sub_axis_.shutdown ();
505 boost::mutex::scoped_lock lock (
mutex_);
507 if (model->values.size () < 3)
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 ());
512 NODELET_DEBUG (
"[%s::axis_callback] Received axis direction: %f %f %f",
getName ().c_str (), model->values[0], model->values[1], model->values[2]);
514 Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]);
515 impl_.setAxis (axis);
522 boost::mutex::scoped_lock lock (
mutex_);
524 if (
impl_.getDistanceThreshold () != config.distance_threshold)
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);
530 if (
impl_.getEpsAngle () != config.eps_angle)
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);
536 if (
impl_.getMaxIterations () != config.max_iterations)
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);
550 if (
impl_.getProbability () != config.probability)
552 impl_.setProbability (config.probability);
553 NODELET_DEBUG (
"[%s::config_callback] Setting new probability to: %f.",
getName ().c_str (), config.probability);
556 if (
impl_.getOptimizeCoefficients () != config.optimize_coefficients)
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");
562 if (
impl_.getNormalDistanceWeight () != config.normal_distance_weight)
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);
568 double radius_min, radius_max;
569 impl_.getRadiusLimits (radius_min, radius_max);
570 if (radius_min != config.radius_min)
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);
576 if (radius_max != config.radius_max)
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);
592 boost::mutex::scoped_lock lock (
mutex_);
597 inliers.header = model.header =
fromPCL(cloud->header);
599 if (
impl_.getModelType () < 0)
601 NODELET_ERROR (
"[%s::input_normals_indices_callback] Model type not set!",
getName ().c_str ());
615 if (indices && !
isValid (indices))
624 if (indices && !indices->header.frame_id.empty ())
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.",
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 ());
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.",
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 ());
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)
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);
658 if (indices && !indices->header.frame_id.empty ())
659 indices_ptr.reset (
new std::vector<int> (indices->indices));
661 impl_.setIndices (indices_ptr);
664 if (!cloud->points.empty ()) {
665 pcl::PointIndices pcl_inliers;
666 pcl::ModelCoefficients pcl_model;
669 impl_.segment (pcl_inliers, pcl_model);
677 inliers.indices.clear ();
678 model.values.clear ();
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 ())
ros::Publisher pub_model_
The output ModelCoefficients publisher.
pcl_msgs::ModelCoefficients ModelCoefficients
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.
#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.
std::string tf_output_frame_
The output TF frame the data should be transformed into, if input.header.frame_id is different...
virtual void unsubscribe()
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.
bool latched_indices_
Set to true if the indices topic is latched.
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...
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< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud, PointIndices > > > sync_input_indices_e_
Synchronized input, and indices.
pcl_msgs::PointIndices PointIndices
virtual void onInit()
Nodelet initialization routine.
pcl::PointCloud< pcl::PointXYZ > PointCloud
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).
std::string tf_input_orig_frame_
The original data input TF frame.
pcl::IndicesPtr IndicesPtr
pcl::SACSegmentation< pcl::PointXYZ > impl_
The PCL implementation used.
int max_queue_size_
The maximum queue size (default: 3).
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.
virtual void unsubscribe()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
boost::shared_ptr< T > pcl_ptr(const boost::shared_ptr< T > &p)
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.
void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
PointIndices::ConstPtr PointIndicesConstPtr
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.
Connection registerCallback(const C &callback)
void input_normals_indices_callback(const PointCloudConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, const PointIndicesConstPtr &indices)
Input point cloud callback.