Apply ament_uncrustify --reformat (#297)
Signed-off-by: Sean Kelly <sean@seankelly.dev>
This commit is contained in:
Sean Kelly
GitHub
@@ -39,35 +39,36 @@
|
||||
#include "pcl_ros/features/boundary.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
pcl_ros::BoundaryEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
|
||||
|
||||
@@ -53,219 +53,263 @@
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::onInit ()
|
||||
pcl_ros::Feature::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
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 ());
|
||||
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 ());
|
||||
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_);
|
||||
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);
|
||||
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);
|
||||
|
||||
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_);
|
||||
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 ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::subscribe ()
|
||||
pcl_ros::Feature::subscribe()
|
||||
{
|
||||
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
|
||||
if (use_indices_ || use_surface_)
|
||||
{
|
||||
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_);
|
||||
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_)
|
||||
{
|
||||
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
|
||||
{
|
||||
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));
|
||||
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_);
|
||||
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));
|
||||
} 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_);
|
||||
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 ()));
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
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 ();
|
||||
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
|
||||
sub_surface_filter_.unsubscribe ();
|
||||
} 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()));
|
||||
}
|
||||
else
|
||||
sub_input_.shutdown ();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level)
|
||||
pcl_ros::Feature::unsubscribe()
|
||||
{
|
||||
if (k_ != config.k_search)
|
||||
{
|
||||
k_ = config.k_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
|
||||
}
|
||||
if (search_radius_ != config.radius_search)
|
||||
{
|
||||
search_radius_ = config.radius_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
|
||||
pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
|
||||
{
|
||||
if (k_ != config.k_search) {
|
||||
k_ = config.k_search;
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
|
||||
}
|
||||
if (search_radius_ != config.radius_search) {
|
||||
search_radius_ = config.radius_search;
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the nearest neighbors search radius for each point: %f.",
|
||||
search_radius_);
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::input_surface_indices_callback(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (!isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If surface is given, check if it's valid
|
||||
if (cloud_surface && !isValid (cloud_surface, "surface"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (cloud_surface && !isValid(cloud_surface, "surface")) {
|
||||
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (cloud_surface)
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
|
||||
|
||||
else
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
if (cloud_surface) {
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(), fromPCL(
|
||||
cloud_surface->header).stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(), fromPCL(
|
||||
cloud_surface->header).stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
|
||||
}
|
||||
} else if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
|
||||
if ((int)(cloud->width * cloud->height) < k_)
|
||||
{
|
||||
NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish (cloud);
|
||||
if ((int)(cloud->width * cloud->height) < k_) {
|
||||
NODELET_ERROR(
|
||||
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_,
|
||||
(int)(cloud->width * cloud->height));
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices given...
|
||||
IndicesPtr vindices;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
if (indices && !indices->header.frame_id.empty()) {
|
||||
vindices.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
computePublish (cloud, cloud_surface, vindices);
|
||||
computePublish(cloud, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -274,223 +318,289 @@ pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cl
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::onInit ()
|
||||
pcl_ros::FeatureFromNormals::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
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 ());
|
||||
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 ());
|
||||
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_);
|
||||
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 (&FeatureFromNormals::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
|
||||
dynamic_reconfigure::Server<FeatureConfig>::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_);
|
||||
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 ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::subscribe ()
|
||||
pcl_ros::FeatureFromNormals::subscribe()
|
||||
{
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
|
||||
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 <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
|
||||
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
|
||||
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
|
||||
}
|
||||
|
||||
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
|
||||
if (use_indices_ || use_surface_)
|
||||
{
|
||||
if (use_indices_)
|
||||
{
|
||||
if (use_indices_ || use_surface_) {
|
||||
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
|
||||
{
|
||||
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_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
}
|
||||
else // Use only indices
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_)
|
||||
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_,
|
||||
sub_surface_filter_,
|
||||
sub_indices_filter_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_,
|
||||
sub_surface_filter_,
|
||||
sub_indices_filter_);
|
||||
}
|
||||
} else { // Use only indices
|
||||
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_) {
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_, nf_pc_,
|
||||
sub_indices_filter_);
|
||||
} else {
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_, nf_pc_,
|
||||
sub_indices_filter_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else // Use only surface
|
||||
{
|
||||
} else { // Use only surface
|
||||
// indices not enabled, connect the input-surface duo and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
|
||||
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
|
||||
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
sub_surface_filter_, nf_pi_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
sub_surface_filter_, nf_pi_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
} else {
|
||||
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
nf_pc_, nf_pi_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
nf_pc_, nf_pi_);
|
||||
}
|
||||
}
|
||||
|
||||
// Register callbacks
|
||||
if (approximate_sync_)
|
||||
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));
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
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 ();
|
||||
if (approximate_sync_) {
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
|
||||
const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
|
||||
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
|
||||
const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
|
||||
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// If cloud+normals is given, check if it's valid
|
||||
if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
|
||||
NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If surface is given, check if it's valid
|
||||
if (cloud_surface && !isValid (cloud_surface, "surface"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (cloud_surface && !isValid(cloud_surface, "surface")) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_normals_surface_indices_callback] Invalid input surface!",
|
||||
getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_normals_surface_indices_callback] Invalid input indices!",
|
||||
getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (cloud_surface)
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
|
||||
else
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
|
||||
if (cloud_surface) {
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(), fromPCL(
|
||||
cloud_surface->header).stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(), fromPCL(
|
||||
cloud_normals->header).stamp.toSec(),
|
||||
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(), fromPCL(
|
||||
cloud_surface->header).stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(), fromPCL(
|
||||
cloud_normals->header).stamp.toSec(),
|
||||
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
|
||||
}
|
||||
} else if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(), fromPCL(
|
||||
cloud_normals->header).stamp.toSec(),
|
||||
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(), fromPCL(
|
||||
cloud_normals->header).stamp.toSec(),
|
||||
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
if ((int)(cloud->width * cloud->height) < k_)
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish (cloud);
|
||||
if ((int)(cloud->width * cloud->height) < k_) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!",
|
||||
getName().c_str(), k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices given...
|
||||
IndicesPtr vindices;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
if (indices && !indices->header.frame_id.empty()) {
|
||||
vindices.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
computePublish (cloud, cloud_normals, cloud_surface, vindices);
|
||||
computePublish(cloud, cloud_normals, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
|
||||
@@ -38,39 +38,39 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::FPFHEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimation FPFHEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -38,39 +38,39 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh_omp.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
|
||||
PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -38,37 +38,37 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/moment_invariants.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -38,37 +38,37 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::NormalEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimation NormalEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -38,37 +38,37 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d_omp.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -40,42 +40,42 @@
|
||||
|
||||
#if defined HAVE_TBB
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloud output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet)
|
||||
|
||||
#endif // HAVE_TBB
|
||||
|
||||
|
||||
@@ -38,39 +38,39 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/pfh.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::PFHEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::PFHEstimation PFHEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -38,39 +38,39 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/principal_curvatures.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -37,39 +37,39 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/shot.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::SHOTEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::SHOTEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::SHOTEstimation SHOTEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -37,39 +37,39 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/shot_omp.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::SHOTEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::SHOTEstimationOMP::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP;
|
||||
PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -38,39 +38,39 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/vfh.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::VFHEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (pcl_ptr(surface));
|
||||
impl_.setInputNormals (pcl_ptr(normals));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(pcl_ptr(surface));
|
||||
impl_.setInputNormals(pcl_ptr(normals));
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::VFHEstimation VFHEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
*
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
@@ -32,7 +32,7 @@
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: cropbox.cpp
|
||||
* $Id: cropbox.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -41,24 +41,25 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::CropBox::child_init (ros::NodeHandle &nh, bool &has_service)
|
||||
pcl_ros::CropBox::child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
{
|
||||
// Enable the dynamic reconfigure service
|
||||
has_service = true;
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::CropBoxConfig> > (nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>::CallbackType f = boost::bind (&CropBox::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>>(nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>::CallbackType f = boost::bind(
|
||||
&CropBox::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t level)
|
||||
pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
Eigen::Vector4f min_point,max_point;
|
||||
Eigen::Vector4f min_point, max_point;
|
||||
min_point = impl_.getMin();
|
||||
max_point = impl_.getMax();
|
||||
|
||||
@@ -67,49 +68,54 @@ pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t leve
|
||||
new_max_point << config.max_x, config.max_y, config.max_z, 0.0;
|
||||
|
||||
// Check the current values for minimum point
|
||||
if (min_point != new_min_point)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the minimum point to: %f %f %f.", getName ().c_str (), new_min_point(0),new_min_point(1),new_min_point(2));
|
||||
if (min_point != new_min_point) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the minimum point to: %f %f %f.",
|
||||
getName().c_str(), new_min_point(0), new_min_point(1), new_min_point(2));
|
||||
// Set the filter min point if different
|
||||
impl_.setMin(new_min_point);
|
||||
}
|
||||
// Check the current values for the maximum point
|
||||
if (max_point != new_max_point)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the maximum point to: %f %f %f.", getName ().c_str (), new_max_point(0),new_max_point(1),new_max_point(2));
|
||||
// Check the current values for the maximum point
|
||||
if (max_point != new_max_point) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the maximum point to: %f %f %f.",
|
||||
getName().c_str(), new_max_point(0), new_max_point(1), new_max_point(2));
|
||||
// Set the filter max point if different
|
||||
impl_.setMax(new_max_point);
|
||||
}
|
||||
|
||||
// Check the current value for keep_organized
|
||||
if (impl_.getKeepOrganized () != config.keep_organized)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false");
|
||||
if (impl_.getKeepOrganized() != config.keep_organized) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter keep_organized value to: %s.",
|
||||
getName().c_str(), config.keep_organized ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setKeepOrganized (config.keep_organized);
|
||||
impl_.setKeepOrganized(config.keep_organized);
|
||||
}
|
||||
|
||||
// Check the current value for the negative flag
|
||||
if (impl_.getNegative() != config.negative)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.negative ? "true" : "false");
|
||||
if (impl_.getNegative() != config.negative) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter negative flag to: %s.",
|
||||
getName().c_str(), config.negative ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setNegative(config.negative);
|
||||
}
|
||||
|
||||
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
|
||||
if (tf_input_frame_ != config.input_frame)
|
||||
{
|
||||
if (tf_input_frame_ != config.input_frame) {
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the input TF frame to: %s.",
|
||||
getName().c_str(), tf_input_frame_.c_str());
|
||||
}
|
||||
if (tf_output_frame_ != config.output_frame)
|
||||
{
|
||||
if (tf_output_frame_ != config.output_frame) {
|
||||
tf_output_frame_ = config.output_frame;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the output TF frame to: %s.",
|
||||
getName().c_str(), tf_output_frame_.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::CropBox CropBox;
|
||||
PLUGINLIB_EXPORT_CLASS(CropBox,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet);
|
||||
|
||||
@@ -39,32 +39,33 @@
|
||||
#include "pcl_ros/filters/extract_indices.hpp"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::ExtractIndices::child_init (ros::NodeHandle &nh, bool &has_service)
|
||||
bool
|
||||
pcl_ros::ExtractIndices::child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
{
|
||||
has_service = true;
|
||||
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > (nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind (&ExtractIndices::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>>(nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind(
|
||||
&ExtractIndices::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
use_indices_ = true;
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level)
|
||||
pcl_ros::ExtractIndices::config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if (impl_.getNegative () != config.negative)
|
||||
{
|
||||
impl_.setNegative (config.negative);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the extraction to: %s.", getName ().c_str (), (config.negative ? "indices" : "everything but the indices"));
|
||||
if (impl_.getNegative() != config.negative) {
|
||||
impl_.setNegative(config.negative);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the extraction to: %s.", getName().c_str(),
|
||||
(config.negative ? "indices" : "everything but the indices"));
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::ExtractIndices ExtractIndices;
|
||||
PLUGINLIB_EXPORT_CLASS(ExtractIndices,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet);
|
||||
|
||||
@@ -38,40 +38,41 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/boundary.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
impl_.setInputNormals(normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(BoundaryEstimation,nodelet::Nodelet);
|
||||
PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -53,192 +53,237 @@
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::onInit ()
|
||||
pcl_ros::Feature::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
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 ());
|
||||
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 ());
|
||||
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_);
|
||||
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);
|
||||
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_)
|
||||
{
|
||||
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_);
|
||||
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_)
|
||||
{
|
||||
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
|
||||
{
|
||||
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));
|
||||
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_);
|
||||
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));
|
||||
} 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_);
|
||||
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
|
||||
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 ()));
|
||||
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_);
|
||||
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::config_callback (FeatureConfig &config, uint32_t level)
|
||||
pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
|
||||
{
|
||||
if (k_ != config.k_search)
|
||||
{
|
||||
if (k_ != config.k_search) {
|
||||
k_ = config.k_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
|
||||
}
|
||||
if (search_radius_ != config.radius_search)
|
||||
{
|
||||
if (search_radius_ != config.radius_search) {
|
||||
search_radius_ = config.radius_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the nearest neighbors search radius for each point: %f.",
|
||||
search_radius_);
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
|
||||
pcl_ros::Feature::input_surface_indices_callback(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (!isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If surface is given, check if it's valid
|
||||
if (cloud_surface && !isValid (cloud_surface, "surface"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (cloud_surface && !isValid(cloud_surface, "surface")) {
|
||||
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (cloud_surface)
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
|
||||
|
||||
else
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
if (cloud_surface) {
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(),
|
||||
cloud_surface->header.stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(),
|
||||
cloud_surface->header.stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
|
||||
}
|
||||
} else if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height,
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
|
||||
if ((int)(cloud->width * cloud->height) < k_)
|
||||
{
|
||||
NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish (cloud);
|
||||
if ((int)(cloud->width * cloud->height) < k_) {
|
||||
NODELET_ERROR(
|
||||
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_,
|
||||
(int)(cloud->width * cloud->height));
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices given...
|
||||
IndicesPtr vindices;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
if (indices && !indices->header.frame_id.empty()) {
|
||||
vindices.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
computePublish (cloud, cloud_surface, vindices);
|
||||
computePublish(cloud, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -247,197 +292,264 @@ pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cl
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::onInit ()
|
||||
pcl_ros::FeatureFromNormals::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
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 ());
|
||||
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 ());
|
||||
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_);
|
||||
pnh_->getParam("use_surface", use_surface_);
|
||||
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
|
||||
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<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
|
||||
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
|
||||
&FeatureFromNormals::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
// Create the objects here
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
|
||||
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
|
||||
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
|
||||
}
|
||||
|
||||
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
|
||||
if (use_indices_ || use_surface_)
|
||||
{
|
||||
if (use_indices_)
|
||||
{
|
||||
if (use_indices_ || use_surface_) {
|
||||
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
|
||||
{
|
||||
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_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
}
|
||||
else // Use only indices
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_)
|
||||
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_,
|
||||
sub_surface_filter_,
|
||||
sub_indices_filter_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_,
|
||||
sub_surface_filter_,
|
||||
sub_indices_filter_);
|
||||
}
|
||||
} else { // Use only indices
|
||||
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_) {
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_, nf_pc_,
|
||||
sub_indices_filter_);
|
||||
} else {
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_,
|
||||
sub_normals_filter_, nf_pc_,
|
||||
sub_indices_filter_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else // Use only surface
|
||||
{
|
||||
} else { // Use only surface
|
||||
// indices not enabled, connect the input-surface duo and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
|
||||
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
|
||||
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
sub_surface_filter_, nf_pi_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
sub_surface_filter_, nf_pi_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
} else {
|
||||
sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_normals_surface_indices_a_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
nf_pc_, nf_pi_);
|
||||
} else {
|
||||
sync_input_normals_surface_indices_e_->connectInput(
|
||||
sub_input_filter_, sub_normals_filter_,
|
||||
nf_pc_, nf_pi_);
|
||||
}
|
||||
}
|
||||
|
||||
// Register callbacks
|
||||
if (approximate_sync_)
|
||||
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));
|
||||
if (approximate_sync_) {
|
||||
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_);
|
||||
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::input_normals_surface_indices_callback (
|
||||
const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
|
||||
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
|
||||
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
|
||||
const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
|
||||
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// If cloud+normals is given, check if it's valid
|
||||
if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
|
||||
NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If surface is given, check if it's valid
|
||||
if (cloud_surface && !isValid (cloud_surface, "surface"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (cloud_surface && !isValid(cloud_surface, "surface")) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_normals_surface_indices_callback] Invalid input surface!",
|
||||
getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_normals_surface_indices_callback] Invalid input indices!",
|
||||
getName().c_str());
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (cloud_surface)
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
|
||||
else
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
|
||||
if (cloud_surface) {
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(),
|
||||
cloud_surface->header.stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(),
|
||||
cloud_normals->header.stamp.toSec(),
|
||||
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
|
||||
*cloud_surface).c_str(),
|
||||
cloud_surface->header.stamp.toSec(),
|
||||
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(),
|
||||
cloud_normals->header.stamp.toSec(),
|
||||
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
|
||||
}
|
||||
} else if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(),
|
||||
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"normals").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
|
||||
*cloud_normals).c_str(),
|
||||
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"normals").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
if ((int)(cloud->width * cloud->height) < k_)
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish (cloud);
|
||||
if ((int)(cloud->width * cloud->height) < k_) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!",
|
||||
getName().c_str(), k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish(cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices given...
|
||||
IndicesPtr vindices;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
if (indices && !indices->header.frame_id.empty()) {
|
||||
vindices.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
computePublish (cloud, cloud_normals, cloud_surface, vindices);
|
||||
computePublish(cloud, cloud_normals, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
|
||||
@@ -38,42 +38,42 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::FPFHEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
impl_.setInputNormals(normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimation FPFHEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(FPFHEstimation,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -38,42 +38,42 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh_omp.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
impl_.setInputNormals(normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
|
||||
PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet);
|
||||
|
||||
@@ -38,40 +38,40 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/moment_invariants.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -38,40 +38,40 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::NormalEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimation NormalEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimation,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -38,40 +38,40 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d_omp.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet);
|
||||
|
||||
@@ -40,42 +40,42 @@
|
||||
|
||||
#if defined HAVE_TBB
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloud output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB,nodelet::Nodelet);
|
||||
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet);
|
||||
|
||||
#endif // HAVE_TBB
|
||||
|
||||
|
||||
@@ -38,42 +38,42 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/pfh.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::PFHEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
impl_.setInputNormals(normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PFHEstimation PFHEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(PFHEstimation,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -38,42 +38,42 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/principal_curvatures.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
impl_.setInputNormals(normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -38,42 +38,42 @@
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/vfh.hpp"
|
||||
|
||||
void
|
||||
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
void
|
||||
pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
void
|
||||
pcl_ros::VFHEstimation::computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
impl_.setKSearch(k_);
|
||||
impl_.setRadiusSearch(search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
initTree(spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod(tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
impl_.setInputCloud(cloud);
|
||||
impl_.setIndices(indices);
|
||||
impl_.setSearchSurface(surface);
|
||||
impl_.setInputNormals(normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
impl_.compute(output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
pub_output_.publish(output.makeShared());
|
||||
}
|
||||
|
||||
typedef pcl_ros::VFHEstimation VFHEstimation;
|
||||
PLUGINLIB_EXPORT_CLASS(VFHEstimation,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -61,45 +61,52 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
|
||||
pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices)
|
||||
{
|
||||
PointCloud2 output;
|
||||
// Call the virtual method in the child
|
||||
filter (input, indices, output);
|
||||
filter(input, indices, output);
|
||||
|
||||
PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default
|
||||
PointCloud2::Ptr cloud_tf(new PointCloud2(output)); // set the output by default
|
||||
// Check whether the user has given a different output TF frame
|
||||
if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
|
||||
if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::computePublish] Transforming output dataset from %s to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str());
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud2 cloud_transformed;
|
||||
if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
|
||||
if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_listener_)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::computePublish] Error converting output dataset from %s to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str());
|
||||
return;
|
||||
}
|
||||
cloud_tf.reset (new PointCloud2 (cloud_transformed));
|
||||
cloud_tf.reset(new PointCloud2(cloud_transformed));
|
||||
}
|
||||
if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_)
|
||||
// no tf_output_frame given, transform the dataset to its original frame
|
||||
{
|
||||
NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
|
||||
if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) {
|
||||
// no tf_output_frame given, transform the dataset to its original frame
|
||||
NODELET_DEBUG(
|
||||
"[%s::computePublish] Transforming output dataset from %s back to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud2 cloud_transformed;
|
||||
if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_))
|
||||
if (!pcl_ros::transformPointCloud(
|
||||
tf_input_orig_frame_, output, cloud_transformed,
|
||||
tf_listener_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
|
||||
NODELET_ERROR(
|
||||
"[%s::computePublish] Error converting output dataset from %s back to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
|
||||
return;
|
||||
}
|
||||
cloud_tf.reset (new PointCloud2 (cloud_transformed));
|
||||
cloud_tf.reset(new PointCloud2(cloud_transformed));
|
||||
}
|
||||
|
||||
// Copy timestamp to keep it
|
||||
cloud_tf->header.stamp = input->header.stamp;
|
||||
|
||||
// Publish a boost shared ptr
|
||||
pub_output_.publish (cloud_tf);
|
||||
pub_output_.publish(cloud_tf);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -107,141 +114,152 @@ void
|
||||
pcl_ros::Filter::subscribe()
|
||||
{
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_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_);
|
||||
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<sync_policies::ApproximateTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
|
||||
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
|
||||
if (approximate_sync_) {
|
||||
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
pcl_msgs::PointIndices>>>(max_queue_size_);
|
||||
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
|
||||
} else {
|
||||
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
pcl_msgs::PointIndices>>>(max_queue_size_);
|
||||
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
|
||||
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
}
|
||||
else
|
||||
} 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 ()));
|
||||
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_)
|
||||
{
|
||||
if (use_indices_) {
|
||||
sub_input_filter_.unsubscribe();
|
||||
sub_indices_filter_.unsubscribe();
|
||||
}
|
||||
else
|
||||
} else {
|
||||
sub_input_.shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::onInit ()
|
||||
pcl_ros::Filter::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::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 ());
|
||||
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_);
|
||||
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);
|
||||
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 ());
|
||||
NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str());
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
|
||||
pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
|
||||
{
|
||||
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
|
||||
if (tf_input_frame_ != config.input_frame)
|
||||
{
|
||||
if (tf_input_frame_ != config.input_frame) {
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the input TF frame to: %s.",
|
||||
getName().c_str(), tf_input_frame_.c_str());
|
||||
}
|
||||
if (tf_output_frame_ != config.output_frame)
|
||||
{
|
||||
if (tf_output_frame_ != config.output_frame) {
|
||||
tf_output_frame_ = config.output_frame;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the output TF frame to: %s.",
|
||||
getName().c_str(), tf_output_frame_.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
|
||||
pcl_ros::Filter::input_indices_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
if (!isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.",
|
||||
getName().c_str(), cloud->width * cloud->height,
|
||||
cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
// Check whether the user has given a different input TF frame
|
||||
tf_input_orig_frame_ = cloud->header.frame_id;
|
||||
PointCloud2::ConstPtr cloud_tf;
|
||||
if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
|
||||
if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback] Transforming input dataset from %s to %s.",
|
||||
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
|
||||
// Save the original frame ID
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud2 cloud_transformed;
|
||||
if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
|
||||
if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_indices_callback] Error converting input dataset from %s to %s.",
|
||||
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
|
||||
return;
|
||||
}
|
||||
cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
|
||||
}
|
||||
else
|
||||
cloud_tf = boost::make_shared<PointCloud2>(cloud_transformed);
|
||||
} else {
|
||||
cloud_tf = cloud;
|
||||
}
|
||||
|
||||
// Need setInputCloud () here because we have to extract x/y/z
|
||||
IndicesPtr vindices;
|
||||
if (indices)
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
if (indices) {
|
||||
vindices.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
computePublish (cloud_tf, vindices);
|
||||
computePublish(cloud_tf, vindices);
|
||||
}
|
||||
|
||||
|
||||
@@ -40,81 +40,88 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::PassThrough::child_init (ros::NodeHandle &nh, bool &has_service)
|
||||
pcl_ros::PassThrough::child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
{
|
||||
// Enable the dynamic reconfigure service
|
||||
has_service = true;
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
|
||||
&PassThrough::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
|
||||
pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
double filter_min, filter_max;
|
||||
impl_.getFilterLimits (filter_min, filter_max);
|
||||
impl_.getFilterLimits(filter_min, filter_max);
|
||||
|
||||
// Check the current values for filter min-max
|
||||
if (filter_min != config.filter_limit_min)
|
||||
{
|
||||
if (filter_min != config.filter_limit_min) {
|
||||
filter_min = config.filter_limit_min;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_min);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
|
||||
getName().c_str(), filter_min);
|
||||
// Set the filter min-max if different
|
||||
impl_.setFilterLimits (filter_min, filter_max);
|
||||
impl_.setFilterLimits(filter_min, filter_max);
|
||||
}
|
||||
// Check the current values for filter min-max
|
||||
if (filter_max != config.filter_limit_max)
|
||||
{
|
||||
if (filter_max != config.filter_limit_max) {
|
||||
filter_max = config.filter_limit_max;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_max);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
|
||||
getName().c_str(), filter_max);
|
||||
// Set the filter min-max if different
|
||||
impl_.setFilterLimits (filter_min, filter_max);
|
||||
impl_.setFilterLimits(filter_min, filter_max);
|
||||
}
|
||||
|
||||
// Check the current value for the filter field
|
||||
//std::string filter_field = impl_.getFilterFieldName ();
|
||||
if (impl_.getFilterFieldName () != config.filter_field_name)
|
||||
{
|
||||
if (impl_.getFilterFieldName() != config.filter_field_name) {
|
||||
// Set the filter field if different
|
||||
impl_.setFilterFieldName (config.filter_field_name);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the filter field name to: %s.", getName ().c_str (), config.filter_field_name.c_str ());
|
||||
impl_.setFilterFieldName(config.filter_field_name);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter field name to: %s.",
|
||||
getName().c_str(), config.filter_field_name.c_str());
|
||||
}
|
||||
|
||||
// Check the current value for keep_organized
|
||||
if (impl_.getKeepOrganized () != config.keep_organized)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false");
|
||||
if (impl_.getKeepOrganized() != config.keep_organized) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter keep_organized value to: %s.",
|
||||
getName().c_str(), config.keep_organized ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setKeepOrganized (config.keep_organized);
|
||||
impl_.setKeepOrganized(config.keep_organized);
|
||||
}
|
||||
|
||||
// Check the current value for the negative flag
|
||||
if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
|
||||
if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter negative flag to: %s.",
|
||||
getName().c_str(), config.filter_limit_negative ? "true" : "false");
|
||||
// Call the virtual method in the child
|
||||
impl_.setFilterLimitsNegative (config.filter_limit_negative);
|
||||
impl_.setFilterLimitsNegative(config.filter_limit_negative);
|
||||
}
|
||||
|
||||
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
|
||||
if (tf_input_frame_ != config.input_frame)
|
||||
{
|
||||
if (tf_input_frame_ != config.input_frame) {
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the input TF frame to: %s.",
|
||||
getName().c_str(), tf_input_frame_.c_str());
|
||||
}
|
||||
if (tf_output_frame_ != config.output_frame)
|
||||
{
|
||||
if (tf_output_frame_ != config.output_frame) {
|
||||
tf_output_frame_ = config.output_frame;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the output TF frame to: %s.",
|
||||
getName().c_str(), tf_output_frame_.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::PassThrough PassThrough;
|
||||
PLUGINLIB_EXPORT_CLASS(PassThrough,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet);
|
||||
|
||||
@@ -41,122 +41,134 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::onInit ()
|
||||
pcl_ros::ProjectInliers::onInit()
|
||||
{
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
// The type of model to use (user given parameter).
|
||||
int model_type;
|
||||
if (!pnh_->getParam ("model_type", model_type))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
|
||||
if (!pnh_->getParam("model_type", model_type)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
|
||||
getName().c_str());
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
// True if all data will be returned, false if only the projected inliers. Default: false.
|
||||
bool copy_all_data = false;
|
||||
|
||||
// True if all fields will be returned, false if only XYZ. Default: true.
|
||||
// True if all fields will be returned, false if only XYZ. Default: true.
|
||||
bool copy_all_fields = true;
|
||||
|
||||
pnh_->getParam ("copy_all_data", copy_all_data);
|
||||
pnh_->getParam ("copy_all_fields", copy_all_fields);
|
||||
pnh_->getParam("copy_all_data", copy_all_data);
|
||||
pnh_->getParam("copy_all_fields", copy_all_fields);
|
||||
|
||||
pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
|
||||
pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
|
||||
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - model_type : %d\n"
|
||||
" - copy_all_data : %s\n"
|
||||
" - copy_all_fields : %s",
|
||||
getName ().c_str (),
|
||||
model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - model_type : %d\n"
|
||||
" - copy_all_data : %s\n"
|
||||
" - copy_all_fields : %s",
|
||||
getName().c_str(),
|
||||
model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
|
||||
|
||||
// Set given parameters here
|
||||
impl_.setModelType (model_type);
|
||||
impl_.setCopyAllFields (copy_all_fields);
|
||||
impl_.setCopyAllData (copy_all_data);
|
||||
impl_.setModelType(model_type);
|
||||
impl_.setCopyAllFields(copy_all_fields);
|
||||
impl_.setCopyAllData(copy_all_data);
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::subscribe ()
|
||||
pcl_ros::ProjectInliers::subscribe()
|
||||
{
|
||||
/*
|
||||
TODO : implement use_indices_
|
||||
if (use_indices_)
|
||||
{*/
|
||||
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
|
||||
|
||||
sub_model_.subscribe (*pnh_, "model", max_queue_size_);
|
||||
sub_model_.subscribe(*pnh_, "model", max_queue_size_);
|
||||
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
|
||||
sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_);
|
||||
sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_indices_model_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
|
||||
sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_);
|
||||
sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
|
||||
if (approximate_sync_) {
|
||||
sync_input_indices_model_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2,
|
||||
PointIndices, ModelCoefficients>>>(max_queue_size_);
|
||||
sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
|
||||
sync_input_indices_model_a_->registerCallback(
|
||||
bind(
|
||||
&ProjectInliers::input_indices_model_callback,
|
||||
this, _1, _2, _3));
|
||||
} else {
|
||||
sync_input_indices_model_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2,
|
||||
PointIndices, ModelCoefficients>>>(max_queue_size_);
|
||||
sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
|
||||
sync_input_indices_model_e_->registerCallback(
|
||||
bind(
|
||||
&ProjectInliers::input_indices_model_callback,
|
||||
this, _1, _2, _3));
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::unsubscribe ()
|
||||
pcl_ros::ProjectInliers::unsubscribe()
|
||||
{
|
||||
/*
|
||||
TODO : implement use_indices_
|
||||
if (use_indices_)
|
||||
{*/
|
||||
|
||||
sub_input_filter_.unsubscribe ();
|
||||
sub_model_.unsubscribe ();
|
||||
sub_input_filter_.unsubscribe();
|
||||
sub_model_.unsubscribe();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices,
|
||||
const ModelCoefficientsConstPtr &model)
|
||||
pcl_ros::ProjectInliers::input_indices_model_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices,
|
||||
const ModelCoefficientsConstPtr & model)
|
||||
{
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
if (!isValid (model) || !isValid (indices) || !isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ());
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n"
|
||||
" - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (),
|
||||
model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ());
|
||||
if (!isValid(model) || !isValid(indices) || !isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_indices_model_callback] Invalid input!", getName().c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n"
|
||||
" - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("inliers").c_str(),
|
||||
model->values.size(), model->header.stamp.toSec(),
|
||||
model->header.frame_id.c_str(), pnh_->resolveName("model").c_str());
|
||||
|
||||
tf_input_orig_frame_ = cloud->header.frame_id;
|
||||
|
||||
IndicesPtr vindices;
|
||||
if (indices)
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
if (indices) {
|
||||
vindices.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
model_ = model;
|
||||
computePublish (cloud, vindices);
|
||||
model_ = model;
|
||||
computePublish(cloud, vindices);
|
||||
}
|
||||
|
||||
typedef pcl_ros::ProjectInliers ProjectInliers;
|
||||
PLUGINLIB_EXPORT_CLASS(ProjectInliers,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet);
|
||||
|
||||
@@ -40,38 +40,42 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::RadiusOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service)
|
||||
pcl_ros::RadiusOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
{
|
||||
// Enable the dynamic reconfigure service
|
||||
has_service = true;
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig> > (nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>::CallbackType f = boost::bind (&RadiusOutlierRemoval::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>>(nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>::CallbackType f = boost::bind(
|
||||
&RadiusOutlierRemoval::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level)
|
||||
pcl_ros::RadiusOutlierRemoval::config_callback(
|
||||
pcl_ros::RadiusOutlierRemovalConfig & config,
|
||||
uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if (impl_.getMinNeighborsInRadius () != config.min_neighbors)
|
||||
{
|
||||
impl_.setMinNeighborsInRadius (config.min_neighbors);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the number of neighbors in radius: %d.", getName ().c_str (), config.min_neighbors);
|
||||
if (impl_.getMinNeighborsInRadius() != config.min_neighbors) {
|
||||
impl_.setMinNeighborsInRadius(config.min_neighbors);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the number of neighbors in radius: %d.",
|
||||
getName().c_str(), config.min_neighbors);
|
||||
}
|
||||
|
||||
if (impl_.getRadiusSearch () != config.radius_search)
|
||||
{
|
||||
impl_.setRadiusSearch (config.radius_search);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the radius to search neighbors: %f.", getName ().c_str (), config.radius_search);
|
||||
if (impl_.getRadiusSearch() != config.radius_search) {
|
||||
impl_.setRadiusSearch(config.radius_search);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the radius to search neighbors: %f.",
|
||||
getName().c_str(), config.radius_search);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval;
|
||||
PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval, nodelet::Nodelet);
|
||||
|
||||
@@ -40,42 +40,47 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::StatisticalOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service)
|
||||
pcl_ros::StatisticalOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
{
|
||||
// Enable the dynamic reconfigure service
|
||||
has_service = true;
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > (nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>>(
|
||||
nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f =
|
||||
boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level)
|
||||
pcl_ros::StatisticalOutlierRemoval::config_callback(
|
||||
pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if (impl_.getMeanK () != config.mean_k)
|
||||
{
|
||||
impl_.setMeanK (config.mean_k);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", getName ().c_str (), config.mean_k);
|
||||
}
|
||||
|
||||
if (impl_.getStddevMulThresh () != config.stddev)
|
||||
{
|
||||
impl_.setStddevMulThresh (config.stddev);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev);
|
||||
if (impl_.getMeanK() != config.mean_k) {
|
||||
impl_.setMeanK(config.mean_k);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.",
|
||||
getName().c_str(), config.mean_k);
|
||||
}
|
||||
|
||||
if (impl_.getNegative () != config.negative)
|
||||
{
|
||||
impl_.setNegative (config.negative);
|
||||
NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true");
|
||||
if (impl_.getStddevMulThresh() != config.stddev) {
|
||||
impl_.setStddevMulThresh(config.stddev);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.",
|
||||
getName().c_str(), config.stddev);
|
||||
}
|
||||
|
||||
if (impl_.getNegative() != config.negative) {
|
||||
impl_.setNegative(config.negative);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Returning only inliers: %s.",
|
||||
getName().c_str(), config.negative ? "false" : "true");
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval;
|
||||
PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval, nodelet::Nodelet);
|
||||
|
||||
@@ -40,88 +40,92 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::VoxelGrid::child_init (ros::NodeHandle &nh, bool &has_service)
|
||||
pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
{
|
||||
// Enable the dynamic reconfigure service
|
||||
has_service = true;
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > (nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>>(nh);
|
||||
dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind(
|
||||
&VoxelGrid::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input,
|
||||
const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
pcl_ros::VoxelGrid::filter(
|
||||
const PointCloud2::ConstPtr & input,
|
||||
const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL (*(input), *(pcl_input));
|
||||
impl_.setInputCloud (pcl_input);
|
||||
impl_.setIndices (indices);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
impl_.setIndices(indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter (pcl_output);
|
||||
impl_.filter(pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level)
|
||||
pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
Eigen::Vector3f leaf_size = impl_.getLeafSize ();
|
||||
Eigen::Vector3f leaf_size = impl_.getLeafSize();
|
||||
|
||||
if (leaf_size[0] != config.leaf_size)
|
||||
{
|
||||
leaf_size.setConstant (config.leaf_size);
|
||||
NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
|
||||
impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]);
|
||||
if (leaf_size[0] != config.leaf_size) {
|
||||
leaf_size.setConstant(config.leaf_size);
|
||||
NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
|
||||
impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]);
|
||||
}
|
||||
|
||||
double filter_min, filter_max;
|
||||
impl_.getFilterLimits (filter_min, filter_max);
|
||||
if (filter_min != config.filter_limit_min)
|
||||
{
|
||||
impl_.getFilterLimits(filter_min, filter_max);
|
||||
if (filter_min != config.filter_limit_min) {
|
||||
filter_min = config.filter_limit_min;
|
||||
NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min);
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
|
||||
filter_min);
|
||||
}
|
||||
if (filter_max != config.filter_limit_max)
|
||||
{
|
||||
if (filter_max != config.filter_limit_max) {
|
||||
filter_max = config.filter_limit_max;
|
||||
NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max);
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
|
||||
filter_max);
|
||||
}
|
||||
impl_.setFilterLimits (filter_min, filter_max);
|
||||
impl_.setFilterLimits(filter_min, filter_max);
|
||||
|
||||
if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
|
||||
{
|
||||
impl_.setFilterLimitsNegative (config.filter_limit_negative);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
|
||||
if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
|
||||
impl_.setFilterLimitsNegative(config.filter_limit_negative);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the filter negative flag to: %s.",
|
||||
getName().c_str(), config.filter_limit_negative ? "true" : "false");
|
||||
}
|
||||
|
||||
if (impl_.getFilterFieldName () != config.filter_field_name)
|
||||
{
|
||||
impl_.setFilterFieldName (config.filter_field_name);
|
||||
NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ());
|
||||
if (impl_.getFilterFieldName() != config.filter_field_name) {
|
||||
impl_.setFilterFieldName(config.filter_field_name);
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the filter field name to: %s.",
|
||||
config.filter_field_name.c_str());
|
||||
}
|
||||
|
||||
// ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter
|
||||
if (tf_input_frame_ != config.input_frame)
|
||||
{
|
||||
if (tf_input_frame_ != config.input_frame) {
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
|
||||
NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str());
|
||||
}
|
||||
if (tf_output_frame_ != config.output_frame)
|
||||
{
|
||||
if (tf_output_frame_ != config.output_frame) {
|
||||
tf_output_frame_ = config.output_frame;
|
||||
NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the output TF frame to: %s.",
|
||||
tf_output_frame_.c_str());
|
||||
}
|
||||
// ]---
|
||||
}
|
||||
|
||||
typedef pcl_ros::VoxelGrid VoxelGrid;
|
||||
PLUGINLIB_EXPORT_CLASS(VoxelGrid,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet);
|
||||
|
||||
@@ -39,75 +39,74 @@
|
||||
#include "pcl_ros/io/bag_io.hpp"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name)
|
||||
bool
|
||||
pcl_ros::BAGReader::open(const std::string & file_name, const std::string & topic_name)
|
||||
{
|
||||
try
|
||||
{
|
||||
bag_.open (file_name, rosbag::bagmode::Read);
|
||||
view_.addQuery (bag_, rosbag::TopicQuery (topic_name));
|
||||
try {
|
||||
bag_.open(file_name, rosbag::bagmode::Read);
|
||||
view_.addQuery(bag_, rosbag::TopicQuery(topic_name));
|
||||
|
||||
if (view_.size () == 0)
|
||||
return (false);
|
||||
if (view_.size() == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
it_ = view_.begin ();
|
||||
it_ = view_.begin();
|
||||
} catch (rosbag::BagException & e) {
|
||||
return false;
|
||||
}
|
||||
catch (rosbag::BagException &e)
|
||||
{
|
||||
return (false);
|
||||
}
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::BAGReader::onInit ()
|
||||
pcl_ros::BAGReader::onInit()
|
||||
{
|
||||
boost::shared_ptr<ros::NodeHandle> pnh_;
|
||||
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
|
||||
pnh_.reset(new ros::NodeHandle(getMTPrivateNodeHandle()));
|
||||
// ---[ Mandatory parameters
|
||||
if (!pnh_->getParam ("file_name", file_name_))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
|
||||
if (!pnh_->getParam("file_name", file_name_)) {
|
||||
NODELET_ERROR("[onInit] Need a 'file_name' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
if (!pnh_->getParam ("topic_name", topic_name_))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!");
|
||||
if (!pnh_->getParam("topic_name", topic_name_)) {
|
||||
NODELET_ERROR("[onInit] Need a 'topic_name' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
int max_queue_size = 1;
|
||||
pnh_->getParam ("publish_rate", publish_rate_);
|
||||
pnh_->getParam ("max_queue_size", max_queue_size);
|
||||
pnh_->getParam("publish_rate", publish_rate_);
|
||||
pnh_->getParam("max_queue_size", max_queue_size);
|
||||
|
||||
ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size);
|
||||
ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2>("output", max_queue_size);
|
||||
|
||||
NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - file_name : %s\n"
|
||||
" - topic_name : %s",
|
||||
file_name_.c_str (), topic_name_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - file_name : %s\n"
|
||||
" - topic_name : %s",
|
||||
file_name_.c_str(), topic_name_.c_str());
|
||||
|
||||
if (!open (file_name_, topic_name_))
|
||||
if (!open(file_name_, topic_name_)) {
|
||||
return;
|
||||
}
|
||||
PointCloud output;
|
||||
output_ = boost::make_shared<PointCloud> (output);
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
output_ = boost::make_shared<PointCloud>(output);
|
||||
output_->header.stamp = ros::Time::now();
|
||||
|
||||
// Continous publishing enabled?
|
||||
while (pnh_->ok ())
|
||||
{
|
||||
PointCloudConstPtr cloud = getNextCloud ();
|
||||
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ());
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
while (pnh_->ok()) {
|
||||
PointCloudConstPtr cloud = getNextCloud();
|
||||
NODELET_DEBUG(
|
||||
"Publishing data (%d points) on topic %s in frame %s.",
|
||||
output_->width * output_->height, pnh_->resolveName(
|
||||
"output").c_str(), output_->header.frame_id.c_str());
|
||||
output_->header.stamp = ros::Time::now();
|
||||
|
||||
pub_output.publish (output_);
|
||||
pub_output.publish(output_);
|
||||
|
||||
ros::Duration (publish_rate_).sleep ();
|
||||
ros::spinOnce ();
|
||||
ros::Duration(publish_rate_).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::BAGReader BAGReader;
|
||||
PLUGINLIB_EXPORT_CLASS(BAGReader,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet);
|
||||
|
||||
@@ -44,226 +44,260 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit()
|
||||
{
|
||||
nodelet_topic_tools::NodeletLazy::onInit ();
|
||||
nodelet_topic_tools::NodeletLazy::onInit();
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
pnh_->getParam ("output_frame", output_frame_);
|
||||
pnh_->getParam ("approximate_sync", approximate_sync_);
|
||||
pnh_->getParam("output_frame", output_frame_);
|
||||
pnh_->getParam("approximate_sync", approximate_sync_);
|
||||
|
||||
if (output_frame_.empty ())
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
|
||||
if (output_frame_.empty()) {
|
||||
NODELET_ERROR("[onInit] Need an 'output_frame' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!pnh_->getParam ("input_topics", input_topics_))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
|
||||
if (!pnh_->getParam("input_topics", input_topics_)) {
|
||||
NODELET_ERROR("[onInit] Need a 'input_topics' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray)
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
|
||||
if (input_topics_.getType() != XmlRpc::XmlRpcValue::TypeArray) {
|
||||
NODELET_ERROR("[onInit] Invalid 'input_topics' parameter given!");
|
||||
return;
|
||||
}
|
||||
if (input_topics_.size () == 1)
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
|
||||
if (input_topics_.size() == 1) {
|
||||
NODELET_ERROR("[onInit] Only one topic given. Need at least two topics to continue.");
|
||||
return;
|
||||
}
|
||||
if (input_topics_.size () > 8)
|
||||
{
|
||||
NODELET_ERROR ("[onInit] More than 8 topics passed!");
|
||||
if (input_topics_.size() > 8) {
|
||||
NODELET_ERROR("[onInit] More than 8 topics passed!");
|
||||
return;
|
||||
}
|
||||
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("max_queue_size", maximum_queue_size_);
|
||||
pnh_->getParam("max_queue_size", maximum_queue_size_);
|
||||
|
||||
// Output
|
||||
pub_output_ = advertise<PointCloud2> (*pnh_, "output", maximum_queue_size_);
|
||||
pub_output_ = advertise<PointCloud2>(*pnh_, "output", maximum_queue_size_);
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe ()
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe()
|
||||
{
|
||||
ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:");
|
||||
for (int d = 0; d < input_topics_.size (); ++d)
|
||||
ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d]));
|
||||
ROS_INFO_STREAM("Subscribing to " << input_topics_.size() << " user given topics as inputs:");
|
||||
for (int d = 0; d < input_topics_.size(); ++d) {
|
||||
ROS_INFO_STREAM(" - " << (std::string)(input_topics_[d]));
|
||||
}
|
||||
|
||||
// Subscribe to the filters
|
||||
filters_.resize (input_topics_.size ());
|
||||
filters_.resize(input_topics_.size());
|
||||
|
||||
// 8 topics
|
||||
if (approximate_sync_)
|
||||
ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2>
|
||||
> (maximum_queue_size_));
|
||||
else
|
||||
ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2>
|
||||
> (maximum_queue_size_));
|
||||
if (approximate_sync_) {
|
||||
ts_a_.reset(
|
||||
new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2,
|
||||
PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2>
|
||||
>(maximum_queue_size_));
|
||||
} else {
|
||||
ts_e_.reset(
|
||||
new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2,
|
||||
PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2>
|
||||
>(maximum_queue_size_));
|
||||
}
|
||||
|
||||
// First input_topics_.size () filters are valid
|
||||
for (int d = 0; d < input_topics_.size (); ++d)
|
||||
{
|
||||
filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
|
||||
filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
|
||||
for (int d = 0; d < input_topics_.size(); ++d) {
|
||||
filters_[d].reset(new message_filters::Subscriber<PointCloud2>());
|
||||
filters_[d]->subscribe(*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
|
||||
}
|
||||
|
||||
// Bogus null filter
|
||||
filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
|
||||
filters_[0]->registerCallback(
|
||||
bind(
|
||||
&PointCloudConcatenateDataSynchronizer::input_callback, this,
|
||||
_1));
|
||||
|
||||
switch (input_topics_.size ())
|
||||
{
|
||||
switch (input_topics_.size()) {
|
||||
case 2:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
{
|
||||
if (approximate_sync_) {
|
||||
ts_a_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
|
||||
} else {
|
||||
ts_e_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
{
|
||||
if (approximate_sync_) {
|
||||
ts_a_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
|
||||
} else {
|
||||
ts_e_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
{
|
||||
if (approximate_sync_) {
|
||||
ts_a_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
|
||||
nf_);
|
||||
} else {
|
||||
ts_e_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
|
||||
nf_);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 5:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
{
|
||||
if (approximate_sync_) {
|
||||
ts_a_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
nf_, nf_, nf_);
|
||||
} else {
|
||||
ts_e_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
nf_, nf_, nf_);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 6:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
|
||||
break;
|
||||
}
|
||||
{
|
||||
if (approximate_sync_) {
|
||||
ts_a_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
*filters_[5], nf_, nf_);
|
||||
} else {
|
||||
ts_e_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
*filters_[5], nf_, nf_);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 7:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
|
||||
break;
|
||||
}
|
||||
{
|
||||
if (approximate_sync_) {
|
||||
ts_a_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
*filters_[5], *filters_[6], nf_);
|
||||
} else {
|
||||
ts_e_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
*filters_[5], *filters_[6], nf_);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 8:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
|
||||
break;
|
||||
}
|
||||
{
|
||||
if (approximate_sync_) {
|
||||
ts_a_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
*filters_[5], *filters_[6], *filters_[7]);
|
||||
} else {
|
||||
ts_e_->connectInput(
|
||||
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
|
||||
*filters_[5], *filters_[6], *filters_[7]);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
NODELET_FATAL ("Invalid 'input_topics' parameter given!");
|
||||
return;
|
||||
}
|
||||
{
|
||||
NODELET_FATAL("Invalid 'input_topics' parameter given!");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (approximate_sync_)
|
||||
ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
|
||||
else
|
||||
ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
|
||||
if (approximate_sync_) {
|
||||
ts_a_->registerCallback(
|
||||
boost::bind(
|
||||
&PointCloudConcatenateDataSynchronizer::input, this, _1, _2,
|
||||
_3, _4, _5, _6, _7, _8));
|
||||
} else {
|
||||
ts_e_->registerCallback(
|
||||
boost::bind(
|
||||
&PointCloudConcatenateDataSynchronizer::input, this, _1, _2,
|
||||
_3, _4, _5, _6, _7, _8));
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe ()
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe()
|
||||
{
|
||||
for (int d = 0; d < filters_.size (); ++d)
|
||||
{
|
||||
filters_[d]->unsubscribe ();
|
||||
for (int d = 0; d < filters_.size(); ++d) {
|
||||
filters_[d]->unsubscribe();
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(
|
||||
const PointCloud2 & in1,
|
||||
const PointCloud2 & in2,
|
||||
PointCloud2 & out)
|
||||
{
|
||||
//ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
|
||||
PointCloud2::Ptr in1_t (new PointCloud2 ());
|
||||
PointCloud2::Ptr in2_t (new PointCloud2 ());
|
||||
PointCloud2::Ptr in1_t(new PointCloud2());
|
||||
PointCloud2::Ptr in2_t(new PointCloud2());
|
||||
|
||||
// Transform the point clouds into the specified output frame
|
||||
if (output_frame_ != in1.header.frame_id)
|
||||
pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_);
|
||||
else
|
||||
in1_t = boost::make_shared<PointCloud2> (in1);
|
||||
if (output_frame_ != in1.header.frame_id) {
|
||||
pcl_ros::transformPointCloud(output_frame_, in1, *in1_t, tf_);
|
||||
} else {
|
||||
in1_t = boost::make_shared<PointCloud2>(in1);
|
||||
}
|
||||
|
||||
if (output_frame_ != in2.header.frame_id)
|
||||
pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_);
|
||||
else
|
||||
in2_t = boost::make_shared<PointCloud2> (in2);
|
||||
if (output_frame_ != in2.header.frame_id) {
|
||||
pcl_ros::transformPointCloud(output_frame_, in2, *in2_t, tf_);
|
||||
} else {
|
||||
in2_t = boost::make_shared<PointCloud2>(in2);
|
||||
}
|
||||
|
||||
// Concatenate the results
|
||||
pcl::concatenatePointCloud (*in1_t, *in2_t, out);
|
||||
pcl::concatenatePointCloud(*in1_t, *in2_t, out);
|
||||
// Copy header
|
||||
out.header.stamp = in1.header.stamp;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::input (
|
||||
const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
|
||||
const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
|
||||
const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
|
||||
const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::input(
|
||||
const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2,
|
||||
const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4,
|
||||
const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6,
|
||||
const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8)
|
||||
{
|
||||
PointCloud2::Ptr out1 (new PointCloud2 ());
|
||||
PointCloud2::Ptr out2 (new PointCloud2 ());
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1);
|
||||
if (in3 && in3->width * in3->height > 0)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2);
|
||||
PointCloud2::Ptr out1(new PointCloud2());
|
||||
PointCloud2::Ptr out2(new PointCloud2());
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*in1, *in2, *out1);
|
||||
if (in3 && in3->width * in3->height > 0) {
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in3, *out2);
|
||||
out1 = out2;
|
||||
if (in4 && in4->width * in4->height > 0)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1);
|
||||
if (in5 && in5->width * in5->height > 0)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2);
|
||||
if (in4 && in4->width * in4->height > 0) {
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in4, *out1);
|
||||
if (in5 && in5->width * in5->height > 0) {
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in5, *out2);
|
||||
out1 = out2;
|
||||
if (in6 && in6->width * in6->height > 0)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1);
|
||||
if (in7 && in7->width * in7->height > 0)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2);
|
||||
if (in6 && in6->width * in6->height > 0) {
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in6, *out1);
|
||||
if (in7 && in7->width * in7->height > 0) {
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in7, *out2);
|
||||
out1 = out2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
pub_output_.publish (boost::make_shared<PointCloud2> (*out1));
|
||||
pub_output_.publish(boost::make_shared<PointCloud2>(*out1));
|
||||
}
|
||||
|
||||
typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer;
|
||||
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer, nodelet::Nodelet);
|
||||
|
||||
@@ -45,126 +45,129 @@
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
|
||||
{
|
||||
nodelet_topic_tools::NodeletLazy::onInit ();
|
||||
nodelet_topic_tools::NodeletLazy::onInit();
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
if (!pnh_->getParam ("input_messages", input_messages_))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
|
||||
if (!pnh_->getParam("input_messages", input_messages_)) {
|
||||
NODELET_ERROR("[onInit] Need a 'input_messages' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
if (input_messages_ < 2)
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!");
|
||||
if (input_messages_ < 2) {
|
||||
NODELET_ERROR("[onInit] Invalid 'input_messages' parameter given!");
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("max_queue_size", maximum_queue_size_);
|
||||
pnh_->getParam ("maximum_seconds", maximum_seconds_);
|
||||
pub_output_ = advertise<sensor_msgs::PointCloud2> (*pnh_, "output", maximum_queue_size_);
|
||||
pnh_->getParam("max_queue_size", maximum_queue_size_);
|
||||
pnh_->getParam("maximum_seconds", maximum_seconds_);
|
||||
pub_output_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", maximum_queue_size_);
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe ()
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
|
||||
{
|
||||
sub_input_ = pnh_->subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this);
|
||||
sub_input_ = pnh_->subscribe(
|
||||
"input", maximum_queue_size_,
|
||||
&PointCloudConcatenateFieldsSynchronizer::input_callback, this);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe ()
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe()
|
||||
{
|
||||
sub_input_.shutdown ();
|
||||
sub_input_.shutdown();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud)
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud)
|
||||
{
|
||||
NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(),
|
||||
pnh_->resolveName("input").c_str());
|
||||
|
||||
// Erase old data in the queue
|
||||
if (maximum_seconds_ > 0 && queue_.size () > 0)
|
||||
{
|
||||
while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
|
||||
if (maximum_seconds_ > 0 && queue_.size() > 0) {
|
||||
while (fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ) > maximum_seconds_ &&
|
||||
queue_.size() > 0)
|
||||
{
|
||||
NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
|
||||
(*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ));
|
||||
queue_.erase (queue_.begin ());
|
||||
NODELET_WARN(
|
||||
"[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
|
||||
(*queue_.begin()).first.toSec(),
|
||||
fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ));
|
||||
queue_.erase(queue_.begin());
|
||||
}
|
||||
}
|
||||
|
||||
// Push back new data
|
||||
queue_[cloud->header.stamp].push_back (cloud);
|
||||
if ((int)queue_[cloud->header.stamp].size () >= input_messages_)
|
||||
{
|
||||
queue_[cloud->header.stamp].push_back(cloud);
|
||||
if ((int)queue_[cloud->header.stamp].size() >= input_messages_) {
|
||||
// Concatenate together and publish
|
||||
std::vector<PointCloudConstPtr> &clouds = queue_[cloud->header.stamp];
|
||||
std::vector<PointCloudConstPtr> & clouds = queue_[cloud->header.stamp];
|
||||
PointCloud cloud_out = *clouds[0];
|
||||
|
||||
// Resize the output dataset
|
||||
int data_size = cloud_out.data.size ();
|
||||
int nr_fields = cloud_out.fields.size ();
|
||||
int data_size = cloud_out.data.size();
|
||||
int nr_fields = cloud_out.fields.size();
|
||||
int nr_points = cloud_out.width * cloud_out.height;
|
||||
for (size_t i = 1; i < clouds.size (); ++i)
|
||||
{
|
||||
assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
|
||||
for (size_t i = 1; i < clouds.size(); ++i) {
|
||||
assert(
|
||||
clouds[i]->data.size() / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
|
||||
|
||||
if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height)
|
||||
{
|
||||
NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
|
||||
i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
|
||||
if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) {
|
||||
NODELET_ERROR(
|
||||
"[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
|
||||
i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
|
||||
break;
|
||||
}
|
||||
// Point step must increase with the length of each new field
|
||||
cloud_out.point_step += clouds[i]->point_step;
|
||||
// Resize data to hold all clouds
|
||||
data_size += clouds[i]->data.size ();
|
||||
data_size += clouds[i]->data.size();
|
||||
|
||||
// Concatenate fields
|
||||
cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ());
|
||||
int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype);
|
||||
for (size_t d = 0; d < clouds[i]->fields.size (); ++d)
|
||||
{
|
||||
cloud_out.fields.resize(nr_fields + clouds[i]->fields.size());
|
||||
int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize(
|
||||
cloud_out.fields[nr_fields - 1].datatype);
|
||||
for (size_t d = 0; d < clouds[i]->fields.size(); ++d) {
|
||||
cloud_out.fields[nr_fields + d] = clouds[i]->fields[d];
|
||||
cloud_out.fields[nr_fields + d].offset += delta_offset;
|
||||
}
|
||||
nr_fields += clouds[i]->fields.size ();
|
||||
nr_fields += clouds[i]->fields.size();
|
||||
}
|
||||
// Recalculate row_step
|
||||
cloud_out.row_step = cloud_out.point_step * cloud_out.width;
|
||||
cloud_out.data.resize (data_size);
|
||||
cloud_out.data.resize(data_size);
|
||||
|
||||
// Iterate over each point and perform the appropriate memcpys
|
||||
int point_offset = 0;
|
||||
for (int cp = 0; cp < nr_points; ++cp)
|
||||
{
|
||||
for (size_t i = 0; i < clouds.size (); ++i)
|
||||
{
|
||||
for (int cp = 0; cp < nr_points; ++cp) {
|
||||
for (size_t i = 0; i < clouds.size(); ++i) {
|
||||
// Copy each individual point
|
||||
memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step);
|
||||
memcpy(
|
||||
&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step],
|
||||
clouds[i]->point_step);
|
||||
point_offset += clouds[i]->point_step;
|
||||
}
|
||||
}
|
||||
pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out));
|
||||
queue_.erase (cloud->header.stamp);
|
||||
pub_output_.publish(boost::make_shared<const PointCloud>(cloud_out));
|
||||
queue_.erase(cloud->header.stamp);
|
||||
}
|
||||
|
||||
// Clean the queue to avoid overflowing
|
||||
if (maximum_queue_size_ > 0)
|
||||
{
|
||||
while ((int)queue_.size () > maximum_queue_size_)
|
||||
queue_.erase (queue_.begin ());
|
||||
if (maximum_queue_size_ > 0) {
|
||||
while ((int)queue_.size() > maximum_queue_size_) {
|
||||
queue_.erase(queue_.begin());
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;
|
||||
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet);
|
||||
|
||||
@@ -42,7 +42,8 @@
|
||||
#include <nodelet_topic_tools/nodelet_mux.h>
|
||||
#include <nodelet_topic_tools/nodelet_demux.h>
|
||||
|
||||
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2, message_filters::Subscriber<sensor_msgs::PointCloud2> > NodeletMUX;
|
||||
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2,
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2>> NodeletMUX;
|
||||
//typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2, pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX;
|
||||
typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
|
||||
|
||||
@@ -51,7 +52,6 @@ typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
|
||||
//#include "concatenate_fields.cpp"
|
||||
//#include "concatenate_data.cpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(NodeletMUX,nodelet::Nodelet);
|
||||
PLUGINLIB_EXPORT_CLASS(NodeletDEMUX,nodelet::Nodelet);
|
||||
PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet);
|
||||
PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet);
|
||||
//PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);
|
||||
|
||||
|
||||
@@ -40,143 +40,150 @@
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PCDReader::onInit ()
|
||||
pcl_ros::PCDReader::onInit()
|
||||
{
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
// Provide a latched topic
|
||||
ros::Publisher pub_output = pnh_->advertise<PointCloud2> ("output", max_queue_size_, true);
|
||||
ros::Publisher pub_output = pnh_->advertise<PointCloud2>("output", max_queue_size_, true);
|
||||
|
||||
pnh_->getParam ("publish_rate", publish_rate_);
|
||||
pnh_->getParam ("tf_frame", tf_frame_);
|
||||
pnh_->getParam("publish_rate", publish_rate_);
|
||||
pnh_->getParam("tf_frame", tf_frame_);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - publish_rate : %f\n"
|
||||
" - tf_frame : %s",
|
||||
getName ().c_str (),
|
||||
publish_rate_, tf_frame_.c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - publish_rate : %f\n"
|
||||
" - tf_frame : %s",
|
||||
getName().c_str(),
|
||||
publish_rate_, tf_frame_.c_str());
|
||||
|
||||
PointCloud2Ptr output_new;
|
||||
output_ = boost::make_shared<PointCloud2> ();
|
||||
output_new = boost::make_shared<PointCloud2> ();
|
||||
output_ = boost::make_shared<PointCloud2>();
|
||||
output_new = boost::make_shared<PointCloud2>();
|
||||
|
||||
// Wait in a loop until someone connects
|
||||
do
|
||||
{
|
||||
ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
|
||||
ros::spinOnce ();
|
||||
ros::Duration (0.01).sleep ();
|
||||
}
|
||||
while (pnh_->ok () && pub_output.getNumSubscribers () == 0);
|
||||
do {
|
||||
ROS_DEBUG_ONCE("[%s::onInit] Waiting for a client to connect...", getName().c_str());
|
||||
ros::spinOnce();
|
||||
ros::Duration(0.01).sleep();
|
||||
} while (pnh_->ok() && pub_output.getNumSubscribers() == 0);
|
||||
|
||||
std::string file_name;
|
||||
|
||||
while (pnh_->ok ())
|
||||
{
|
||||
while (pnh_->ok()) {
|
||||
// Get the current filename parameter. If no filename set, loop
|
||||
if (!pnh_->getParam ("filename", file_name_) && file_name_.empty ())
|
||||
{
|
||||
ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
|
||||
ros::Duration (0.01).sleep ();
|
||||
ros::spinOnce ();
|
||||
if (!pnh_->getParam("filename", file_name_) && file_name_.empty()) {
|
||||
ROS_ERROR_ONCE(
|
||||
"[%s::onInit] Need a 'filename' parameter to be set before continuing!",
|
||||
getName().c_str());
|
||||
ros::Duration(0.01).sleep();
|
||||
ros::spinOnce();
|
||||
continue;
|
||||
}
|
||||
|
||||
// If the filename parameter holds a different value than the last one we read
|
||||
if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
|
||||
{
|
||||
NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
|
||||
if (file_name_.compare(file_name) != 0 && !file_name_.empty()) {
|
||||
NODELET_INFO("[%s::onInit] New file given: %s", getName().c_str(), file_name_.c_str());
|
||||
file_name = file_name_;
|
||||
pcl::PCLPointCloud2 cloud;
|
||||
if (impl_.read (file_name_, cloud) < 0)
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
|
||||
if (impl_.read(file_name_, cloud) < 0) {
|
||||
NODELET_ERROR("[%s::onInit] Error reading %s !", getName().c_str(), file_name_.c_str());
|
||||
return;
|
||||
}
|
||||
pcl_conversions::moveFromPCL(cloud, *(output_));
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
output_->header.stamp = ros::Time::now();
|
||||
output_->header.frame_id = tf_frame_;
|
||||
}
|
||||
|
||||
// We do not publish empty data
|
||||
if (output_->data.size () == 0)
|
||||
if (output_->data.size() == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (publish_rate_ == 0)
|
||||
{
|
||||
if (output_ != output_new)
|
||||
{
|
||||
NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
|
||||
pub_output.publish (output_);
|
||||
if (publish_rate_ == 0) {
|
||||
if (output_ != output_new) {
|
||||
NODELET_DEBUG(
|
||||
"Publishing data once (%d points) on topic %s in frame %s.",
|
||||
output_->width * output_->height,
|
||||
getMTPrivateNodeHandle().resolveName("output").c_str(), output_->header.frame_id.c_str());
|
||||
pub_output.publish(output_);
|
||||
output_new = output_;
|
||||
}
|
||||
ros::Duration (0.01).sleep ();
|
||||
}
|
||||
else
|
||||
{
|
||||
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
pub_output.publish (output_);
|
||||
ros::Duration(0.01).sleep();
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"Publishing data (%d points) on topic %s in frame %s.",
|
||||
output_->width * output_->height, getMTPrivateNodeHandle().resolveName(
|
||||
"output").c_str(), output_->header.frame_id.c_str());
|
||||
output_->header.stamp = ros::Time::now();
|
||||
pub_output.publish(output_);
|
||||
|
||||
ros::Duration (publish_rate_).sleep ();
|
||||
ros::Duration(publish_rate_).sleep();
|
||||
}
|
||||
|
||||
ros::spinOnce ();
|
||||
ros::spinOnce();
|
||||
// Update parameters from server
|
||||
pnh_->getParam ("publish_rate", publish_rate_);
|
||||
pnh_->getParam ("tf_frame", tf_frame_);
|
||||
pnh_->getParam("publish_rate", publish_rate_);
|
||||
pnh_->getParam("tf_frame", tf_frame_);
|
||||
}
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PCDWriter::onInit ()
|
||||
pcl_ros::PCDWriter::onInit()
|
||||
{
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
sub_input_ = pnh_->subscribe ("input", 1, &PCDWriter::input_callback, this);
|
||||
sub_input_ = pnh_->subscribe("input", 1, &PCDWriter::input_callback, this);
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("filename", file_name_);
|
||||
pnh_->getParam ("binary_mode", binary_mode_);
|
||||
pnh_->getParam("filename", file_name_);
|
||||
pnh_->getParam("binary_mode", binary_mode_);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - filename : %s\n"
|
||||
" - binary_mode : %s",
|
||||
getName ().c_str (),
|
||||
file_name_.c_str (), (binary_mode_) ? "true" : "false");
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - filename : %s\n"
|
||||
" - binary_mode : %s",
|
||||
getName().c_str(),
|
||||
file_name_.c_str(), (binary_mode_) ? "true" : "false");
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
|
||||
pcl_ros::PCDWriter::input_callback(const PointCloud2ConstPtr & cloud)
|
||||
{
|
||||
if (!isValid (cloud))
|
||||
if (!isValid(cloud)) {
|
||||
return;
|
||||
|
||||
pnh_->getParam ("filename", file_name_);
|
||||
}
|
||||
|
||||
pnh_->getParam("filename", file_name_);
|
||||
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.",
|
||||
getName().c_str(), cloud->width * cloud->height,
|
||||
cloud->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("input").c_str());
|
||||
|
||||
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
|
||||
|
||||
std::string fname;
|
||||
if (file_name_.empty ())
|
||||
fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
|
||||
else
|
||||
if (file_name_.empty()) {
|
||||
fname = boost::lexical_cast<std::string>(cloud->header.stamp.toSec()) + ".pcd";
|
||||
} else {
|
||||
fname = file_name_;
|
||||
}
|
||||
pcl::PCLPointCloud2 pcl_cloud;
|
||||
// It is safe to remove the const here because we are the only subscriber callback.
|
||||
pcl_conversions::moveToPCL(*(const_cast<PointCloud2*>(cloud.get())), pcl_cloud);
|
||||
impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
|
||||
pcl_conversions::moveToPCL(*(const_cast<PointCloud2 *>(cloud.get())), pcl_cloud);
|
||||
impl_.write(
|
||||
fname, pcl_cloud, Eigen::Vector4f::Zero(),
|
||||
Eigen::Quaternionf::Identity(), binary_mode_);
|
||||
|
||||
NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
|
||||
NODELET_DEBUG("[%s::input_callback] Data saved to %s", getName().c_str(), fname.c_str());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PCDReader PCDReader;
|
||||
typedef pcl_ros::PCDWriter PCDWriter;
|
||||
PLUGINLIB_EXPORT_CLASS(PCDReader,nodelet::Nodelet);
|
||||
PLUGINLIB_EXPORT_CLASS(PCDWriter,nodelet::Nodelet);
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(PCDReader, nodelet::Nodelet);
|
||||
PLUGINLIB_EXPORT_CLASS(PCDWriter, nodelet::Nodelet);
|
||||
|
||||
@@ -48,139 +48,155 @@ using pcl_conversions::toPCL;
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::onInit ()
|
||||
pcl_ros::EuclideanClusterExtraction::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// ---[ 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 ());
|
||||
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 ());
|
||||
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;
|
||||
}
|
||||
|
||||
//private_nh.getParam ("use_indices", use_indices_);
|
||||
pnh_->getParam ("publish_indices", publish_indices_);
|
||||
pnh_->getParam("publish_indices", publish_indices_);
|
||||
|
||||
if (publish_indices_)
|
||||
pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
|
||||
else
|
||||
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
|
||||
if (publish_indices_) {
|
||||
pub_output_ = advertise<PointIndices>(*pnh_, "output", max_queue_size_);
|
||||
} else {
|
||||
pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
|
||||
}
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>>(*pnh_);
|
||||
dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind(
|
||||
&EuclideanClusterExtraction::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
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",
|
||||
getName ().c_str (),
|
||||
max_queue_size_,
|
||||
(use_indices_) ? "true" : "false", cluster_tolerance);
|
||||
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",
|
||||
getName().c_str(),
|
||||
max_queue_size_,
|
||||
(use_indices_) ? "true" : "false", cluster_tolerance);
|
||||
|
||||
// Set given parameters here
|
||||
impl_.setClusterTolerance (cluster_tolerance);
|
||||
impl_.setClusterTolerance(cluster_tolerance);
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::subscribe ()
|
||||
pcl_ros::EuclideanClusterExtraction::subscribe()
|
||||
{
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_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_);
|
||||
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 (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
|
||||
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(
|
||||
&EuclideanClusterExtraction::
|
||||
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(
|
||||
&EuclideanClusterExtraction::
|
||||
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 (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
}
|
||||
else
|
||||
} else {
|
||||
// Subscribe in an old fashion to input only (no filters)
|
||||
sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
|
||||
sub_input_ =
|
||||
pnh_->subscribe<PointCloud>(
|
||||
"input", max_queue_size_,
|
||||
bind(&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr()));
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::unsubscribe ()
|
||||
pcl_ros::EuclideanClusterExtraction::unsubscribe()
|
||||
{
|
||||
if (use_indices_)
|
||||
{
|
||||
sub_input_filter_.unsubscribe ();
|
||||
sub_indices_filter_.unsubscribe ();
|
||||
if (use_indices_) {
|
||||
sub_input_filter_.unsubscribe();
|
||||
sub_indices_filter_.unsubscribe();
|
||||
} else {
|
||||
sub_input_.shutdown();
|
||||
}
|
||||
else
|
||||
sub_input_.shutdown ();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level)
|
||||
pcl_ros::EuclideanClusterExtraction::config_callback(
|
||||
EuclideanClusterExtractionConfig & config,
|
||||
uint32_t level)
|
||||
{
|
||||
if (impl_.getClusterTolerance () != config.cluster_tolerance)
|
||||
{
|
||||
impl_.setClusterTolerance (config.cluster_tolerance);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
|
||||
if (impl_.getClusterTolerance() != config.cluster_tolerance) {
|
||||
impl_.setClusterTolerance(config.cluster_tolerance);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting new clustering tolerance to: %f.",
|
||||
getName().c_str(), config.cluster_tolerance);
|
||||
}
|
||||
if (impl_.getMinClusterSize () != config.cluster_min_size)
|
||||
{
|
||||
impl_.setMinClusterSize (config.cluster_min_size);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
|
||||
if (impl_.getMinClusterSize() != config.cluster_min_size) {
|
||||
impl_.setMinClusterSize(config.cluster_min_size);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the minimum cluster size to: %d.",
|
||||
getName().c_str(), config.cluster_min_size);
|
||||
}
|
||||
if (impl_.getMaxClusterSize () != config.cluster_max_size)
|
||||
{
|
||||
impl_.setMaxClusterSize (config.cluster_max_size);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
|
||||
if (impl_.getMaxClusterSize() != config.cluster_max_size) {
|
||||
impl_.setMaxClusterSize(config.cluster_max_size);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the maximum cluster size to: %d.",
|
||||
getName().c_str(), config.cluster_max_size);
|
||||
}
|
||||
if (max_clusters_ != config.max_clusters)
|
||||
{
|
||||
if (max_clusters_ != config.max_clusters) {
|
||||
max_clusters_ = config.max_clusters;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the maximum number of clusters to extract to: %d.",
|
||||
getName().c_str(), config.max_clusters);
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::input_indices_callback (
|
||||
const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
|
||||
pcl_ros::EuclideanClusterExtraction::input_indices_callback(
|
||||
const PointCloudConstPtr & cloud, const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
if (!isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -188,65 +204,72 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
|
||||
if (indices) {
|
||||
std_msgs::Header cloud_header = fromPCL(cloud->header);
|
||||
std_msgs::Header indices_header = indices->header;
|
||||
NODELET_DEBUG ("[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud_header.stamp.toSec(), cloud_header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
|
||||
indices->indices.size(), indices_header.stamp.toSec(),
|
||||
indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(), cloud->width * cloud->height, fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices)
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
if (indices) {
|
||||
indices_ptr.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices_ptr);
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices_ptr);
|
||||
|
||||
std::vector<pcl::PointIndices> clusters;
|
||||
impl_.extract (clusters);
|
||||
impl_.extract(clusters);
|
||||
|
||||
if (publish_indices_)
|
||||
{
|
||||
for (size_t i = 0; i < clusters.size (); ++i)
|
||||
{
|
||||
if ((int)i >= max_clusters_)
|
||||
if (publish_indices_) {
|
||||
for (size_t i = 0; i < clusters.size(); ++i) {
|
||||
if ((int)i >= max_clusters_) {
|
||||
break;
|
||||
}
|
||||
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
|
||||
pcl_msgs::PointIndices ros_pi;
|
||||
moveFromPCL(clusters[i], ros_pi);
|
||||
ros_pi.header.stamp += ros::Duration (i * 0.001);
|
||||
pub_output_.publish (ros_pi);
|
||||
ros_pi.header.stamp += ros::Duration(i * 0.001);
|
||||
pub_output_.publish(ros_pi);
|
||||
}
|
||||
|
||||
NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
|
||||
}
|
||||
else
|
||||
{
|
||||
for (size_t i = 0; i < clusters.size (); ++i)
|
||||
{
|
||||
if ((int)i >= max_clusters_)
|
||||
NODELET_DEBUG(
|
||||
"[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s",
|
||||
clusters.size(), pnh_->resolveName("output").c_str());
|
||||
} else {
|
||||
for (size_t i = 0; i < clusters.size(); ++i) {
|
||||
if ((int)i >= max_clusters_) {
|
||||
break;
|
||||
}
|
||||
PointCloud output;
|
||||
copyPointCloud (*cloud, clusters[i].indices, output);
|
||||
copyPointCloud(*cloud, clusters[i].indices, output);
|
||||
|
||||
//PointCloud output_blob; // Convert from the templated output to the PointCloud blob
|
||||
//pcl::toROSMsg (output, output_blob);
|
||||
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
|
||||
std_msgs::Header header = fromPCL(output.header);
|
||||
header.stamp += ros::Duration (i * 0.001);
|
||||
header.stamp += ros::Duration(i * 0.001);
|
||||
toPCL(header, output.header);
|
||||
// Publish a Boost shared ptr const data
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
|
||||
i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
NODELET_DEBUG(
|
||||
"[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
|
||||
i, clusters[i].indices.size(), header.stamp.toSec(), pnh_->resolveName("output").c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction;
|
||||
PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -47,174 +47,206 @@ using pcl_conversions::moveToPCL;
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::onInit ()
|
||||
pcl_ros::ExtractPolygonalPrismData::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>>(*pnh_);
|
||||
dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind(
|
||||
&ExtractPolygonalPrismData::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
// Advertise the output topics
|
||||
pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
|
||||
pub_output_ = advertise<PointIndices>(*pnh_, "output", max_queue_size_);
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::subscribe ()
|
||||
pcl_ros::ExtractPolygonalPrismData::subscribe()
|
||||
{
|
||||
sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_hull_filter_.subscribe(*pnh_, "planar_hull", max_queue_size_);
|
||||
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
|
||||
|
||||
// Create the objects here
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
|
||||
else
|
||||
sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
|
||||
|
||||
if (use_indices_)
|
||||
{
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_hull_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointCloud, PointIndices>>>(max_queue_size_);
|
||||
} else {
|
||||
sync_input_hull_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
|
||||
PointCloud, PointIndices>>>(max_queue_size_);
|
||||
}
|
||||
else
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1));
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
|
||||
else
|
||||
sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
|
||||
if (use_indices_) {
|
||||
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
|
||||
if (approximate_sync_) {
|
||||
sync_input_hull_indices_a_->connectInput(
|
||||
sub_input_filter_, sub_hull_filter_,
|
||||
sub_indices_filter_);
|
||||
} else {
|
||||
sync_input_hull_indices_e_->connectInput(
|
||||
sub_input_filter_, sub_hull_filter_,
|
||||
sub_indices_filter_);
|
||||
}
|
||||
} else {
|
||||
sub_input_filter_.registerCallback(bind(&ExtractPolygonalPrismData::input_callback, this, _1));
|
||||
|
||||
if (approximate_sync_) {
|
||||
sync_input_hull_indices_a_->connectInput(sub_input_filter_, sub_hull_filter_, nf_);
|
||||
} else {
|
||||
sync_input_hull_indices_e_->connectInput(sub_input_filter_, sub_hull_filter_, nf_);
|
||||
}
|
||||
}
|
||||
// Register callbacks
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
|
||||
else
|
||||
sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
|
||||
if (approximate_sync_) {
|
||||
sync_input_hull_indices_a_->registerCallback(
|
||||
bind(
|
||||
&ExtractPolygonalPrismData::
|
||||
input_hull_indices_callback, this, _1, _2, _3));
|
||||
} else {
|
||||
sync_input_hull_indices_e_->registerCallback(
|
||||
bind(
|
||||
&ExtractPolygonalPrismData::
|
||||
input_hull_indices_callback, this, _1, _2, _3));
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::unsubscribe ()
|
||||
pcl_ros::ExtractPolygonalPrismData::unsubscribe()
|
||||
{
|
||||
sub_hull_filter_.unsubscribe ();
|
||||
sub_input_filter_.unsubscribe ();
|
||||
sub_hull_filter_.unsubscribe();
|
||||
sub_input_filter_.unsubscribe();
|
||||
|
||||
if (use_indices_)
|
||||
sub_indices_filter_.unsubscribe ();
|
||||
if (use_indices_) {
|
||||
sub_indices_filter_.unsubscribe();
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level)
|
||||
pcl_ros::ExtractPolygonalPrismData::config_callback(
|
||||
ExtractPolygonalPrismDataConfig & config,
|
||||
uint32_t level)
|
||||
{
|
||||
double height_min, height_max;
|
||||
impl_.getHeightLimits (height_min, height_max);
|
||||
if (height_min != config.height_min)
|
||||
{
|
||||
impl_.getHeightLimits(height_min, height_max);
|
||||
if (height_min != config.height_min) {
|
||||
height_min = config.height_min;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min);
|
||||
impl_.setHeightLimits (height_min, height_max);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting new minimum height to the planar model to: %f.",
|
||||
getName().c_str(), height_min);
|
||||
impl_.setHeightLimits(height_min, height_max);
|
||||
}
|
||||
if (height_max != config.height_max)
|
||||
{
|
||||
if (height_max != config.height_max) {
|
||||
height_max = config.height_max;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
|
||||
impl_.setHeightLimits (height_min, height_max);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting new maximum height to the planar model to: %f.",
|
||||
getName().c_str(), height_max);
|
||||
impl_.setHeightLimits(height_min, height_max);
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
|
||||
const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &hull,
|
||||
const PointIndicesConstPtr &indices)
|
||||
pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointCloudConstPtr & hull,
|
||||
const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Copy the header (stamp + frame_id)
|
||||
pcl_msgs::PointIndices inliers;
|
||||
inliers.header = fromPCL(cloud->header);
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud) || !isValid (hull, "planar_hull"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
|
||||
pub_output_.publish (inliers);
|
||||
if (!isValid(cloud) || !isValid(hull, "planar_hull")) {
|
||||
NODELET_ERROR("[%s::input_hull_indices_callback] Invalid input!", getName().c_str());
|
||||
pub_output_.publish(inliers);
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
pub_output_.publish (inliers);
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR("[%s::input_hull_indices_callback] Invalid indices!", getName().c_str());
|
||||
pub_output_.publish(inliers);
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_hull_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL(
|
||||
hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"planar_hull").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_hull_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL(
|
||||
hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"planar_hull").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
if (cloud->header.frame_id != hull->header.frame_id)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
|
||||
if (cloud->header.frame_id != hull->header.frame_id) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.",
|
||||
getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str());
|
||||
PointCloud planar_hull;
|
||||
if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
|
||||
{
|
||||
if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) {
|
||||
// Publish empty before return
|
||||
pub_output_.publish (inliers);
|
||||
pub_output_.publish(inliers);
|
||||
return;
|
||||
}
|
||||
impl_.setInputPlanarHull (pcl_ptr(planar_hull.makeShared ()));
|
||||
impl_.setInputPlanarHull(pcl_ptr(planar_hull.makeShared()));
|
||||
} else {
|
||||
impl_.setInputPlanarHull(pcl_ptr(hull));
|
||||
}
|
||||
else
|
||||
impl_.setInputPlanarHull (pcl_ptr(hull));
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
if (indices && !indices->header.frame_id.empty()) {
|
||||
indices_ptr.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices_ptr);
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices_ptr);
|
||||
|
||||
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
|
||||
if (!cloud->points.empty ()) {
|
||||
if (!cloud->points.empty()) {
|
||||
pcl::PointIndices pcl_inliers;
|
||||
moveToPCL(inliers, pcl_inliers);
|
||||
impl_.segment (pcl_inliers);
|
||||
impl_.segment(pcl_inliers);
|
||||
moveFromPCL(pcl_inliers, inliers);
|
||||
}
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
inliers.header = fromPCL(cloud->header);
|
||||
pub_output_.publish (inliers);
|
||||
NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
|
||||
pub_output_.publish(inliers);
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_hull_callback] Publishing %zu indices.",
|
||||
getName().c_str(), inliers.indices.size());
|
||||
}
|
||||
|
||||
typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
|
||||
PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet)
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -41,103 +41,119 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::onInit ()
|
||||
pcl_ros::SegmentDifferences::onInit()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
|
||||
pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - max_queue_size : %d",
|
||||
getName ().c_str (),
|
||||
max_queue_size_);
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - max_queue_size : %d",
|
||||
getName().c_str(),
|
||||
max_queue_size_);
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::subscribe ()
|
||||
pcl_ros::SegmentDifferences::subscribe()
|
||||
{
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_);
|
||||
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
|
||||
sub_target_filter_.subscribe(*pnh_, "target", max_queue_size_);
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<SegmentDifferencesConfig>>(*pnh_);
|
||||
dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind(
|
||||
&SegmentDifferences::config_callback, this, _1, _2);
|
||||
srv_->setCallback(f);
|
||||
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
|
||||
sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_);
|
||||
sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > (max_queue_size_);
|
||||
sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_);
|
||||
sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
|
||||
if (approximate_sync_) {
|
||||
sync_input_target_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointCloud>>>(max_queue_size_);
|
||||
sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_);
|
||||
sync_input_target_a_->registerCallback(
|
||||
bind(
|
||||
&SegmentDifferences::input_target_callback, this,
|
||||
_1, _2));
|
||||
} else {
|
||||
sync_input_target_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
|
||||
PointCloud>>>(max_queue_size_);
|
||||
sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_);
|
||||
sync_input_target_e_->registerCallback(
|
||||
bind(
|
||||
&SegmentDifferences::input_target_callback, this,
|
||||
_1, _2));
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::unsubscribe ()
|
||||
pcl_ros::SegmentDifferences::unsubscribe()
|
||||
{
|
||||
sub_input_filter_.unsubscribe ();
|
||||
sub_target_filter_.unsubscribe ();
|
||||
sub_input_filter_.unsubscribe();
|
||||
sub_target_filter_.unsubscribe();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level)
|
||||
pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level)
|
||||
{
|
||||
if (impl_.getDistanceThreshold () != config.distance_threshold)
|
||||
{
|
||||
impl_.setDistanceThreshold (config.distance_threshold);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
|
||||
if (impl_.getDistanceThreshold() != config.distance_threshold) {
|
||||
impl_.setDistanceThreshold(config.distance_threshold);
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting new distance threshold to: %f.",
|
||||
getName().c_str(), config.distance_threshold);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &cloud_target)
|
||||
pcl_ros::SegmentDifferences::input_target_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointCloudConstPtr & cloud_target)
|
||||
{
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
if (!isValid (cloud) || !isValid (cloud_target, "target"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
PointCloud output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
if (pub_output_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
NODELET_DEBUG ("[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
|
||||
if (!isValid(cloud) || !isValid(cloud_target, "target")) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
|
||||
PointCloud output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
return;
|
||||
}
|
||||
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setTargetCloud (pcl_ptr(cloud_target));
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
cloud_target->width * cloud_target->height, pcl::getFieldsList(*cloud_target).c_str(),
|
||||
fromPCL(cloud_target->header).stamp.toSec(),
|
||||
cloud_target->header.frame_id.c_str(), pnh_->resolveName("target").c_str());
|
||||
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setTargetCloud(pcl_ptr(cloud_target));
|
||||
|
||||
PointCloud output;
|
||||
impl_.segment (output);
|
||||
impl_.segment(output);
|
||||
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
|
||||
output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ());
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
NODELET_DEBUG(
|
||||
"[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s",
|
||||
getName().c_str(),
|
||||
output.points.size(), fromPCL(output.header).stamp.toSec(),
|
||||
pnh_->resolveName("output").c_str());
|
||||
}
|
||||
|
||||
typedef pcl_ros::SegmentDifferences SegmentDifferences;
|
||||
PLUGINLIB_EXPORT_CLASS(SegmentDifferences, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -40,5 +40,3 @@
|
||||
//#include "extract_polygonal_prism_data.cpp"
|
||||
//#include "sac_segmentation.cpp"
|
||||
//#include "segment_differences.cpp"
|
||||
|
||||
|
||||
|
||||
@@ -42,20 +42,21 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ConvexHull2D::onInit ()
|
||||
pcl_ros::ConvexHull2D::onInit()
|
||||
{
|
||||
PCLNodelet::onInit ();
|
||||
PCLNodelet::onInit();
|
||||
|
||||
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
|
||||
pub_plane_ = advertise<geometry_msgs::PolygonStamped> (*pnh_, "output_polygon", max_queue_size_);
|
||||
pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
|
||||
pub_plane_ = advertise<geometry_msgs::PolygonStamped>(*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");
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - use_indices : %s",
|
||||
getName().c_str(),
|
||||
(use_indices_) ? "true" : "false");
|
||||
|
||||
onInitPostProcess();
|
||||
}
|
||||
@@ -65,139 +66,150 @@ void
|
||||
pcl_ros::ConvexHull2D::subscribe()
|
||||
{
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_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_)
|
||||
{
|
||||
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
|
||||
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_);
|
||||
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));
|
||||
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
|
||||
} 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 ()));
|
||||
sub_input_ =
|
||||
pnh_->subscribe<PointCloud>(
|
||||
"input", 1,
|
||||
bind(&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr()));
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ConvexHull2D::unsubscribe()
|
||||
{
|
||||
if (use_indices_)
|
||||
{
|
||||
if (use_indices_) {
|
||||
sub_input_filter_.unsubscribe();
|
||||
sub_indices_filter_.unsubscribe();
|
||||
}
|
||||
else
|
||||
} else {
|
||||
sub_input_.shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices)
|
||||
pcl_ros::ConvexHull2D::input_indices_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0 && pub_plane_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
PointCloud output;
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
if (!isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
|
||||
// Publish an empty message
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices, "indices"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
if (indices && !isValid(indices, "indices")) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
|
||||
// Publish an empty message
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
|
||||
getMTPrivateNodeHandle().resolveName("input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(), cloud->width * cloud->height, fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
|
||||
getMTPrivateNodeHandle().resolveName("input").c_str());
|
||||
}
|
||||
|
||||
// Reset the indices and surface pointers
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices)
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
if (indices) {
|
||||
indices_ptr.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setIndices (indices_ptr);
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
impl_.setIndices(indices_ptr);
|
||||
|
||||
// Estimate the feature
|
||||
impl_.reconstruct (output);
|
||||
impl_.reconstruct(output);
|
||||
|
||||
// If more than 3 points are present, send a PolygonStamped hull too
|
||||
if (output.points.size () >= 3)
|
||||
{
|
||||
if (output.points.size() >= 3) {
|
||||
geometry_msgs::PolygonStamped poly;
|
||||
poly.header = fromPCL(output.header);
|
||||
poly.polygon.points.resize (output.points.size ());
|
||||
poly.polygon.points.resize(output.points.size());
|
||||
// Get three consecutive points (without copying)
|
||||
pcl::Vector4fMap O = output.points[1].getVector4fMap ();
|
||||
pcl::Vector4fMap B = output.points[0].getVector4fMap ();
|
||||
pcl::Vector4fMap A = output.points[2].getVector4fMap ();
|
||||
pcl::Vector4fMap O = output.points[1].getVector4fMap();
|
||||
pcl::Vector4fMap B = output.points[0].getVector4fMap();
|
||||
pcl::Vector4fMap A = output.points[2].getVector4fMap();
|
||||
// Check the direction of points -- polygon must have CCW direction
|
||||
Eigen::Vector4f OA = A - O;
|
||||
Eigen::Vector4f OB = B - O;
|
||||
Eigen::Vector4f N = OA.cross3 (OB);
|
||||
double theta = N.dot (O);
|
||||
Eigen::Vector4f N = OA.cross3(OB);
|
||||
double theta = N.dot(O);
|
||||
bool reversed = false;
|
||||
if (theta < (M_PI / 2.0))
|
||||
if (theta < (M_PI / 2.0)) {
|
||||
reversed = true;
|
||||
for (size_t i = 0; i < output.points.size (); ++i)
|
||||
{
|
||||
if (reversed)
|
||||
{
|
||||
size_t j = output.points.size () - i - 1;
|
||||
}
|
||||
for (size_t i = 0; i < output.points.size(); ++i) {
|
||||
if (reversed) {
|
||||
size_t j = output.points.size() - i - 1;
|
||||
poly.polygon.points[i].x = output.points[j].x;
|
||||
poly.polygon.points[i].y = output.points[j].y;
|
||||
poly.polygon.points[i].z = output.points[j].z;
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
poly.polygon.points[i].x = output.points[i].x;
|
||||
poly.polygon.points[i].y = output.points[i].y;
|
||||
poly.polygon.points[i].z = output.points[i].z;
|
||||
}
|
||||
}
|
||||
pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
|
||||
pub_plane_.publish(boost::make_shared<const geometry_msgs::PolygonStamped>(poly));
|
||||
}
|
||||
// Publish a Boost shared ptr const data
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
}
|
||||
|
||||
typedef pcl_ros::ConvexHull2D ConvexHull2D;
|
||||
PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet)
|
||||
|
||||
|
||||
@@ -40,192 +40,211 @@
|
||||
#include <pcl/io/io.h>
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::onInit ()
|
||||
pcl_ros::MovingLeastSquares::onInit()
|
||||
{
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
PCLNodelet::onInit();
|
||||
|
||||
//ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
|
||||
pub_output_ = advertise<PointCloudIn> (*pnh_, "output", max_queue_size_);
|
||||
pub_normals_ = advertise<NormalCloudOut> (*pnh_, "normals", max_queue_size_);
|
||||
|
||||
//if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
|
||||
if (!pnh_->getParam ("search_radius", search_radius_))
|
||||
{
|
||||
pub_output_ = advertise<PointCloudIn>(*pnh_, "output", max_queue_size_);
|
||||
pub_normals_ = advertise<NormalCloudOut>(*pnh_, "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 ());
|
||||
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 ());
|
||||
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);
|
||||
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_);
|
||||
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");
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - use_indices : %s",
|
||||
getName().c_str(),
|
||||
(use_indices_) ? "true" : "false");
|
||||
|
||||
onInitPostProcess ();
|
||||
onInitPostProcess();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::subscribe ()
|
||||
pcl_ros::MovingLeastSquares::subscribe()
|
||||
{
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_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_)
|
||||
{
|
||||
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_);
|
||||
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_);
|
||||
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));
|
||||
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
|
||||
} 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 ()));
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::unsubscribe ()
|
||||
{
|
||||
if (use_indices_)
|
||||
{
|
||||
sub_input_filter_.unsubscribe ();
|
||||
sub_indices_filter_.unsubscribe ();
|
||||
sub_input_ =
|
||||
pnh_->subscribe<PointCloudIn>(
|
||||
"input", 1,
|
||||
bind(&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr()));
|
||||
}
|
||||
else
|
||||
sub_input_.shutdown ();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices)
|
||||
pcl_ros::MovingLeastSquares::unsubscribe()
|
||||
{
|
||||
if (use_indices_) {
|
||||
sub_input_filter_.unsubscribe();
|
||||
sub_indices_filter_.unsubscribe();
|
||||
} else {
|
||||
sub_input_.shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::input_indices_callback(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
|
||||
if (pub_output_.getNumSubscribers() <= 0 && pub_normals_.getNumSubscribers() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Output points have the same type as the input, they are only smoothed
|
||||
PointCloudIn output;
|
||||
|
||||
|
||||
// Normals are also estimated and published on a separate topic
|
||||
NormalCloudOut::Ptr normals (new NormalCloudOut ());
|
||||
NormalCloudOut::Ptr normals(new NormalCloudOut());
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
if (!isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices, "indices"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
if (indices && !isValid(indices, "indices")) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
|
||||
getMTPrivateNodeHandle().resolveName("input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
|
||||
getName().c_str(), cloud->width * cloud->height, fromPCL(
|
||||
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
|
||||
getMTPrivateNodeHandle().resolveName("input").c_str());
|
||||
}
|
||||
///
|
||||
|
||||
// Reset the indices and surface pointers
|
||||
impl_.setInputCloud (pcl_ptr(cloud));
|
||||
impl_.setInputCloud(pcl_ptr(cloud));
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices)
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
if (indices) {
|
||||
indices_ptr.reset(new std::vector<int>(indices->indices));
|
||||
}
|
||||
|
||||
impl_.setIndices (indices_ptr);
|
||||
impl_.setIndices(indices_ptr);
|
||||
|
||||
// Initialize the spatial locator
|
||||
|
||||
|
||||
// Do the reconstructon
|
||||
//impl_.process (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (ros_ptr(output.makeShared ()));
|
||||
pub_output_.publish(ros_ptr(output.makeShared()));
|
||||
normals->header = cloud->header;
|
||||
pub_normals_.publish (ros_ptr(normals));
|
||||
pub_normals_.publish(ros_ptr(normals));
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
|
||||
pcl_ros::MovingLeastSquares::config_callback(MLSConfig & config, uint32_t level)
|
||||
{
|
||||
// \Note Zoli, shouldn't this be implemented in MLS too?
|
||||
/*if (k_ != config.k_search)
|
||||
{
|
||||
k_ = config.k_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_);
|
||||
NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_);
|
||||
}*/
|
||||
if (search_radius_ != config.search_radius)
|
||||
{
|
||||
if (search_radius_ != config.search_radius) {
|
||||
search_radius_ = config.search_radius;
|
||||
NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
|
||||
impl_.setSearchRadius (search_radius_);
|
||||
NODELET_DEBUG("[config_callback] Setting the search radius: %f.", search_radius_);
|
||||
impl_.setSearchRadius(search_radius_);
|
||||
}
|
||||
if (spatial_locator_type_ != config.spatial_locator)
|
||||
{
|
||||
if (spatial_locator_type_ != config.spatial_locator) {
|
||||
spatial_locator_type_ = config.spatial_locator;
|
||||
NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the spatial locator to type: %d.",
|
||||
spatial_locator_type_);
|
||||
}
|
||||
if (use_polynomial_fit_ != config.use_polynomial_fit)
|
||||
{
|
||||
if (use_polynomial_fit_ != config.use_polynomial_fit) {
|
||||
use_polynomial_fit_ = config.use_polynomial_fit;
|
||||
NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
|
||||
impl_.setPolynomialFit (use_polynomial_fit_);
|
||||
NODELET_DEBUG(
|
||||
"[config_callback] Setting the use_polynomial_fit flag to: %d.",
|
||||
use_polynomial_fit_);
|
||||
impl_.setPolynomialFit(use_polynomial_fit_);
|
||||
}
|
||||
if (polynomial_order_ != config.polynomial_order)
|
||||
{
|
||||
if (polynomial_order_ != config.polynomial_order) {
|
||||
polynomial_order_ = config.polynomial_order;
|
||||
NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
|
||||
impl_.setPolynomialOrder (polynomial_order_);
|
||||
NODELET_DEBUG("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
|
||||
impl_.setPolynomialOrder(polynomial_order_);
|
||||
}
|
||||
if (gaussian_parameter_ != config.gaussian_parameter)
|
||||
{
|
||||
if (gaussian_parameter_ != config.gaussian_parameter) {
|
||||
gaussian_parameter_ = config.gaussian_parameter;
|
||||
NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
|
||||
impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
|
||||
NODELET_DEBUG("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
|
||||
impl_.setSqrGaussParam(gaussian_parameter_ * gaussian_parameter_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -44,5 +44,3 @@
|
||||
// Include the implementations here instead of compiling them separately to speed up compile time
|
||||
//#include "convex_hull.cpp"
|
||||
//#include "moving_least_squares.cpp"
|
||||
|
||||
|
||||
|
||||
@@ -58,7 +58,7 @@ typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PCDType;
|
||||
/// Sets pcl_stamp from stamp, BUT alters stamp
|
||||
/// a little to round it to millisecond. This is because converting back
|
||||
/// and forth from pcd to ros time induces some rounding errors.
|
||||
void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp)
|
||||
void setStamp(ros::Time & stamp, std::uint64_t & pcl_stamp)
|
||||
{
|
||||
// round to millisecond
|
||||
static const uint32_t mult = 1e6;
|
||||
@@ -71,7 +71,7 @@ void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp)
|
||||
{
|
||||
ros::Time t;
|
||||
pcl_conversions::fromPCL(pcl_stamp, t);
|
||||
ROS_ASSERT_MSG(t==stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec);
|
||||
ROS_ASSERT_MSG(t == stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -79,18 +79,18 @@ class Notification
|
||||
{
|
||||
public:
|
||||
Notification(int expected_count)
|
||||
: count_(0)
|
||||
, expected_count_(expected_count)
|
||||
, failure_count_(0)
|
||||
: count_(0),
|
||||
expected_count_(expected_count),
|
||||
failure_count_(0)
|
||||
{
|
||||
}
|
||||
|
||||
void notify(const PCDType::ConstPtr& message)
|
||||
void notify(const PCDType::ConstPtr & message)
|
||||
{
|
||||
++count_;
|
||||
}
|
||||
|
||||
void failure(const PCDType::ConstPtr& message, FilterFailureReason reason)
|
||||
void failure(const PCDType::ConstPtr & message, FilterFailureReason reason)
|
||||
{
|
||||
++failure_count_;
|
||||
}
|
||||
@@ -144,7 +144,11 @@ TEST(MessageFilter, preexistingTransforms)
|
||||
ros::Time stamp = ros::Time::now();
|
||||
setStamp(stamp, msg->header.stamp);
|
||||
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
|
||||
tf::StampedTransform transform(tf::Transform(
|
||||
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
|
||||
1, 2,
|
||||
3)), stamp, "frame1",
|
||||
"frame2");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
msg->header.frame_id = "frame2";
|
||||
@@ -169,7 +173,11 @@ TEST(MessageFilter, postTransforms)
|
||||
|
||||
EXPECT_EQ(0, n.count_);
|
||||
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
|
||||
tf::StampedTransform transform(tf::Transform(
|
||||
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
|
||||
1, 2,
|
||||
3)), stamp, "frame1",
|
||||
"frame2");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
ros::WallDuration(0.1).sleep();
|
||||
@@ -190,8 +198,7 @@ TEST(MessageFilter, queueSize)
|
||||
std::uint64_t pcl_stamp;
|
||||
setStamp(stamp, pcl_stamp);
|
||||
|
||||
for (int i = 0; i < 20; ++i)
|
||||
{
|
||||
for (int i = 0; i < 20; ++i) {
|
||||
PCDType::Ptr msg(new PCDType);
|
||||
msg->header.stamp = pcl_stamp;
|
||||
msg->header.frame_id = "frame2";
|
||||
@@ -202,7 +209,11 @@ TEST(MessageFilter, queueSize)
|
||||
EXPECT_EQ(0, n.count_);
|
||||
EXPECT_EQ(10, n.failure_count_);
|
||||
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
|
||||
tf::StampedTransform transform(tf::Transform(
|
||||
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
|
||||
1, 2,
|
||||
3)), stamp, "frame1",
|
||||
"frame2");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
ros::WallDuration(0.1).sleep();
|
||||
@@ -224,7 +235,11 @@ TEST(MessageFilter, setTargetFrame)
|
||||
setStamp(stamp, msg->header.stamp);
|
||||
msg->header.frame_id = "frame2";
|
||||
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2");
|
||||
tf::StampedTransform transform(tf::Transform(
|
||||
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
|
||||
1, 2,
|
||||
3)), stamp, "frame1000",
|
||||
"frame2");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
filter.add(msg);
|
||||
@@ -249,7 +264,11 @@ TEST(MessageFilter, multipleTargetFrames)
|
||||
PCDType::Ptr msg(new PCDType);
|
||||
setStamp(stamp, msg->header.stamp);
|
||||
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3");
|
||||
tf::StampedTransform transform(tf::Transform(
|
||||
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
|
||||
1, 2,
|
||||
3)), stamp, "frame1",
|
||||
"frame3");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
msg->header.frame_id = "frame3";
|
||||
@@ -283,7 +302,11 @@ TEST(MessageFilter, tolerance)
|
||||
ros::Time stamp = ros::Time::now();
|
||||
std::uint64_t pcl_stamp;
|
||||
setStamp(stamp, pcl_stamp);
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
|
||||
tf::StampedTransform transform(tf::Transform(
|
||||
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
|
||||
1, 2,
|
||||
3)), stamp, "frame1",
|
||||
"frame2");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
PCDType::Ptr msg(new PCDType);
|
||||
@@ -295,7 +318,7 @@ TEST(MessageFilter, tolerance)
|
||||
|
||||
//ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
|
||||
|
||||
transform.stamp_ += offset*1.1;
|
||||
transform.stamp_ += offset * 1.1;
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
ros::WallDuration(0.1).sleep();
|
||||
@@ -323,7 +346,11 @@ TEST(MessageFilter, outTheBackFailure)
|
||||
PCDType::Ptr msg(new PCDType);
|
||||
setStamp(stamp, msg->header.stamp);
|
||||
|
||||
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
|
||||
tf::StampedTransform transform(tf::Transform(
|
||||
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
|
||||
1, 2,
|
||||
3)), stamp, "frame1",
|
||||
"frame2");
|
||||
tf_client.setTransform(transform);
|
||||
|
||||
transform.stamp_ = stamp + ros::Duration(10000);
|
||||
@@ -359,16 +386,17 @@ TEST(MessageFilter, removeCallback)
|
||||
|
||||
// TF filters; no data needs to be published
|
||||
boost::scoped_ptr<tf::TransformListener> tf_listener;
|
||||
boost::scoped_ptr<tf::MessageFilter<PCDType> > tf_filter;
|
||||
boost::scoped_ptr<tf::MessageFilter<PCDType>> tf_filter;
|
||||
|
||||
spinner.start();
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
tf_listener.reset(new tf::TransformListener());
|
||||
// Have callback fire at high rate to increase chances of race condition
|
||||
tf_filter.reset(
|
||||
new tf::MessageFilter<PCDType>(*tf_listener,
|
||||
"map", 5, threaded_nh,
|
||||
ros::Duration(0.000001)));
|
||||
new tf::MessageFilter<PCDType>(
|
||||
*tf_listener,
|
||||
"map", 5, threaded_nh,
|
||||
ros::Duration(0.000001)));
|
||||
|
||||
// Sleep and reset; sleeping increases chances of race condition
|
||||
ros::Duration(0.001).sleep();
|
||||
@@ -378,7 +406,7 @@ TEST(MessageFilter, removeCallback)
|
||||
spinner.stop();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
|
||||
+239
-133
@@ -45,159 +45,145 @@ namespace pcl_ros
|
||||
{
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in,
|
||||
sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)
|
||||
transformPointCloud(
|
||||
const std::string & target_frame, const sensor_msgs::PointCloud2 & in,
|
||||
sensor_msgs::PointCloud2 & out, const tf::TransformListener & tf_listener)
|
||||
{
|
||||
if (in.header.frame_id == target_frame)
|
||||
{
|
||||
if (in.header.frame_id == target_frame) {
|
||||
out = in;
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Get the TF transform
|
||||
tf::StampedTransform transform;
|
||||
try
|
||||
{
|
||||
tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform);
|
||||
}
|
||||
catch (tf::LookupException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
}
|
||||
catch (tf::ExtrapolationException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
try {
|
||||
tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform);
|
||||
} catch (tf::LookupException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
} catch (tf::ExtrapolationException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
// Convert the TF transform to Eigen format
|
||||
Eigen::Matrix4f eigen_transform;
|
||||
transformAsMatrix (transform, eigen_transform);
|
||||
transformAsMatrix(transform, eigen_transform);
|
||||
|
||||
transformPointCloud (eigen_transform, in, out);
|
||||
transformPointCloud(eigen_transform, in, out);
|
||||
|
||||
out.header.frame_id = target_frame;
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform,
|
||||
const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
|
||||
void
|
||||
transformPointCloud(
|
||||
const std::string & target_frame, const tf::Transform & net_transform,
|
||||
const sensor_msgs::PointCloud2 & in, sensor_msgs::PointCloud2 & out)
|
||||
{
|
||||
if (in.header.frame_id == target_frame)
|
||||
{
|
||||
if (in.header.frame_id == target_frame) {
|
||||
out = in;
|
||||
return;
|
||||
}
|
||||
|
||||
// Get the transformation
|
||||
Eigen::Matrix4f transform;
|
||||
transformAsMatrix (net_transform, transform);
|
||||
transformAsMatrix(net_transform, transform);
|
||||
|
||||
transformPointCloud (transform, in, out);
|
||||
transformPointCloud(transform, in, out);
|
||||
|
||||
out.header.frame_id = target_frame;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in,
|
||||
sensor_msgs::PointCloud2 &out)
|
||||
void
|
||||
transformPointCloud(
|
||||
const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in,
|
||||
sensor_msgs::PointCloud2 & out)
|
||||
{
|
||||
// Get X-Y-Z indices
|
||||
int x_idx = pcl::getFieldIndex (in, "x");
|
||||
int y_idx = pcl::getFieldIndex (in, "y");
|
||||
int z_idx = pcl::getFieldIndex (in, "z");
|
||||
int x_idx = pcl::getFieldIndex(in, "x");
|
||||
int y_idx = pcl::getFieldIndex(in, "y");
|
||||
int z_idx = pcl::getFieldIndex(in, "z");
|
||||
|
||||
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
|
||||
{
|
||||
ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
|
||||
if (x_idx == -1 || y_idx == -1 || z_idx == -1) {
|
||||
ROS_ERROR("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
|
||||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
|
||||
in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
|
||||
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
|
||||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
|
||||
in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
|
||||
{
|
||||
ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.");
|
||||
ROS_ERROR("X-Y-Z coordinates not floats. Currently only floats are supported.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check if distance is available
|
||||
int dist_idx = pcl::getFieldIndex (in, "distance");
|
||||
int dist_idx = pcl::getFieldIndex(in, "distance");
|
||||
|
||||
// Copy the other data
|
||||
if (&in != &out)
|
||||
{
|
||||
if (&in != &out) {
|
||||
out.header = in.header;
|
||||
out.height = in.height;
|
||||
out.width = in.width;
|
||||
out.width = in.width;
|
||||
out.fields = in.fields;
|
||||
out.is_bigendian = in.is_bigendian;
|
||||
out.point_step = in.point_step;
|
||||
out.row_step = in.row_step;
|
||||
out.is_dense = in.is_dense;
|
||||
out.data.resize (in.data.size ());
|
||||
out.point_step = in.point_step;
|
||||
out.row_step = in.row_step;
|
||||
out.is_dense = in.is_dense;
|
||||
out.data.resize(in.data.size());
|
||||
// Copy everything as it's faster than copying individual elements
|
||||
memcpy (&out.data[0], &in.data[0], in.data.size ());
|
||||
memcpy(&out.data[0], &in.data[0], in.data.size());
|
||||
}
|
||||
|
||||
Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
|
||||
Eigen::Array4i xyz_offset(in.fields[x_idx].offset, in.fields[y_idx].offset,
|
||||
in.fields[z_idx].offset, 0);
|
||||
|
||||
for (size_t i = 0; i < in.width * in.height; ++i)
|
||||
{
|
||||
Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
|
||||
for (size_t i = 0; i < in.width * in.height; ++i) {
|
||||
Eigen::Vector4f pt(*(float *)&in.data[xyz_offset[0]], *(float *)&in.data[xyz_offset[1]],
|
||||
*(float *)&in.data[xyz_offset[2]], 1);
|
||||
Eigen::Vector4f pt_out;
|
||||
|
||||
|
||||
bool max_range_point = false;
|
||||
int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
|
||||
float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
|
||||
if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
|
||||
{
|
||||
if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point
|
||||
{
|
||||
int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset;
|
||||
float * distance_ptr = (dist_idx < 0 ? NULL : (float *)(&in.data[distance_ptr_offset]));
|
||||
if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
|
||||
if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
|
||||
pt_out = pt;
|
||||
}
|
||||
else // max range point
|
||||
{
|
||||
} else { // max range point
|
||||
pt[0] = *distance_ptr; // Replace x with the x value saved in distance
|
||||
pt_out = transform * pt;
|
||||
max_range_point = true;
|
||||
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
pt_out = transform * pt;
|
||||
}
|
||||
|
||||
if (max_range_point)
|
||||
{
|
||||
if (max_range_point) {
|
||||
// Save x value in distance again
|
||||
*(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
|
||||
*(float *)(&out.data[distance_ptr_offset]) = pt_out[0];
|
||||
pt_out[0] = std::numeric_limits<float>::quiet_NaN();
|
||||
}
|
||||
|
||||
memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float));
|
||||
memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float));
|
||||
memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float));
|
||||
|
||||
|
||||
memcpy(&out.data[xyz_offset[0]], &pt_out[0], sizeof(float));
|
||||
memcpy(&out.data[xyz_offset[1]], &pt_out[1], sizeof(float));
|
||||
memcpy(&out.data[xyz_offset[2]], &pt_out[2], sizeof(float));
|
||||
|
||||
|
||||
xyz_offset += in.point_step;
|
||||
}
|
||||
|
||||
// Check if the viewpoint information is present
|
||||
int vp_idx = pcl::getFieldIndex (in, "vp_x");
|
||||
if (vp_idx != -1)
|
||||
{
|
||||
int vp_idx = pcl::getFieldIndex(in, "vp_x");
|
||||
if (vp_idx != -1) {
|
||||
// Transform the viewpoint info too
|
||||
for (size_t i = 0; i < out.width * out.height; ++i)
|
||||
{
|
||||
float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
|
||||
for (size_t i = 0; i < out.width * out.height; ++i) {
|
||||
float * pstep = (float *)&out.data[i * out.point_step + out.fields[vp_idx].offset];
|
||||
// Assume vp_x, vp_y, vp_z are consecutive
|
||||
Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
|
||||
Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1);
|
||||
Eigen::Vector4f vp_out = transform * vp_in;
|
||||
|
||||
pstep[0] = vp_out[0];
|
||||
@@ -209,73 +195,193 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
|
||||
transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat)
|
||||
{
|
||||
double mv[12];
|
||||
bt.getBasis ().getOpenGLSubMatrix (mv);
|
||||
bt.getBasis().getOpenGLSubMatrix(mv);
|
||||
|
||||
tf::Vector3 origin = bt.getOrigin ();
|
||||
tf::Vector3 origin = bt.getOrigin();
|
||||
|
||||
out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
|
||||
out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
|
||||
out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
|
||||
|
||||
out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
|
||||
out_mat (0, 3) = origin.x ();
|
||||
out_mat (1, 3) = origin.y ();
|
||||
out_mat (2, 3) = origin.z ();
|
||||
out_mat(0, 0) = mv[0]; out_mat(0, 1) = mv[4]; out_mat(0, 2) = mv[8];
|
||||
out_mat(1, 0) = mv[1]; out_mat(1, 1) = mv[5]; out_mat(1, 2) = mv[9];
|
||||
out_mat(2, 0) = mv[2]; out_mat(2, 1) = mv[6]; out_mat(2, 2) = mv[10];
|
||||
|
||||
out_mat(3, 0) = out_mat(3, 1) = out_mat(3, 2) = 0; out_mat(3, 3) = 1;
|
||||
out_mat(0, 3) = origin.x();
|
||||
out_mat(1, 3) = origin.y();
|
||||
out_mat(2, 3) = origin.z();
|
||||
}
|
||||
|
||||
} // namespace pcl_ros
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
|
||||
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
|
||||
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
|
||||
const tf::Transform &);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
|
||||
const tf::TransformListener &);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZ> (const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::InterestPoint> (const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointWithRange> (const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZ>(
|
||||
const pcl::PointCloud<pcl::PointXYZ> &,
|
||||
pcl::PointCloud<pcl::PointXYZ> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZI>(
|
||||
const pcl::PointCloud<pcl::PointXYZI> &,
|
||||
pcl::PointCloud<pcl::PointXYZI> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
|
||||
const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
|
||||
const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::InterestPoint>(
|
||||
const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointNormal>(
|
||||
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
|
||||
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointWithRange>(
|
||||
const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
|
||||
const tf::Transform &);
|
||||
template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
|
||||
const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
|
||||
const tf::Transform &);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZ> &,
|
||||
pcl::PointCloud<pcl::PointXYZ> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZI> &,
|
||||
pcl::PointCloud<pcl::PointXYZI> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
|
||||
const std::string &,
|
||||
const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
|
||||
const tf::TransformListener &);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZ> &, const std::string &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZI> &, const std::string &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBA> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGB> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::InterestPoint> &, const std::string &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithRange> &, const std::string &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithViewpoint> &, const std::string &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
|
||||
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
|
||||
const std::string &, const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZ> &,
|
||||
const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZ> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
|
||||
const std::string &, const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZI> &,
|
||||
const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZI> &,
|
||||
const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGBA> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZRGBA> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
|
||||
const std::string &, const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGB> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZRGB> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::InterestPoint> &, const std::string &,
|
||||
pcl::PointCloud<pcl::InterestPoint> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
|
||||
const std::string &, const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointWithRange> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointWithRange> &, const tf::TransformListener &);
|
||||
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
|
||||
const std::string &,
|
||||
const ros::Time &,
|
||||
const pcl::PointCloud<pcl::PointWithViewpoint> &, const std::string &,
|
||||
pcl::PointCloud<pcl::PointWithViewpoint> &, const tf::TransformListener &);
|
||||
|
||||
Reference in New Issue
Block a user