LazyNodelet for features/*

pcl_ros/include/pcl_ros/features/boundary.h

@@ -65,7 +65,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/feature.h

@@ -165,6 +165,10 @@ namespace pcl_ros
       /** \brief Nodelet initialization routine. */
       virtual void onInit ();
 
+      /** \brief NodeletLazy connection routine. */
+      virtual void subscribe ();
+      virtual void unsubscribe ();
+
       /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
         * \param cloud the pointer to the input point cloud
         * \param cloud_surface the pointer to the surface point cloud
@@ -224,6 +228,10 @@ namespace pcl_ros
       /** \brief Nodelet initialization routine. */
       virtual void onInit ();
 
+      /** \brief NodeletLazy connection routine. */
+      virtual void subscribe ();
+      virtual void unsubscribe ();
+
       /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
         * \param cloud the pointer to the input point cloud
         * \param cloud_normals the pointer to the input point cloud normals

pcl_ros/include/pcl_ros/features/fpfh.h

@@ -77,7 +77,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/fpfh_omp.h

@@ -74,7 +74,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/moment_invariants.h

@@ -61,7 +61,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/normal_3d.h

@@ -63,7 +63,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/normal_3d_omp.h

@@ -59,7 +59,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/normal_3d_tbb.h

@@ -62,7 +62,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloud> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloud> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/pfh.h

@@ -77,7 +77,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/principal_curvatures.h

@@ -63,7 +63,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/shot.h

@@ -57,7 +57,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/shot_omp.h

@@ -56,7 +56,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/include/pcl_ros/features/vfh.h

@@ -62,7 +62,7 @@ namespace pcl_ros
       childInit (ros::NodeHandle &nh)
       {
         // Create the output publisher
-        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
         return (true);
       }
 

pcl_ros/src/pcl_ros/features/feature.cpp

@@ -85,6 +85,21 @@ pcl_ros::Feature::onInit ()
   dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
   srv_->setCallback (f);
 
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - use_surface    : %s\n"
+                 " - k_search       : %d\n"
+                 " - radius_search  : %f\n"
+                 " - spatial_locator: %d",
+                getName ().c_str (),
+                (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+
+  onInitPostProcess ();
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::subscribe ()
+{
   // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
   if (use_indices_ || use_surface_)
   {
@@ -138,14 +153,26 @@ pcl_ros::Feature::onInit ()
   else
     // Subscribe in an old fashion to input only (no filters)
     sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_,  bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
+}
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+////////////////////////////////////////////////////////////////////////////////////////////
-                 " - use_surface    : %s\n"
+void
-                 " - k_search       : %d\n"
+pcl_ros::Feature::unsubscribe ()
-                 " - radius_search  : %f\n"
+{
-                 " - spatial_locator: %d",
+  if (use_indices_ || use_surface_)
-                getName ().c_str (),
+  {
-                (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+    sub_input_filter_.unsubscribe ();
+    if (use_indices_)
+    {
+      sub_indices_filter_.unsubscribe ();
+      if (use_surface_)
+        sub_surface_filter_.unsubscribe ();
+    }
+    else
+      sub_surface_filter_.unsubscribe ();
+  }
+  else
+    sub_input_.shutdown ();
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////
@@ -273,14 +300,29 @@ pcl_ros::FeatureFromNormals::onInit ()
   // ---[ Optional parameters
   pnh_->getParam ("use_surface", use_surface_);
 
-  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
-  sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
-
   // Enable the dynamic reconfigure service
   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
   dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
   srv_->setCallback (f);
 
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - use_surface    : %s\n"
+                 " - k_search       : %d\n"
+                 " - radius_search  : %f\n"
+                 " - spatial_locator: %d",
+                getName ().c_str (),
+                 (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+
+  onInitPostProcess ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::subscribe ()
+{
+  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+  sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
+
   // Create the objects here
   if (approximate_sync_)
     sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
@@ -341,14 +383,25 @@ pcl_ros::FeatureFromNormals::onInit ()
     sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
   else
     sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+}
 
-  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+//////////////////////////////////////////////////////////////////////////////////////////////
-                 " - use_surface    : %s\n"
+void
-                 " - k_search       : %d\n"
+pcl_ros::FeatureFromNormals::unsubscribe ()
-                 " - radius_search  : %f\n"
+{
-                 " - spatial_locator: %d",
+  sub_input_filter_.unsubscribe ();
-                getName ().c_str (),
+  sub_normals_filter_.unsubscribe ();
-                 (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+  if (use_indices_ || use_surface_)
+  {
+    if (use_indices_)
+    {
+      sub_indices_filter_.unsubscribe ();
+      if (use_surface_)
+        sub_surface_filter_.unsubscribe ();
+    }
+    else
+      sub_surface_filter_.unsubscribe ();
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////