apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

LazyNodelet for io/PointCloudConcatenateFieldsSynchronizer

Browse Source
This commit is contained in:
Kentaro Wada 2017-08-22 17:15:43 +00:00 committed by Paul Bovbel
parent 7124f54462
commit 2d13abfc8e
2 changed files with 27 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
pcl_ros/include/pcl_ros/io/concatenate_fields.h
View File

@ -39,7 +39,7 @@
#define PCL_IO_CONCATENATE_FIELDS_H_ #define PCL_IO_CONCATENATE_FIELDS_H_
// ROS includes // ROS includes
#include <nodelet/nodelet.h> #include <nodelet_topic_tools/nodelet_lazy.h>
#include <message_filters/subscriber.h> #include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h> #include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h> #include <message_filters/sync_policies/exact_time.h>
@ -54,7 +54,7 @@ namespace pcl_ros
* a single PointCloud output message. * a single PointCloud output message.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class PointCloudConcatenateFieldsSynchronizer: public nodelet::Nodelet class PointCloudConcatenateFieldsSynchronizer: public nodelet_topic_tools::NodeletLazy
{ {
public: public:
typedef sensor_msgs::PointCloud2 PointCloud; typedef sensor_msgs::PointCloud2 PointCloud;
@ -73,12 +73,11 @@ namespace pcl_ros
virtual ~PointCloudConcatenateFieldsSynchronizer () {}; virtual ~PointCloudConcatenateFieldsSynchronizer () {};
void onInit (); void onInit ();
void subscribe ();
void unsubscribe ();
void input_callback (const PointCloudConstPtr &cloud); void input_callback (const PointCloudConstPtr &cloud);
private: private:
/** \brief ROS local node handle. */
ros::NodeHandle private_nh_;
/** \brief The input PointCloud subscriber. */ /** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_; ros::Subscriber sub_input_;

30
pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
View File

@ -47,9 +47,10 @@
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
{ {
private_nh_ = getMTPrivateNodeHandle (); nodelet_topic_tools::NodeletLazy::onInit ();
// ---[ Mandatory parameters // ---[ 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!"); NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
return; return;
@ -60,10 +61,25 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
private_nh_.getParam ("max_queue_size", maximum_queue_size_); pnh_->getParam ("max_queue_size", maximum_queue_size_);
private_nh_.getParam ("maximum_seconds", maximum_seconds_); pnh_->getParam ("maximum_seconds", maximum_seconds_);
sub_input_ = private_nh_.subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); pub_output_ = advertise<sensor_msgs::PointCloud2> (*pnh_, "output", maximum_queue_size_);
pub_output_ = private_nh_.advertise<sensor_msgs::PointCloud2> ("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) 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.", 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 // Erase old data in the queue
if (maximum_seconds_ > 0 && queue_.size () > 0) if (maximum_seconds_ > 0 && queue_.size () > 0)