LazyNodelet for io/PointCloudConcatenateDataSynchronizer

pcl_ros/include/pcl_ros/io/concatenate_data.h

@@ -40,7 +40,7 @@
 
 // ROS includes
 #include <tf/transform_listener.h>
-#include <nodelet/nodelet.h>
+#include <nodelet_topic_tools/nodelet_lazy.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/pass_through.h>
@@ -57,7 +57,7 @@ namespace pcl_ros
     * PointCloud output message.
     * \author Radu Bogdan Rusu
     */
-  class PointCloudConcatenateDataSynchronizer: public nodelet::Nodelet
+  class PointCloudConcatenateDataSynchronizer: public nodelet_topic_tools::NodeletLazy
   {
     public:
       typedef sensor_msgs::PointCloud2 PointCloud2;
@@ -76,11 +76,10 @@ namespace pcl_ros
       virtual ~PointCloudConcatenateDataSynchronizer () {};
 
       void onInit ();
+      void subscribe ();
+      void unsubscribe ();
 
     private:
-      /** \brief ROS local node handle. */
-      ros::NodeHandle private_nh_;
-      
       /** \brief The output PointCloud publisher. */
       ros::Publisher pub_output_;
 
@@ -96,6 +95,9 @@ namespace pcl_ros
       /** \brief Output TF frame the concatenated points should be transformed to. */
       std::string output_frame_;
 
+      /** \brief Input point cloud topics. */
+      XmlRpc::XmlRpcValue input_topics_;
+
       /** \brief TF listener object. */
       tf::TransformListener tf_;
 

pcl_ros/src/pcl_ros/io/concatenate_data.cpp

@@ -46,10 +46,11 @@
 void
 pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
 {
-  private_nh_ = getMTPrivateNodeHandle ();
+  nodelet_topic_tools::NodeletLazy::onInit ();
+
   // ---[ Mandatory parameters
-  private_nh_.getParam ("output_frame", output_frame_);
+  pnh_->getParam ("output_frame", output_frame_);
-  private_nh_.getParam ("approximate_sync", approximate_sync_);
+  pnh_->getParam ("approximate_sync", approximate_sync_);
 
   if (output_frame_.empty ())
   {
@@ -57,41 +58,46 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
     return;
   }
 
-  // ---[ Optional parameters
+  if (!pnh_->getParam ("input_topics", input_topics_))
-  private_nh_.getParam ("max_queue_size", maximum_queue_size_);
-
-  // Output
-  pub_output_ = private_nh_.advertise<PointCloud2> ("output", maximum_queue_size_);
-
-  XmlRpc::XmlRpcValue input_topics;
-  if (!private_nh_.getParam ("input_topics", input_topics))
   {
     NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
     return;
   }
-  // Check the type
+  if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray)
-  switch (input_topics.getType ())
   {
-    case XmlRpc::XmlRpcValue::TypeArray:
+    NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
-    {
+    return;
-      if (input_topics.size () == 1)
+  }
+  if (input_topics_.size () == 1)
   {
     NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
     return;
   }
-
+  if (input_topics_.size () > 8)
-      if (input_topics.size () > 8)
   {
     NODELET_ERROR ("[onInit] More than 8 topics passed!");
     return;
   }
 
-      ROS_INFO_STREAM ("[onInit] Subscribing to " << input_topics.size () << " user given topics as inputs:");
+  // ---[ Optional parameters
-      for (int d = 0; d < input_topics.size (); ++d)
+  pnh_->getParam ("max_queue_size", maximum_queue_size_);
-        ROS_INFO_STREAM (" - " << (std::string)(input_topics[d]));
+
+  // Output
+  pub_output_ = advertise<PointCloud2> (*pnh_, "output", maximum_queue_size_);
+
+  onInitPostProcess ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe ()
+{
+  ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:");
+  for (int d = 0; d < input_topics_.size (); ++d)
+    ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d]));
 
   // Subscribe to the filters
-      filters_.resize (input_topics.size ());
+  filters_.resize (input_topics_.size ());
 
   // 8 topics
   if (approximate_sync_)
@@ -105,17 +111,17 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
                                                                             PointCloud2, PointCloud2>
                                                   > (maximum_queue_size_));
 
-      // First input_topics.size () filters are valid
+  // First input_topics_.size () filters are valid
-      for (int d = 0; d < input_topics.size (); ++d)
+  for (int d = 0; d < input_topics_.size (); ++d)
   {
     filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
-        filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_);
+    filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
   }
 
   // Bogus null filter
   filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
 
-      switch (input_topics.size ())
+  switch (input_topics_.size ())
   {
     case 2:
     {
@@ -175,15 +181,7 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
     }
     default:
     {
-          NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
+      NODELET_FATAL ("Invalid 'input_topics' parameter given!");
-          return;
-        }
-      }
-      break;
-    }
-    default:
-    {
-      NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
       return;
     }
   }
@@ -194,6 +192,16 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
     ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe ()
+{
+  for (int d = 0; d < filters_.size (); ++d)
+  {
+    filters_[d]->unsubscribe ();
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void 
 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)