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

Inherit NodeletLazy for pipeline with less cpu load

Browse Source
This commit is contained in:
Kentaro Wada 2017-08-09 04:46:35 +09:00 committed by Paul Bovbel
parent fa0813e124
commit c370da8a48
4 changed files with 57 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
pcl_ros/CMakeLists.txt
View File

@ -76,6 +76,7 @@ catkin_package(
dynamic_reconfigure
message_filters
nodelet
nodelet_topic_tools
pcl_conversions
pcl_msgs
rosbag

8
pcl_ros/include/pcl_ros/filters/filter.h
View File

@ -114,6 +114,14 @@ namespace pcl_ros
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output) = 0;
/** \brief Lazy transport subscribe routine. */
virtual void
subscribe();
/** \brief Lazy transport unsubscribe routine. */
virtual void
unsubscribe();
/** \brief Nodelet initialization routine. */
virtual void
onInit ();

15
pcl_ros/include/pcl_ros/pcl_nodelet.h
View File

@ -52,7 +52,7 @@
#include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/point_cloud.h"
// ROS Nodelet includes
#include <nodelet/nodelet.h>
#include <nodelet_topic_tools/nodelet_lazy.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
@ -69,7 +69,7 @@ namespace pcl_ros
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */
class PCLNodelet : public nodelet::Nodelet
class PCLNodelet : public nodelet_topic_tools::NodeletLazy
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
@ -94,9 +94,6 @@ namespace pcl_ros
max_queue_size_ (3), approximate_sync_ (false) {};
protected:
/** \brief The ROS NodeHandle used for parameters, publish/subscribe, etc. */
boost::shared_ptr<ros::NodeHandle> pnh_;
/** \brief Set to true if point indices are used.
*
* When receiving a point cloud, if use_indices_ is false, the entire
@ -197,12 +194,16 @@ namespace pcl_ros
return (true);
}
/** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */
virtual void subscribe () {}
virtual void unsubscribe () {}
/** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
virtual void
onInit ()
{
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
nodelet_topic_tools::NodeletLazy::onInit();
// Parameters that we care about only at startup
pnh_->getParam ("max_queue_size", max_queue_size_);

66
pcl_ros/src/pcl_ros/filters/filter.cpp
View File

@ -104,29 +104,8 @@ pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const Indic
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Filter::onInit ()
pcl_ros::Filter::subscribe()
{
// Call the super onInit ()
PCLNodelet::onInit ();
// Call the child's local init
bool has_service = false;
if (!child_init (*pnh_, has_service))
{
NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
return;
}
pub_output_ = pnh_->advertise<PointCloud2> ("output", max_queue_size_);
// Enable the dynamic reconfigure service
if (!has_service)
{
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2);
srv_->setCallback (f);
}
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
@ -150,6 +129,45 @@ pcl_ros::Filter::onInit ()
else
// Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Filter::unsubscribe()
{
if (use_indices_)
{
sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe();
}
else
sub_input_.shutdown();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Filter::onInit ()
{
// Call the super onInit ()
PCLNodelet::onInit ();
// Call the child's local init
bool has_service = false;
if (!child_init (*pnh_, has_service))
{
NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
return;
}
pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
// Enable the dynamic reconfigure service
if (!has_service)
{
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2);
srv_->setCallback (f);
}
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
}
@ -175,10 +193,6 @@ pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
void
pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
return;
// If cloud is given, check if it's valid
if (!isValid (cloud))
{