void pcl_ros::PieceExtraction::onInit () { // Call the super onInit () pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); // Parameters that we care about only at startup pnh_->getParam ("max_queue_size", max_queue_size_); // ---[ Optional parameters pnh_->getParam ("use_indices", use_indices_); pnh_->getParam ("latched_indices", latched_indices_); pnh_->getParam ("approximate_sync", approximate_sync_); NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n" " - approximate_sync : %s\n" " - use_indices : %s\n" " - latched_indices : %s\n" " - max_queue_size : %d", getName ().c_str (), (approximate_sync_) ? "true" : "false", (use_indices_) ? "true" : "false", (latched_indices_) ? "true" : "false", max_queue_size_); // ---[ Mandatory parameters double cluster_tolerance; if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance)) { NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ()); return; } int spatial_locator; if (!pnh_->getParam ("spatial_locator", spatial_locator)) { NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); return; } // output pub_output_ = pnh_->advertise<chess_msgs::ChessBoard>("output", max_queue_size_); //pub_output_ = pnh_->advertise<PointCloud>("output", max_queue_size_); // Enable the dynamic reconfigure service srv_ = boost::make_shared <dynamic_reconfigure::Server<PieceExtractionConfig> > (*pnh_); dynamic_reconfigure::Server<PieceExtractionConfig>::CallbackType f = boost::bind (&PieceExtraction::config_callback, this, _1, _2); srv_->setCallback (f); // 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_); if (approximate_sync_) { sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::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 (&PieceExtraction::input_indices_callback, this, _1, _2)); } else { sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::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 (&PieceExtraction::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 (&PieceExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ())); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" " - max_queue_size : %d\n" " - use_indices : %s\n" " - cluster_tolerance : %f\n" " - spatial_locator : %d", getName ().c_str (), max_queue_size_, (use_indices_) ? "true" : "false", cluster_tolerance, spatial_locator); // Set given parameters here impl_.setSpatialLocator (spatial_locator); impl_.setClusterTolerance (cluster_tolerance); }
void pcl_ros::MovingLeastSquares::onInit () { PCLNodelet::onInit (); //ros::NodeHandle private_nh = getMTPrivateNodeHandle (); pub_output_ = pnh_->advertise<PointCloudIn> ("output", max_queue_size_); pub_normals_ = pnh_->advertise<NormalCloudOut> ("normals", max_queue_size_); //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) if (!pnh_->getParam ("search_radius", search_radius_)) { //NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ()); return; } if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) { NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); return; } // Enable the dynamic reconfigure service srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_); dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 ); srv_->setCallback (f); // ---[ Optional parameters pnh_->getParam ("use_indices", use_indices_); // 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); // If indices are enabled, subscribe to the indices sub_indices_filter_.subscribe (*pnh_, "indices", 1); if (approximate_sync_) { sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_); // surface not enabled, connect the input-indices duo and register sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); } else { sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_); // surface not enabled, connect the input-indices duo and register sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); } } else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1, bind (&MovingLeastSquares::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::Feature::onInit () { // Call the super onInit () PCLNodelet::onInit (); // Call the child init childInit (*pnh_); // Allow each individual class that inherits from us to declare their own Publisher // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); // ---[ Mandatory parameters if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) { NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); return; } if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) { NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); return; } // ---[ Optional parameters pnh_->getParam ("use_surface", use_surface_); // Enable the dynamic reconfigure service srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_); dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); srv_->setCallback (f); // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages if (use_indices_ || use_surface_) { // Create the objects here if (approximate_sync_) sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_); else sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_); // Subscribe to the input using a filter sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); if (use_indices_) { // If indices are enabled, subscribe to the indices sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); if (use_surface_) // Use both indices and surface { // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); if (approximate_sync_) sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); else sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); } else // Use only indices { sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); // surface not enabled, connect the input-indices duo and register if (approximate_sync_) sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); else sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); } } else // Use only surface { sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); // indices not enabled, connect the input-surface duo and register sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); if (approximate_sync_) sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); else sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); } // Register callbacks if (approximate_sync_) sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); else sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); } else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh_->subscribe<PointCloudIn> ("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::ConvexHull2D::onInit () { ros::NodeHandle pnh = getMTPrivateNodeHandle (); pub_output_ = pnh.advertise<PointCloud> ("output", max_queue_size_); pub_plane_ = pnh.advertise<geometry_msgs::PolygonStamped>("output_polygon", max_queue_size_); // ---[ Optional parameters pnh.getParam ("use_indices", use_indices_); // 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); // If indices are enabled, subscribe to the indices sub_indices_filter_.subscribe (pnh, "indices", 1); if (approximate_sync_) { sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_); // surface not enabled, connect the input-indices duo and register sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::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_); // surface not enabled, connect the input-indices duo and register sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2)); } } else // Subscribe in an old fashion to input only (no filters) sub_input_ = pnh.subscribe<PointCloud> ("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"); }