migrating nodelets and tools from fuerte release to pcl_ros
This commit is contained in:
Julius Kammerl
committed by
Paul Bovbel
Paul Bovbel
parent
d5d9e3816a
commit
a3701bb3df
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: extract_indices.cpp 35876 2011-02-09 01:04:36Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/filters/extract_indices.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
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);
|
||||
|
||||
use_indices_ = true;
|
||||
return (true);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level)
|
||||
{
|
||||
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"));
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::ExtractIndices ExtractIndices;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, ExtractIndices, ExtractIndices, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/boundary.h"
|
||||
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
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_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, BoundaryEstimation, BoundaryEstimation, nodelet::Nodelet);
|
||||
@@ -0,0 +1,443 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
//#include <pluginlib/class_list_macros.h>
|
||||
// Include the implementations here instead of compiling them separately to speed up compile time
|
||||
//#include "normal_3d.cpp"
|
||||
//#include "boundary.cpp"
|
||||
//#include "fpfh.cpp"
|
||||
//#include "fpfh_omp.cpp"
|
||||
//#include "moment_invariants.cpp"
|
||||
//#include "normal_3d_omp.cpp"
|
||||
//#include "normal_3d_tbb.cpp"
|
||||
//#include "pfh.cpp"
|
||||
//#include "principal_curvatures.cpp"
|
||||
//#include "vfh.cpp"
|
||||
#include <pcl/io/io.h>
|
||||
#include "pcl_ros/features/feature.h"
|
||||
#include <message_filters/null_types.h>
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
|
||||
// Allow each individual class that inherits from us to declare their own Publisher
|
||||
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
|
||||
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("use_surface", use_surface_);
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
|
||||
if (use_indices_ || use_surface_)
|
||||
{
|
||||
// Create the objects here
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
|
||||
else
|
||||
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
|
||||
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
if (use_indices_)
|
||||
{
|
||||
// If indices are enabled, subscribe to the indices
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
if (use_surface_) // Use both indices and surface
|
||||
{
|
||||
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
}
|
||||
else // Use only indices
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
|
||||
}
|
||||
}
|
||||
else // Use only surface
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
|
||||
// indices not enabled, connect the input-surface duo and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
|
||||
else
|
||||
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
|
||||
}
|
||||
// Register callbacks
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
|
||||
else
|
||||
sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
|
||||
}
|
||||
else
|
||||
// Subscribe in an old fashion to input only (no filters)
|
||||
sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - use_surface : %s\n"
|
||||
" - k_search : %d\n"
|
||||
" - radius_search : %f\n"
|
||||
" - spatial_locator: %d",
|
||||
getName ().c_str (),
|
||||
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::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)
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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 ((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));
|
||||
|
||||
computePublish (cloud, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
|
||||
// Allow each individual class that inherits from us to declare their own Publisher
|
||||
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
|
||||
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("use_surface", use_surface_);
|
||||
|
||||
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);
|
||||
|
||||
// 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 we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
|
||||
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
|
||||
{
|
||||
// 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_)
|
||||
// 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
|
||||
// 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_);
|
||||
}
|
||||
}
|
||||
else // Use only surface
|
||||
{
|
||||
// indices not enabled, connect the input-surface duo and register
|
||||
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_);
|
||||
}
|
||||
}
|
||||
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_);
|
||||
}
|
||||
|
||||
// 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));
|
||||
|
||||
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)
|
||||
{
|
||||
// No subscribers, no work
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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 ((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));
|
||||
|
||||
computePublish (cloud, cloud_normals, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh.h"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
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_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimation FPFHEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimation, FPFHEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh_omp.h"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
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_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimationOMP, FPFHEstimationOMP, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/moment_invariants.h"
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, MomentInvariantsEstimation, MomentInvariantsEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d.h"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimation NormalEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimation, NormalEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d_omp.h"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationOMP, NormalEstimationOMP, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d_tbb.h"
|
||||
|
||||
#if defined HAVE_TBB
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloud output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationTBB, NormalEstimationTBB, nodelet::Nodelet);
|
||||
|
||||
#endif // HAVE_TBB
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/pfh.h"
|
||||
|
||||
void
|
||||
pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
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_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PFHEstimation PFHEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PFHEstimation, PFHEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/principal_curvatures.h"
|
||||
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
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_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PrincipalCurvaturesEstimation, PrincipalCurvaturesEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/vfh.h"
|
||||
|
||||
void
|
||||
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
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_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut 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 ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::VFHEstimation VFHEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, VFHEstimation, VFHEstimation, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,230 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: filter.cpp 35876 2011-02-09 01:04:36Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pcl/io/io.h>
|
||||
#include "pcl_ros/transforms.h"
|
||||
#include "pcl_ros/filters/filter.h"
|
||||
|
||||
/*//#include <pcl/filters/pixel_grid.h>
|
||||
//#include <pcl/filters/filter_dimension.h>
|
||||
*/
|
||||
|
||||
/*//typedef pcl::PixelGrid PixelGrid;
|
||||
//typedef pcl::FilterDimension FilterDimension;
|
||||
*/
|
||||
|
||||
// Include the implementations instead of compiling them separately to speed up compile time
|
||||
//#include "extract_indices.cpp"
|
||||
//#include "passthrough.cpp"
|
||||
//#include "project_inliers.cpp"
|
||||
//#include "radius_outlier_removal.cpp"
|
||||
//#include "statistical_outlier_removal.cpp"
|
||||
//#include "voxel_grid.cpp"
|
||||
|
||||
/*//PLUGINLIB_DECLARE_CLASS (pcl, PixelGrid, PixelGrid, nodelet::Nodelet);
|
||||
//PLUGINLIB_DECLARE_CLASS (pcl, FilterDimension, FilterDimension, nodelet::Nodelet);
|
||||
*/
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
|
||||
{
|
||||
PointCloud2 output;
|
||||
// Call the virtual method in the child
|
||||
filter (input, indices, output);
|
||||
|
||||
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 ());
|
||||
// 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 ());
|
||||
return;
|
||||
}
|
||||
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 ());
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud2 cloud_transformed;
|
||||
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 ());
|
||||
return;
|
||||
}
|
||||
cloud_tf.reset (new PointCloud2 (cloud_transformed));
|
||||
}
|
||||
|
||||
// Publish a boost shared ptr
|
||||
pub_output_.publish (cloud_tf);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::onInit ()
|
||||
{
|
||||
// Call the super 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 ());
|
||||
return;
|
||||
}
|
||||
|
||||
pub_output_ = pnh_->advertise<PointCloud2> ("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 we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_)
|
||||
{
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl::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::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
|
||||
// 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::PointIndicesConstPtr ()));
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
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)
|
||||
{
|
||||
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 ());
|
||||
}
|
||||
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 ());
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
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 ());
|
||||
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 ());
|
||||
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 ());
|
||||
///
|
||||
|
||||
// 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 ());
|
||||
// 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 ());
|
||||
return;
|
||||
}
|
||||
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));
|
||||
|
||||
computePublish (cloud_tf, vindices);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,120 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: passthrough.cpp 36194 2011-02-23 07:49:21Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/filters/passthrough.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
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);
|
||||
|
||||
return (true);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
double 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)
|
||||
{
|
||||
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);
|
||||
// Set the filter min-max if different
|
||||
impl_.setFilterLimits (filter_min, filter_max);
|
||||
}
|
||||
// Check the current values for filter min-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);
|
||||
// Set the filter min-max if different
|
||||
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)
|
||||
{
|
||||
// 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 ());
|
||||
}
|
||||
|
||||
// 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");
|
||||
// Call the virtual method in the child
|
||||
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");
|
||||
// Call the virtual method in the child
|
||||
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)
|
||||
{
|
||||
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 ());
|
||||
}
|
||||
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 ());
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::PassThrough PassThrough;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PassThrough, PassThrough, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: project_inliers.cpp 35876 2011-02-09 01:04:36Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/filters/project_inliers.h"
|
||||
#include <pcl/io/io.h>
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ProjectInliers::onInit ()
|
||||
{
|
||||
// No need to call the super onInit as we are overwriting everything
|
||||
//Filter<sensor_msgs::PointCloud2>::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 ());
|
||||
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.
|
||||
bool copy_all_fields = true;
|
||||
|
||||
pnh_->getParam ("copy_all_data", copy_all_data);
|
||||
pnh_->getParam ("copy_all_fields", copy_all_fields);
|
||||
|
||||
pub_output_ = pnh_->advertise<PointCloud2> ("output", max_queue_size_);
|
||||
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
|
||||
/*
|
||||
TODO : implement use_indices_
|
||||
if (use_indices_)
|
||||
{*/
|
||||
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", 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));
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
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 ());
|
||||
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));
|
||||
|
||||
model_ = model;
|
||||
computePublish (cloud, vindices);
|
||||
}
|
||||
|
||||
typedef pcl_ros::ProjectInliers ProjectInliers;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, ProjectInliers, ProjectInliers, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: radius_outlier_removal.cpp 33319 2010-10-15 04:49:28Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/filters/radius_outlier_removal.h"
|
||||
|
||||
typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, RadiusOutlierRemoval, RadiusOutlierRemoval, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: statistical_outlier_removal.cpp 35876 2011-02-09 01:04:36Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/filters/statistical_outlier_removal.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
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);
|
||||
|
||||
return (true);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level)
|
||||
{
|
||||
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_.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_DECLARE_CLASS (pcl, StatisticalOutlierRemoval, StatisticalOutlierRemoval, nodelet::Nodelet);
|
||||
|
||||
@@ -0,0 +1,124 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/filters/voxel_grid.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
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);
|
||||
|
||||
return (true);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input,
|
||||
const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
impl_.setInputCloud (input);
|
||||
impl_.setIndices (indices);
|
||||
impl_.filter (output);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
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]);
|
||||
}
|
||||
|
||||
double filter_min, filter_max;
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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_.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)
|
||||
{
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
|
||||
}
|
||||
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 ());
|
||||
}
|
||||
// ]---
|
||||
}
|
||||
|
||||
typedef pcl_ros::VoxelGrid VoxelGrid;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, VoxelGrid, VoxelGrid, nodelet::Nodelet);
|
||||
|
||||
Reference in New Issue
Block a user