From ebe25b4b891d4eedbdc6afeee7023336bbe0b1eb Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Tue, 22 Aug 2017 16:28:36 +0000
Subject: [PATCH] LazyNodelet for segmentation/ExtractPolygonalPrismData

---
 .../extract_polygonal_prism_data.h            |  6 +++-
 .../extract_polygonal_prism_data.cpp          | 28 +++++++++++++++----
 2 files changed, 28 insertions(+), 6 deletions(-)

diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h
index 8c24a889..7134f905 100644
--- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h
+++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h
@@ -97,9 +97,13 @@ namespace pcl_ros
         nf_.add (boost::make_shared<PointIndices> (cloud));
       }
 
-       /** \brief Nodelet initialization routine. */
+      /** \brief Nodelet initialization routine. */
       void onInit ();
 
+      /** \brief LazyNodelet connection routine. */
+      void subscribe ();
+      void unsubscribe ();
+
       /** \brief Dynamic reconfigure callback
         * \param config the config object
         * \param level the dynamic reconfigure level
diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
index b75b3ecd..683a9ec0 100644
--- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
+++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
@@ -52,14 +52,24 @@ pcl_ros::ExtractPolygonalPrismData::onInit ()
   // Call the super onInit ()
   PCLNodelet::onInit ();
 
-  sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
-  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
-
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
   dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
   srv_->setCallback (f);
 
+  // Advertise the output topics
+  pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
+
+  onInitPostProcess ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::ExtractPolygonalPrismData::subscribe ()
+{
+  sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
+  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+
   // Create the objects here
   if (approximate_sync_)
     sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
@@ -88,9 +98,17 @@ pcl_ros::ExtractPolygonalPrismData::onInit ()
     sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
   else
     sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
+}
 
-  // Advertise the output topics
-  pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::ExtractPolygonalPrismData::unsubscribe ()
+{
+  sub_hull_filter_.unsubscribe ();
+  sub_input_filter_.unsubscribe ();
+
+  if (use_indices_)
+    sub_indices_filter_.unsubscribe ();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////