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

LazyNodelet for segmentation/SACSegmentation

Browse Source
This commit is contained in:
Kentaro Wada 2017-08-22 16:42:33 +00:00 committed by Paul Bovbel
parent ebe25b4b89
commit 3ef2b1bf8a
2 changed files with 71 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h
View File

@ -117,6 +117,10 @@ namespace pcl_ros
/** \brief Nodelet initialization routine. */
virtual void onInit ();
/** \brief LazyNodelet connection routine. */
virtual void subscribe ();
virtual void unsubscribe ();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level

117
pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp
View File

@ -50,54 +50,10 @@ pcl_ros::SACSegmentation::onInit ()
// Call the super onInit ()
PCLNodelet::onInit ();
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
// Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
// when "use_indices" is set to true, and "latched_indices" is set to true,
// we'll subscribe and get a separate callback for PointIndices that will
// save the indices internally, and a PointCloud + PointIndices callback
// will take care of meshing the new PointClouds with the old saved indices.
if (latched_indices_)
{
// Subscribe to a callback that saves the indices
sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
// Subscribe to a callback that sets the header of the saved indices to the cloud header
sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
// Synchronize the two topics. No need for an approximate synchronizer here, as we'll
// match the timestamps exactly
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
}
// "latched_indices" not set, proceed with regular <input,indices> pairs
else
{
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
}
}
}
else
// Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
// Advertise the output topics
pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
pub_model_ = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
pub_indices_ = advertise<PointIndices> (*pnh_, "inliers", max_queue_size_);
pub_model_ = advertise<ModelCoefficients> (*pnh_, "model", max_queue_size_);
// ---[ Mandatory parameters
int model_type;
@ -167,6 +123,71 @@ pcl_ros::SACSegmentation::onInit ()
impl_.setModelType (model_type);
impl_.setMethodType (method_type);
impl_.setAxis (axis);
onInitPostProcess ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SACSegmentation::subscribe ()
{
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
// Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
// when "use_indices" is set to true, and "latched_indices" is set to true,
// we'll subscribe and get a separate callback for PointIndices that will
// save the indices internally, and a PointCloud + PointIndices callback
// will take care of meshing the new PointClouds with the old saved indices.
if (latched_indices_)
{
// Subscribe to a callback that saves the indices
sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
// Subscribe to a callback that sets the header of the saved indices to the cloud header
sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
// Synchronize the two topics. No need for an approximate synchronizer here, as we'll
// match the timestamps exactly
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
}
// "latched_indices" not set, proceed with regular <input,indices> pairs
else
{
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
}
}
}
else
// Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SACSegmentation::unsubscribe ()
{
if (use_indices_)
{
sub_input_filter_.unsubscribe ();
sub_indices_filter_.unsubscribe ();
}
else
sub_input_.shutdown ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
@ -249,10 +270,6 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
{
boost::mutex::scoped_lock lock (mutex_);
// No subscribers, no work
if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
return;
pcl_msgs::PointIndices inliers;
pcl_msgs::ModelCoefficients model;
// Enforce that the TF frame and the timestamp are copied