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

---
 .../pcl_ros/segmentation/extract_clusters.h   |  4 ++
 .../pcl_ros/segmentation/extract_clusters.cpp | 44 ++++++++++++++-----
 2 files changed, 36 insertions(+), 12 deletions(-)

diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h
index 38a6d379..dc79f796 100644
--- a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h
+++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h
@@ -74,6 +74,10 @@ namespace pcl_ros
       /** \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_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
index 686c0006..ce23f4b5 100644
--- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
+++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
@@ -71,15 +71,33 @@ pcl_ros::EuclideanClusterExtraction::onInit ()
   pnh_->getParam ("publish_indices", publish_indices_);
 
   if (publish_indices_)
-    pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
+    pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
   else
-    pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
+    pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
 
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
   dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f =  boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
   srv_->setCallback (f);
 
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - max_queue_size    : %d\n"
+                 " - use_indices       : %s\n"
+                 " - cluster_tolerance : %f\n",
+                 getName ().c_str (),
+                 max_queue_size_,
+                 (use_indices_) ? "true" : "false", cluster_tolerance);
+
+  // Set given parameters here
+  impl_.setClusterTolerance (cluster_tolerance);
+
+  onInitPostProcess ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::EuclideanClusterExtraction::subscribe ()
+{
   // If we're supposed to look for PointIndices (indices)
   if (use_indices_)
   {
@@ -103,17 +121,19 @@ pcl_ros::EuclideanClusterExtraction::onInit ()
   else
     // Subscribe in an old fashion to input only (no filters)
     sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+}
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
-                 " - max_queue_size    : %d\n"
-                 " - use_indices       : %s\n"
-                 " - cluster_tolerance : %f\n",
-                 getName ().c_str (),
-                 max_queue_size_,
-                 (use_indices_) ? "true" : "false", cluster_tolerance);
-
-  // Set given parameters here
-  impl_.setClusterTolerance (cluster_tolerance);
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::EuclideanClusterExtraction::unsubscribe ()
+{
+  if (use_indices_)
+  {
+    sub_input_filter_.unsubscribe ();
+    sub_indices_filter_.unsubscribe ();
+  }
+  else
+    sub_input_.shutdown ();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////