From 3cab2abb155359398cd33005fac45293cbca36a2 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 16:42:50 +0000 Subject: [PATCH] LazyNodelet for segmentation/SACSegmentationFromNormals --- .../pcl_ros/segmentation/sac_segmentation.h | 4 + .../pcl_ros/segmentation/sac_segmentation.cpp | 106 ++++++++++-------- 2 files changed, 65 insertions(+), 45 deletions(-) diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h index 2d96a95b..af2c9126 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h @@ -242,6 +242,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief LazyNodelet connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Model callback * \param model the sample consensus model found */ diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index 22a976ab..f623d7c4 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -371,48 +371,9 @@ pcl_ros::SACSegmentationFromNormals::onInit () dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2); srv_->setCallback (f); - // Subscribe to the input and normals using filters - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); - - // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) - sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this); - - if (approximate_sync_) - sync_input_normals_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_normals_indices_e_ = boost::make_shared > > (max_queue_size_); - - // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { - // Subscribe to the input using a filter - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); - - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - } - else - { - // Create a different callback for copying over the timestamp to fake indices - sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1)); - - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); - } - - if (approximate_sync_) - sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); - else - sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); - // Advertise the output topics - pub_indices_ = pnh_->advertise ("inliers", max_queue_size_); - pub_model_ = pnh_->advertise ("model", max_queue_size_); + pub_indices_ = advertise (*pnh_, "inliers", max_queue_size_); + pub_model_ = advertise (*pnh_, "model", max_queue_size_); // ---[ Mandatory parameters int model_type; @@ -477,6 +438,65 @@ pcl_ros::SACSegmentationFromNormals::onInit () impl_.setModelType (model_type); impl_.setMethodType (method_type); impl_.setAxis (axis); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::subscribe () +{ + // Subscribe to the input and normals using filters + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + + // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) + sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + + if (approximate_sync_) + sync_input_normals_indices_a_ = boost::make_shared > > (max_queue_size_); + else + sync_input_normals_indices_e_ = boost::make_shared > > (max_queue_size_); + + // If we're supposed to look for PointIndices (indices) + if (use_indices_) + { + // Subscribe to the input using a filter + sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + + if (approximate_sync_) + sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); + else + sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); + } + else + { + // Create a different callback for copying over the timestamp to fake indices + sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1)); + + if (approximate_sync_) + sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + else + sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + } + + if (approximate_sync_) + sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); + else + sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentationFromNormals::unsubscribe () +{ + sub_input_filter_.unsubscribe (); + sub_normals_filter_.unsubscribe (); + + sub_axis_.shutdown (); + + if (use_indices_) + sub_indices_filter_.unsubscribe (); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -572,10 +592,6 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( { boost::mutex::scoped_lock lock (mutex_); - // No subscribers, no work - if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0) - return; - PointIndices inliers; ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied