40 #include <pcl/io/io.h> 60 if (!pnh_->getParam (
"model_type", model_type))
62 NODELET_ERROR (
"[onInit] Need a 'model_type' parameter to be set before continuing!");
66 if (!pnh_->getParam (
"distance_threshold", threshold))
68 NODELET_ERROR (
"[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
74 pnh_->getParam (
"method_type", method_type);
77 pnh_->getParam (
"axis", axis_param);
78 Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
84 if (axis_param.
size () != 3)
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 ());
89 for (
int i = 0; i < 3; ++i)
93 NODELET_ERROR (
"[%s::onInit] Need floating point values for 'axis' parameter.",
getName ().c_str ());
96 double value = axis_param[i]; axis[i] = value;
110 srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig> >(*pnh_);
112 srv_->setCallback (f);
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]);
123 impl_.setModelType (model_type);
124 impl_.setMethodType (method_type);
125 impl_.setAxis (axis);
197 boost::mutex::scoped_lock lock (
mutex_);
199 if (
impl_.getDistanceThreshold () != config.distance_threshold)
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);
206 if (
impl_.getEpsAngle () != config.eps_angle)
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);
219 if (
impl_.getMaxIterations () != config.max_iterations)
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);
225 if (
impl_.getProbability () != config.probability)
228 impl_.setProbability (config.probability);
229 NODELET_DEBUG (
"[%s::config_callback] Setting new probability to: %f.",
getName ().c_str (), config.probability);
231 if (
impl_.getOptimizeCoefficients () != config.optimize_coefficients)
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");
237 double radius_min, radius_max;
238 impl_.getRadiusLimits (radius_min, radius_max);
239 if (radius_min != config.radius_min)
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);
245 if (radius_max != config.radius_max)
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);
271 boost::mutex::scoped_lock lock (
mutex_);
273 pcl_msgs::PointIndices inliers;
274 pcl_msgs::ModelCoefficients model;
276 inliers.header = model.header =
fromPCL(cloud->header);
287 if (indices && !
isValid (indices))
296 if (indices && !indices->header.frame_id.empty ())
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.",
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 ());
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 ());
325 if (indices && !indices->header.frame_id.empty ())
326 indices_ptr.reset (
new std::vector<int> (indices->indices));
328 impl_.setInputCloud (cloud_tf);
329 impl_.setIndices (indices_ptr);
332 if (!cloud->points.empty ()) {
333 pcl::PointIndices pcl_inliers;
334 pcl::ModelCoefficients pcl_model;
337 impl_.segment (pcl_inliers, pcl_model);
347 inliers.indices.clear ();
348 model.values.clear ();
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 ());
358 if (inliers.indices.empty ())
370 srv_ = boost::make_shared <dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > (*pnh_);
372 srv_->setCallback (f);
380 if (!pnh_->getParam (
"model_type", model_type))
382 NODELET_ERROR (
"[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
getName ().c_str ());
386 if (!pnh_->getParam (
"distance_threshold", threshold))
388 NODELET_ERROR (
"[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!",
getName ().c_str ());
394 pnh_->getParam (
"method_type", method_type);
397 pnh_->getParam (
"axis", axis_param);
398 Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
404 if (axis_param.
size () != 3)
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 ());
409 for (
int i = 0; i < 3; ++i)
413 NODELET_ERROR (
"[%s::onInit] Need floating point values for 'axis' parameter.",
getName ().c_str ());
416 double value = axis_param[i]; axis[i] = value;
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]);
438 impl_.setModelType (model_type);
439 impl_.setMethodType (method_type);
440 impl_.setAxis (axis);
457 sync_input_normals_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > (
max_queue_size_);
459 sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (
max_queue_size_);
478 sync_input_normals_indices_a_->connectInput (
sub_input_filter_, sub_normals_filter_, nf_);
480 sync_input_normals_indices_e_->connectInput (
sub_input_filter_, sub_normals_filter_, nf_);
494 sub_normals_filter_.unsubscribe ();
496 sub_axis_.shutdown ();
506 boost::mutex::scoped_lock lock (
mutex_);
508 if (model->values.size () < 3)
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 ());
513 NODELET_DEBUG (
"[%s::axis_callback] Received axis direction: %f %f %f",
getName ().c_str (), model->values[0], model->values[1], model->values[2]);
515 Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]);
516 impl_.setAxis (axis);
523 boost::mutex::scoped_lock lock (
mutex_);
525 if (
impl_.getDistanceThreshold () != config.distance_threshold)
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);
531 if (
impl_.getEpsAngle () != config.eps_angle)
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);
537 if (
impl_.getMaxIterations () != config.max_iterations)
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);
551 if (
impl_.getProbability () != config.probability)
553 impl_.setProbability (config.probability);
554 NODELET_DEBUG (
"[%s::config_callback] Setting new probability to: %f.",
getName ().c_str (), config.probability);
557 if (
impl_.getOptimizeCoefficients () != config.optimize_coefficients)
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");
563 if (
impl_.getNormalDistanceWeight () != config.normal_distance_weight)
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);
569 double radius_min, radius_max;
570 impl_.getRadiusLimits (radius_min, radius_max);
571 if (radius_min != config.radius_min)
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);
577 if (radius_max != config.radius_max)
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);
593 boost::mutex::scoped_lock lock (
mutex_);
598 inliers.header = model.header =
fromPCL(cloud->header);
600 if (
impl_.getModelType () < 0)
602 NODELET_ERROR (
"[%s::input_normals_indices_callback] Model type not set!",
getName ().c_str ());
616 if (indices && !
isValid (indices))
625 if (indices && !indices->header.frame_id.empty ())
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.",
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 ());
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.",
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 ());
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)
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);
655 impl_.setInputCloud (cloud);
656 impl_.setInputNormals (cloud_normals);
659 if (indices && !indices->header.frame_id.empty ())
660 indices_ptr.reset (
new std::vector<int> (indices->indices));
662 impl_.setIndices (indices_ptr);
665 if (!cloud->points.empty ()) {
666 pcl::PointIndices pcl_inliers;
667 pcl::ModelCoefficients pcl_model;
670 impl_.segment (pcl_inliers, pcl_model);
678 inliers.indices.clear ();
679 model.values.clear ();
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 ())
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 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.
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...
const std::string & getName() const
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Type const & getType() const
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...
ros::Subscriber sub_input_
The input PointCloud subscriber.
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
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.
PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet)
PointCloud::ConstPtr PointCloudConstPtr
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::SACSegmentation< pcl::PointXYZ > impl_
The PCL implementation used.
int max_queue_size_
The maximum queue size (default: 3).
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.
virtual void unsubscribe()
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.