diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.h b/pcl_ros/include/pcl_ros/surface/convex_hull.h index 59fa4f6f..fa8d11d7 100644 --- a/pcl_ros/include/pcl_ros/surface/convex_hull.h +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.h @@ -63,6 +63,10 @@ namespace pcl_ros /** \brief Nodelet initialization routine. */ virtual void onInit (); + /** \brief LazyNodelet connection routine. */ + virtual void subscribe (); + virtual void unsubscribe (); + /** \brief Input point cloud callback. * \param cloud the pointer to the input point cloud * \param indices the pointer to the input point cloud indices diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 97417e58..fffaf568 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -44,20 +44,33 @@ void pcl_ros::ConvexHull2D::onInit () { - ros::NodeHandle pnh = getMTPrivateNodeHandle (); - pub_output_ = pnh.advertise ("output", max_queue_size_); - pub_plane_ = pnh.advertise("output_polygon", max_queue_size_); + PCLNodelet::onInit (); + + pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_plane_ = advertise (*pnh_, "output_polygon", max_queue_size_); // ---[ Optional parameters - pnh.getParam ("use_indices", use_indices_); + pnh_->getParam ("use_indices", use_indices_); + NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - use_indices : %s", + getName ().c_str (), + (use_indices_) ? "true" : "false"); + + onInitPostProcess(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::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", 1); + sub_input_filter_.subscribe (*pnh_, "input", 1); // If indices are enabled, subscribe to the indices - sub_indices_filter_.subscribe (pnh, "indices", 1); + sub_indices_filter_.subscribe (*pnh_, "indices", 1); if (approximate_sync_) { @@ -76,12 +89,20 @@ pcl_ros::ConvexHull2D::onInit () } else // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh.subscribe ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); + sub_input_ = pnh_->subscribe ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); +} - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_indices : %s", - getName ().c_str (), - (use_indices_) ? "true" : "false"); +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::unsubscribe() +{ + if (use_indices_) + { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } + else + sub_input_.shutdown(); } //////////////////////////////////////////////////////////////////////////////////////////////