LazyNodelet for segmentation/SACSegmentation

pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h

@@ -117,6 +117,10 @@ namespace pcl_ros
       /** \brief Nodelet initialization routine. */
       virtual void onInit ();
 
+      /** \brief LazyNodelet connection routine. */
+      virtual void subscribe ();
+      virtual void unsubscribe ();
+
       /** \brief Dynamic reconfigure callback
         * \param config the config object
         * \param level the dynamic reconfigure level

pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp

@@ -50,54 +50,10 @@ pcl_ros::SACSegmentation::onInit ()
   // Call the super onInit ()
   PCLNodelet::onInit ();
 
-  // If we're supposed to look for PointIndices (indices)
-  if (use_indices_)
-  {
-    // Subscribe to the input using a filter
-    sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
-    sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
-
-    // when "use_indices" is set to true, and "latched_indices" is set to true,
-    // we'll subscribe and get a separate callback for PointIndices that will 
-    // save the indices internally, and a PointCloud + PointIndices callback 
-    // will take care of meshing the new PointClouds with the old saved indices. 
-    if (latched_indices_)
-    {
-      // Subscribe to a callback that saves the indices
-      sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
-      // Subscribe to a callback that sets the header of the saved indices to the cloud header
-      sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
-
-      // Synchronize the two topics. No need for an approximate synchronizer here, as we'll
-      // match the timestamps exactly
-      sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
-      sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
-      sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
-    }
-    // "latched_indices" not set, proceed with regular <input,indices> pairs
-    else
-    {
-      if (approximate_sync_)
-      {
-        sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
-        sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
-        sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
-      }
-      else
-      {
-        sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
-        sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
-        sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
-      }
-    }
-  }
-  else
-    // Subscribe in an old fashion to input only (no filters)
-    sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_,  bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
 
   // Advertise the output topics
-  pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
+  pub_indices_ = advertise<PointIndices> (*pnh_, "inliers", max_queue_size_);
-  pub_model_   = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
+  pub_model_   = advertise<ModelCoefficients> (*pnh_, "model", max_queue_size_);
 
   // ---[ Mandatory parameters
   int model_type;
@@ -167,6 +123,71 @@ pcl_ros::SACSegmentation::onInit ()
   impl_.setModelType (model_type);
   impl_.setMethodType (method_type);
   impl_.setAxis (axis);
+
+  onInitPostProcess ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentation::subscribe ()
+{
+  // If we're supposed to look for PointIndices (indices)
+  if (use_indices_)
+  {
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+    sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
+
+    // when "use_indices" is set to true, and "latched_indices" is set to true,
+    // we'll subscribe and get a separate callback for PointIndices that will 
+    // save the indices internally, and a PointCloud + PointIndices callback 
+    // will take care of meshing the new PointClouds with the old saved indices. 
+    if (latched_indices_)
+    {
+      // Subscribe to a callback that saves the indices
+      sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
+      // Subscribe to a callback that sets the header of the saved indices to the cloud header
+      sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
+
+      // Synchronize the two topics. No need for an approximate synchronizer here, as we'll
+      // match the timestamps exactly
+      sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
+      sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
+      sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
+    }
+    // "latched_indices" not set, proceed with regular <input,indices> pairs
+    else
+    {
+      if (approximate_sync_)
+      {
+        sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
+        sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
+        sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
+      }
+      else
+      {
+        sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
+        sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
+        sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
+      }
+    }
+  }
+  else
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_,  bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentation::unsubscribe ()
+{
+  if (use_indices_)
+  {
+    sub_input_filter_.unsubscribe ();
+    sub_indices_filter_.unsubscribe ();
+  }
+  else
+    sub_input_.shutdown ();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -249,10 +270,6 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
 {
   boost::mutex::scoped_lock lock (mutex_);
 
-  // No subscribers, no work
-  if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
-    return;
-
   pcl_msgs::PointIndices inliers;
   pcl_msgs::ModelCoefficients model;
   // Enforce that the TF frame and the timestamp are copied