diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.h b/pcl_ros/include/pcl_ros/io/concatenate_fields.h index 13a9ced3..efa1c458 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.h +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.h @@ -39,7 +39,7 @@ #define PCL_IO_CONCATENATE_FIELDS_H_ // ROS includes -#include +#include #include #include #include @@ -54,7 +54,7 @@ namespace pcl_ros * a single PointCloud output message. * \author Radu Bogdan Rusu */ - class PointCloudConcatenateFieldsSynchronizer: public nodelet::Nodelet + class PointCloudConcatenateFieldsSynchronizer: public nodelet_topic_tools::NodeletLazy { public: typedef sensor_msgs::PointCloud2 PointCloud; @@ -73,12 +73,11 @@ namespace pcl_ros virtual ~PointCloudConcatenateFieldsSynchronizer () {}; void onInit (); + void subscribe (); + void unsubscribe (); void input_callback (const PointCloudConstPtr &cloud); private: - /** \brief ROS local node handle. */ - ros::NodeHandle private_nh_; - /** \brief The input PointCloud subscriber. */ ros::Subscriber sub_input_; diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index ea64c1f6..159f531f 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -47,9 +47,10 @@ void pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () { - private_nh_ = getMTPrivateNodeHandle (); + nodelet_topic_tools::NodeletLazy::onInit (); + // ---[ Mandatory parameters - if (!private_nh_.getParam ("input_messages", input_messages_)) + if (!pnh_->getParam ("input_messages", input_messages_)) { NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!"); return; @@ -60,10 +61,25 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () return; } // ---[ Optional parameters - private_nh_.getParam ("max_queue_size", maximum_queue_size_); - private_nh_.getParam ("maximum_seconds", maximum_seconds_); - sub_input_ = private_nh_.subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); - pub_output_ = private_nh_.advertise ("output", maximum_queue_size_); + pnh_->getParam ("max_queue_size", maximum_queue_size_); + pnh_->getParam ("maximum_seconds", maximum_seconds_); + pub_output_ = advertise (*pnh_, "output", maximum_queue_size_); + + onInitPostProcess (); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe () +{ + sub_input_ = pnh_->subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe () +{ + sub_input_.shutdown (); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -71,7 +87,7 @@ void pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud) { NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), private_nh_.resolveName ("input").c_str ()); + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); // Erase old data in the queue if (maximum_seconds_ > 0 && queue_.size () > 0)