From 9ac65d9d208f1ab9f75973ef7b5cde8bda22a423 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Tue, 22 Aug 2017 17:40:39 +0000 Subject: [PATCH] LazyNodelet for features/* --- pcl_ros/include/pcl_ros/features/boundary.h | 2 +- pcl_ros/include/pcl_ros/features/feature.h | 8 ++ pcl_ros/include/pcl_ros/features/fpfh.h | 2 +- pcl_ros/include/pcl_ros/features/fpfh_omp.h | 2 +- .../pcl_ros/features/moment_invariants.h | 2 +- pcl_ros/include/pcl_ros/features/normal_3d.h | 2 +- .../include/pcl_ros/features/normal_3d_omp.h | 2 +- .../include/pcl_ros/features/normal_3d_tbb.h | 2 +- pcl_ros/include/pcl_ros/features/pfh.h | 2 +- .../pcl_ros/features/principal_curvatures.h | 2 +- pcl_ros/include/pcl_ros/features/shot.h | 2 +- pcl_ros/include/pcl_ros/features/shot_omp.h | 2 +- pcl_ros/include/pcl_ros/features/vfh.h | 2 +- pcl_ros/src/pcl_ros/features/feature.cpp | 87 +++++++++++++++---- 14 files changed, 90 insertions(+), 29 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/boundary.h b/pcl_ros/include/pcl_ros/features/boundary.h index 39a0d10e..84a92d80 100644 --- a/pcl_ros/include/pcl_ros/features/boundary.h +++ b/pcl_ros/include/pcl_ros/features/boundary.h @@ -65,7 +65,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h index ca0dccc0..af128950 100644 --- a/pcl_ros/include/pcl_ros/features/feature.h +++ b/pcl_ros/include/pcl_ros/features/feature.h @@ -165,6 +165,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief NodeletLazy connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. * \param cloud the pointer to the input point cloud * \param cloud_surface the pointer to the surface point cloud @@ -224,6 +228,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief NodeletLazy connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set. * \param cloud the pointer to the input point cloud * \param cloud_normals the pointer to the input point cloud normals diff --git a/pcl_ros/include/pcl_ros/features/fpfh.h b/pcl_ros/include/pcl_ros/features/fpfh.h index aca59cd3..9f7800b5 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh.h +++ b/pcl_ros/include/pcl_ros/features/fpfh.h @@ -77,7 +77,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.h b/pcl_ros/include/pcl_ros/features/fpfh_omp.h index 2db8eb18..c024bbef 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh_omp.h +++ b/pcl_ros/include/pcl_ros/features/fpfh_omp.h @@ -74,7 +74,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.h b/pcl_ros/include/pcl_ros/features/moment_invariants.h index ef6df20a..3915bc92 100644 --- a/pcl_ros/include/pcl_ros/features/moment_invariants.h +++ b/pcl_ros/include/pcl_ros/features/moment_invariants.h @@ -61,7 +61,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.h b/pcl_ros/include/pcl_ros/features/normal_3d.h index 7a1506e4..468a069c 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d.h @@ -63,7 +63,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h b/pcl_ros/include/pcl_ros/features/normal_3d_omp.h index ab265162..0cfe81fb 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d_omp.h @@ -59,7 +59,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h index ec570f75..7c2b8050 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h +++ b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h @@ -62,7 +62,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/pfh.h b/pcl_ros/include/pcl_ros/features/pfh.h index 262ce37c..faef1bc9 100644 --- a/pcl_ros/include/pcl_ros/features/pfh.h +++ b/pcl_ros/include/pcl_ros/features/pfh.h @@ -77,7 +77,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.h b/pcl_ros/include/pcl_ros/features/principal_curvatures.h index d7dcdf49..0ffc0722 100644 --- a/pcl_ros/include/pcl_ros/features/principal_curvatures.h +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.h @@ -63,7 +63,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/shot.h b/pcl_ros/include/pcl_ros/features/shot.h index 73c0d81c..a3a3eea9 100644 --- a/pcl_ros/include/pcl_ros/features/shot.h +++ b/pcl_ros/include/pcl_ros/features/shot.h @@ -57,7 +57,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.h b/pcl_ros/include/pcl_ros/features/shot_omp.h index bc28939c..409f6b79 100644 --- a/pcl_ros/include/pcl_ros/features/shot_omp.h +++ b/pcl_ros/include/pcl_ros/features/shot_omp.h @@ -56,7 +56,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/include/pcl_ros/features/vfh.h b/pcl_ros/include/pcl_ros/features/vfh.h index 90e588f1..0bc6fae6 100644 --- a/pcl_ros/include/pcl_ros/features/vfh.h +++ b/pcl_ros/include/pcl_ros/features/vfh.h @@ -62,7 +62,7 @@ namespace pcl_ros childInit (ros::NodeHandle &nh) { // Create the output publisher - pub_output_ = nh.advertise ("output", max_queue_size_); + pub_output_ = advertise (nh, "output", max_queue_size_); return (true); } diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index 0625279b..b7027c70 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -85,6 +85,21 @@ pcl_ros::Feature::onInit () dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); srv_->setCallback (f); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + + onInitPostProcess (); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::subscribe () +{ // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages if (use_indices_ || use_surface_) { @@ -138,14 +153,26 @@ pcl_ros::Feature::onInit () else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +//////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Feature::unsubscribe () +{ + if (use_indices_ || use_surface_) + { + sub_input_filter_.unsubscribe (); + if (use_indices_) + { + sub_indices_filter_.unsubscribe (); + if (use_surface_) + sub_surface_filter_.unsubscribe (); + } + else + sub_surface_filter_.unsubscribe (); + } + else + sub_input_.shutdown (); } //////////////////////////////////////////////////////////////////////////////////////////// @@ -273,14 +300,29 @@ pcl_ros::FeatureFromNormals::onInit () // ---[ Optional parameters pnh_->getParam ("use_surface", use_surface_); - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); - // Enable the dynamic reconfigure service srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); srv_->setCallback (f); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_surface : %s\n" + " - k_search : %d\n" + " - radius_search : %f\n" + " - spatial_locator: %d", + getName ().c_str (), + (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + + onInitPostProcess (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::subscribe () +{ + sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); + sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); + // Create the objects here if (approximate_sync_) sync_input_normals_surface_indices_a_ = boost::make_shared > > (max_queue_size_); @@ -341,14 +383,25 @@ pcl_ros::FeatureFromNormals::onInit () sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); else sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::FeatureFromNormals::unsubscribe () +{ + sub_input_filter_.unsubscribe (); + sub_normals_filter_.unsubscribe (); + if (use_indices_ || use_surface_) + { + if (use_indices_) + { + sub_indices_filter_.unsubscribe (); + if (use_surface_) + sub_surface_filter_.unsubscribe (); + } + else + sub_surface_filter_.unsubscribe (); + } } //////////////////////////////////////////////////////////////////////////////////////////////