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

LazyNodelet for io/PointCloudConcatenateDataSynchronizer

Browse Source
This commit is contained in:
Kentaro Wada 2017-08-22 17:07:46 +00:00 committed by Paul Bovbel
parent 071de1e3b4
commit 7124f54462
2 changed files with 134 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
pcl_ros/include/pcl_ros/io/concatenate_data.h
View File

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

244
pcl_ros/src/pcl_ros/io/concatenate_data.cpp
View File

@ -46,10 +46,11 @@
void void
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
{ {
private_nh_ = getMTPrivateNodeHandle (); nodelet_topic_tools::NodeletLazy::onInit ();
// ---[ Mandatory parameters // ---[ 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 ()) if (output_frame_.empty ())
{ {
@ -57,133 +58,130 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
return; 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!"); NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
return; 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)
{
NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
return;
}
if (input_topics_.size () > 8)
{
NODELET_ERROR ("[onInit] More than 8 topics passed!");
return;
}
// ---[ Optional parameters
pnh_->getParam ("max_queue_size", maximum_queue_size_);
// 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 ());
// 8 topics
if (approximate_sync_)
ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2>
> (maximum_queue_size_));
else
ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2>
> (maximum_queue_size_));
// First input_topics_.size () filters are valid
for (int d = 0; d < input_topics_.size (); ++d)
{
filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
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 ())
{
case 2:
{ {
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)
{
NODELET_ERROR ("[onInit] More than 8 topics passed!");
return;
}
ROS_INFO_STREAM ("[onInit] 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 ());
// 8 topics
if (approximate_sync_) if (approximate_sync_)
ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2, ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2>
> (maximum_queue_size_));
else else
ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2, ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
PointCloud2, PointCloud2, PointCloud2, break;
PointCloud2, PointCloud2> }
> (maximum_queue_size_)); case 3:
{
// First input_topics.size () filters are valid if (approximate_sync_)
for (int d = 0; d < input_topics.size (); ++d) ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
{ else
filters_[d].reset (new message_filters::Subscriber<PointCloud2> ()); ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_); break;
} }
case 4:
// Bogus null filter {
filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1)); if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
switch (input_topics.size ()) else
{ ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
case 2: break;
{ }
if (approximate_sync_) case 5:
ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); {
else if (approximate_sync_)
ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
break; else
} ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
case 3: break;
{ }
if (approximate_sync_) case 6:
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); {
else if (approximate_sync_)
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
break; else
} ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
case 4: break;
{ }
if (approximate_sync_) case 7:
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); {
else if (approximate_sync_)
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
break; else
} ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
case 5: break;
{ }
if (approximate_sync_) case 8:
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); {
else if (approximate_sync_)
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
break; else
} ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
case 6:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
break;
}
case 7:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
break;
}
case 8:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
break;
}
default:
{
NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
return;
}
}
break; break;
} }
default: default:
{ {
NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!"); NODELET_FATAL ("Invalid 'input_topics' parameter given!");
return; 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)); 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 void
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out) pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)