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");
}
