Inherit NodeletLazy for pipeline with less cpu load

pcl_ros/CMakeLists.txt

@@ -76,6 +76,7 @@ catkin_package(
     dynamic_reconfigure
     message_filters
     nodelet
+    nodelet_topic_tools
     pcl_conversions
     pcl_msgs
     rosbag

pcl_ros/include/pcl_ros/filters/filter.h

@@ -114,6 +114,14 @@ namespace pcl_ros
       filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, 
               PointCloud2 &output) = 0;
 
+      /** \brief Lazy transport subscribe routine. */
+      virtual void
+      subscribe();
+
+      /** \brief Lazy transport unsubscribe routine. */
+      virtual void
+      unsubscribe();
+
       /** \brief Nodelet initialization routine. */
       virtual void 
       onInit ();

pcl_ros/include/pcl_ros/pcl_nodelet.h

@@ -52,7 +52,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include "pcl_ros/point_cloud.h"
 // ROS Nodelet includes
-#include <nodelet/nodelet.h>
+#include <nodelet_topic_tools/nodelet_lazy.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/exact_time.h>
@@ -69,7 +69,7 @@ namespace pcl_ros
   ////////////////////////////////////////////////////////////////////////////////////////////
   ////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */
-  class PCLNodelet : public nodelet::Nodelet
+  class PCLNodelet : public nodelet_topic_tools::NodeletLazy
   {
     public:
       typedef sensor_msgs::PointCloud2 PointCloud2;
@@ -94,9 +94,6 @@ namespace pcl_ros
                       max_queue_size_ (3), approximate_sync_ (false) {};
 
     protected:
-      /** \brief The ROS NodeHandle used for parameters, publish/subscribe, etc. */
-      boost::shared_ptr<ros::NodeHandle> pnh_;
-
       /** \brief Set to true if point indices are used.
        *
        * When receiving a point cloud, if use_indices_ is false, the entire
@@ -197,12 +194,16 @@ namespace pcl_ros
         return (true);
       }
 
+      /** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */
+      virtual void subscribe () {}
+      virtual void unsubscribe () {}
+
       /** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
       virtual void
       onInit ()
       {
-        pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
+        nodelet_topic_tools::NodeletLazy::onInit();
-        
+
         // Parameters that we care about only at startup
         pnh_->getParam ("max_queue_size", max_queue_size_);
         

pcl_ros/src/pcl_ros/filters/filter.cpp

@@ -104,29 +104,8 @@ pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const Indic
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::Filter::onInit ()
+pcl_ros::Filter::subscribe()
 {
-  // Call the super onInit ()
-  PCLNodelet::onInit ();
-
-  // Call the child's local init
-  bool has_service = false;
-  if (!child_init (*pnh_, has_service))
-  {
-    NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
-    return;
-  }
-
-  pub_output_ = pnh_->advertise<PointCloud2> ("output", max_queue_size_);
-
-  // Enable the dynamic reconfigure service
-  if (!has_service)
-  {
-    srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
-    dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f =  boost::bind (&Filter::config_callback, this, _1, _2);
-    srv_->setCallback (f);
-  }
-
   // If we're supposed to look for PointIndices (indices)
   if (use_indices_)
   {
@@ -150,6 +129,45 @@ pcl_ros::Filter::onInit ()
   else
     // Subscribe in an old fashion to input only (no filters)
     sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_,  bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ()));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Filter::unsubscribe()
+{
+  if (use_indices_)
+  {
+    sub_input_filter_.unsubscribe();
+    sub_indices_filter_.unsubscribe();
+  }
+  else
+    sub_input_.shutdown();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Filter::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  // Call the child's local init
+  bool has_service = false;
+  if (!child_init (*pnh_, has_service))
+  {
+    NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
+    return;
+  }
+
+  pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
+
+  // Enable the dynamic reconfigure service
+  if (!has_service)
+  {
+    srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
+    dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f =  boost::bind (&Filter::config_callback, this, _1, _2);
+    srv_->setCallback (f);
+  }
 
   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
 }
@@ -175,10 +193,6 @@ pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
 void
 pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
 {
-  // No subscribers, no work
-  if (pub_output_.getNumSubscribers () <= 0)
-    return;
-
   // If cloud is given, check if it's valid
   if (!isValid (cloud))
   {