apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

migrating nodelets and tools from fuerte release to pcl_ros

Browse Source
This commit is contained in:
Julius Kammerl 2012-12-17 18:09:15 -08:00 committed by Paul Bovbel
parent d5d9e3816a
commit a3701bb3df
46 changed files with 4331 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
pcl_ros/CMakeLists.txt
View File

@ -3,7 +3,7 @@ project(pcl_ros)
# Deal with catkin
find_package(Boost COMPONENTS system filesystem thread REQUIRED)
find_package(catkin REQUIRED genmsg roscpp sensor_msgs std_msgs tf)
find_package(catkin REQUIRED dynamic_reconfigure genmsg roscpp sensor_msgs std_msgs tf rosbag rosconsole roslib)
find_package(Eigen)
find_package(PCL)
@ -17,28 +17,66 @@ include_directories(include)
link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
catkin_package(DEPENDS Eigen PCL roscpp sensor_msgs tf
INCLUDE_DIRS include
LIBRARIES pcl_ros_tf
)
# generate the dynamic_reconfigure config file
generate_dynamic_reconfigure_options(cfg/Filter.cfg
cfg/ExtractIndices.cfg
cfg/StatisticalOutlierRemoval.cfg
cfg/VoxelGrid.cfg
# create messages
### what's below is just to generate the messages that are manually patched into PCL
###project(pcl)
###add_message_files(DIRECTORY msg
### FILES ModelCoefficients.msg
### PointIndices.msg
### PolygonMesh.msg
### Vertices.msg
###)
###generate_messages(DEPENDENCIES sensor_msgs std_msgs)
###project(pcl_ros)
)
include_directories(include cfg/cpp)
catkin_package()
# ---[ Point Cloud Library - Transforms
add_library (pcl_ros_tf SHARED src/transforms.cpp)
target_link_libraries(pcl_ros_tf ${PCL_LIBS} ${Boost_LIBS} ${catkin_LIBS})
target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
add_dependencies(pcl_ros_tf ros_gencpp pcl_ros_copy)
############ NODELETS
# ---[ Point Cloud Library - IO
add_library (pcl_ros_io
src/pcl_ros/io/io.cpp
src/pcl_ros/io/concatenate_fields.cpp
src/pcl_ros/io/concatenate_data.cpp
src/pcl_ros/io/pcd_io.cpp
src/pcl_ros/io/bag_io.cpp
)
#rosbuild_add_compile_flags (pcl_ros_io ${SSE_FLAGS})
target_link_libraries (pcl_ros_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
# ---[ PCL ROS - Filters
add_library (pcl_ros_filters
src/pcl_ros/filters/filter.cpp
src/pcl_ros/filters/passthrough.cpp
src/pcl_ros/filters/project_inliers.cpp
src/pcl_ros/filters/extract_indices.cpp
src/pcl_ros/filters/radius_outlier_removal.cpp
src/pcl_ros/filters/statistical_outlier_removal.cpp
src/pcl_ros/filters/voxel_grid.cpp
)
#add_compile_flags (pcl_ros_filters ${SSE_FLAGS})
target_link_libraries (pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
############ TOOLS
add_executable (pcd_to_pointcloud tools/pcd_to_pointcloud.cpp)
target_link_libraries (pcd_to_pointcloud pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
add_executable (pointcloud_to_pcd tools/pointcloud_to_pcd.cpp)
target_link_libraries (pointcloud_to_pcd pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
add_executable (bag_to_pcd tools/bag_to_pcd.cpp)
target_link_libraries (bag_to_pcd pcl_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
add_executable (convert_pcd_to_image tools/convert_pcd_to_image.cpp)
target_link_libraries (convert_pcd_to_image pcl_io pcl_common ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
add_executable (convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp)
target_link_libraries (convert_pointcloud_to_image pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)

14
pcl_ros/cfg/ExtractIndices.cfg Executable file
View File

@ -0,0 +1,14 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import roslib; roslib.load_manifest (PACKAGE);
from dynamic_reconfigure.parameter_generator import *;
import roslib.packages
gen = ParameterGenerator ()
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("negative", bool_t, 0, "Extract indices or the negative (all-indices)", False)
exit (gen.generate (PACKAGE, "pcl_ros", "ExtractIndices"))

15
pcl_ros/cfg/Filter.cfg Executable file
View File

@ -0,0 +1,15 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import roslib;
roslib.load_manifest (PACKAGE);
from dynamic_reconfigure.parameter_generator import *;
import Filter_common as common
gen = ParameterGenerator ()
common.add_common_parameters (gen)
exit (gen.generate (PACKAGE, "pcl_ros", "Filter"))

19
pcl_ros/cfg/Filter_common.py Normal file
View File

@ -0,0 +1,19 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import roslib;
roslib.load_manifest (PACKAGE);
from dynamic_reconfigure.parameter_generator import *;
def add_common_parameters (gen):
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("filter_field_name", str_t, 0, "The field name used for filtering", "z")
gen.add ("filter_limit_min", double_t, 0, "The minimum allowed field value a point will be considered from", 0.0, -5.0, 5.0)
gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1.0, -5.0, 5.0)
gen.add ("filter_limit_negative", bool_t, 0, "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max].", False)
gen.add ("keep_organized", bool_t, 0, "Set whether the filtered points should be kept and set to NaN, or removed from the PointCloud, thus potentially breaking its organized structure.", False)
gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.", "")
gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different.", "")

BIN
pcl_ros/cfg/Filter_common.pyc Normal file
View File

Binary file not shown.

18
pcl_ros/cfg/StatisticalOutlierRemoval.cfg Executable file
View File

@ -0,0 +1,18 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import roslib;
roslib.load_manifest (PACKAGE);
from dynamic_reconfigure.parameter_generator import *;
gen = ParameterGenerator ()
# enabling/disabling the unit limits
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("mean_k", int_t, 0, "The number of points (k) to use for mean distance estimation", 2, 2, 100)
gen.add ("stddev", double_t, 0, "The standard deviation multiplier threshold. All points outside the mean +- sigma * std_mul will be considered outliers.", 0.0, 0.0, 5.0)
gen.add ("negative", bool_t, 0, "Set whether the inliers should be returned (true) or the outliers (false)", False)
exit (gen.generate (PACKAGE, "pcl_ros", "StatisticalOutlierRemoval"))

16
pcl_ros/cfg/VoxelGrid.cfg Executable file
View File

@ -0,0 +1,16 @@
#! /usr/bin/env python
# set up parameters that we care about
PACKAGE = 'pcl_ros'
import roslib; roslib.load_manifest (PACKAGE);
from dynamic_reconfigure.parameter_generator import *;
import roslib.packages
import Filter_common as common
gen = ParameterGenerator ()
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
gen.add ("leaf_size", double_t, 0, "The size of a leaf (on x,y,z) used for downsampling.", 0.01, 0, 1.0)
common.add_common_parameters (gen)
exit (gen.generate (PACKAGE, "pcl_ros", "VoxelGrid"))

94
pcl_ros/include/pcl_ros/filters/extract_indices.h Normal file
View File

@ -0,0 +1,94 @@
/*
* 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.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_
#define PCL_ROS_FILTERS_EXTRACTINDICES_H_
// PCL includes
#include <pcl/filters/extract_indices.h>
#include "pcl_ros/filters/filter.h"
#include "pcl_ros/ExtractIndicesConfig.h"
namespace pcl_ros
{
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ExtractIndices : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
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);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback. */
void
config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::ExtractIndices<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_

149
pcl_ros/include/pcl_ros/filters/filter.h Normal file
View File

@ -0,0 +1,149 @@
/*
* 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.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTER_H_
#define PCL_ROS_FILTER_H_
// PCL includes
#include <pcl/filters/filter.h>
#include "pcl_ros/pcl_nodelet.h"
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/FilterConfig.h"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters
* are defined here as static methods.
* \author Radu Bogdan Rusu
*/
class Filter : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
Filter () {}
protected:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
message_filters::Subscriber<PointCloud2> sub_input_filter_;
/** \brief The desired user filter field name. */
std::string filter_field_name_;
/** \brief The minimum allowed filter value a point will be considered from. */
double filter_limit_min_;
/** \brief The maximum allowed filter value a point will be considered from. */
double filter_limit_max_;
/** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
bool filter_limit_negative_;
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
std::string tf_input_frame_;
/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
std::string tf_output_frame_;
/** \brief Internal mutex. */
boost::mutex mutex_;
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init (ros::NodeHandle &nh, bool &has_service)
{
has_service = false;
return (true);
}
/** \brief Virtual abstract filter method. To be implemented by every child.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
* \param output the resultant filtered PointCloud2
*/
virtual void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output) = 0;
/** \brief Nodelet initialization routine. */
virtual void
onInit ();
/** \brief Call the child filter () method, optionally transform the result, and publish it.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
*/
void
computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices);
private:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices> > > sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices> > > sync_input_indices_a_;
/** \brief Dynamic reconfigure service callback. */
virtual void
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
/** \brief PointCloud2 + Indices data callback. */
void
input_indices_callback (const PointCloud2::ConstPtr &cloud,
const PointIndicesConstPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTER_H_

94
pcl_ros/include/pcl_ros/filters/passthrough.h Normal file
View File

@ -0,0 +1,94 @@
/*
* 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.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_
#define PCL_ROS_FILTERS_PASSTHROUGH_H_
// PCL includes
#include <pcl/filters/passthrough.h>
#include "pcl_ros/filters/filter.h"
namespace pcl_ros
{
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
* constraints.
* \author Radu Bogdan Rusu
*/
class PassThrough : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
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);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback.
* \param config the dynamic reconfigure FilterConfig object
* \param level the dynamic reconfigure level
*/
void
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::PassThrough<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_

104
pcl_ros/include/pcl_ros/filters/project_inliers.h Normal file
View File

@ -0,0 +1,104 @@
/*
* 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.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_PROJECT_INLIERS_H_
#define PCL_ROS_FILTERS_PROJECT_INLIERS_H_
// PCL includes
#include <pcl/filters/project_inliers.h>
#include "pcl_ros/filters/filter.h"
#include <message_filters/subscriber.h>
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
* separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ProjectInliers : public Filter
{
public:
ProjectInliers () : model_ () {}
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
impl_.setInputCloud (input);
impl_.setIndices (indices);
impl_.setModelCoefficients (model_);
impl_.filter (output);
}
private:
/** \brief A pointer to the vector of model coefficients. */
ModelCoefficientsConstPtr model_;
/** \brief The message filter subscriber for model coefficients. */
message_filters::Subscriber<ModelCoefficients> sub_model_;
/** \brief Synchronized input, indices, and model coefficients.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_;
/** \brief The PCL filter implementation used. */
pcl::ProjectInliers<PointCloud2> impl_;
/** \brief Nodelet initialization routine. */
virtual void
onInit ();
/** \brief PointCloud2 + Indices + Model data callback. */
void
input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
const PointIndicesConstPtr &indices,
const ModelCoefficientsConstPtr &model);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_

82
pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h Normal file
View File

@ -0,0 +1,82 @@
/*
* 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: radius_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_
#define PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_
// PCL includes
#include <pcl/filters/radius_outlier_removal.h>
#include "pcl_ros/filters/filter.h"
namespace pcl_ros
{
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
* search radius is smaller than a given K.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class RadiusOutlierRemoval : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
impl_.setInputCloud (input);
impl_.setIndices (indices);
impl_.filter (output);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
*/
virtual inline bool child_init (ros::NodeHandle &nh) { return (true); }
private:
/** \brief The PCL filter implementation used. */
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_

102
pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h Normal file
View File

@ -0,0 +1,102 @@
/*
* 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: statistical_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_
#define PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_
// PCL includes
#include <pcl/filters/statistical_outlier_removal.h>
#include "pcl_ros/filters/filter.h"
// Dynamic reconfigure
#include "pcl_ros/StatisticalOutlierRemovalConfig.h"
namespace pcl_ros
{
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
* information check:
* <ul>
* <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
* Towards 3D Point Cloud Based Object Maps for Household Environments
* Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
* </ul>
*
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class StatisticalOutlierRemoval : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
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);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_

89
pcl_ros/include/pcl_ros/filters/voxel_grid.h Normal file
View File

@ -0,0 +1,89 @@
/*
* 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.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_VOXEL_H_
#define PCL_ROS_FILTERS_VOXEL_H_
// PCL includes
#include <pcl/filters/voxel_grid.h>
#include "pcl_ros/filters/filter.h"
// Dynamic reconfigure
#include "pcl_ros/VoxelGridConfig.h"
namespace pcl_ros
{
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
* \author Radu Bogdan Rusu
*/
class VoxelGrid : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > srv_;
/** \brief The PCL filter implementation used. */
pcl::VoxelGrid<sensor_msgs::PointCloud2> impl_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
virtual void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output);
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void
config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_

130
pcl_ros/include/pcl_ros/io/bag_io.h Normal file
View File

@ -0,0 +1,130 @@
/*
* 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: bag_io.h 35471 2011-01-25 06:50:00Z rusu $
*
*/
#ifndef PCL_ROS_IO_BAG_IO_H_
#define PCL_ROS_IO_BAG_IO_H_
#include <pcl_ros/pcl_nodelet.h>
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
namespace pcl_ros
{
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief BAG PointCloud file format reader.
* \author Radu Bogdan Rusu
*/
class BAGReader: public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/** \brief Empty constructor. */
BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {};
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; }
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate () { return (publish_rate_); }
/** \brief Get the next point cloud dataset in the BAG file.
* \return the next point cloud dataset as read from the file
*/
inline PointCloudConstPtr
getNextCloud ()
{
if (it_ != view_.end ())
{
output_ = it_->instantiate<sensor_msgs::PointCloud2> ();
++it_;
}
return (output_);
}
/** \brief Open a BAG file for reading and select a specified topic
* \param file_name the BAG file to open
* \param topic_name the topic that we want to read data from
*/
bool open (const std::string &file_name, const std::string &topic_name);
/** \brief Close an open BAG file. */
inline void
close ()
{
bag_.close ();
}
/** \brief Nodelet initialization routine. */
virtual void onInit ();
private:
/** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
double publish_rate_;
/** \brief The BAG object. */
rosbag::Bag bag_;
/** \brief The BAG view object. */
rosbag::View view_;
/** \brief The BAG view iterator object. */
rosbag::View::iterator it_;
/** \brief The name of the topic that contains the PointCloud data. */
std::string topic_name_;
/** \brief The name of the BAG file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloudPtr output_;
/** \brief Signals that a new PointCloud2 message has been read from the file. */
//bool cloud_received_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_IO_BAG_IO_H_

134
pcl_ros/include/pcl_ros/io/concatenate_data.h Normal file
View File

@ -0,0 +1,134 @@
/*
* 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: concatenate_data.h 35231 2011-01-14 05:33:20Z rusu $
*
*/
#ifndef PCL_IO_CONCATENATE_DATA_H_
#define PCL_IO_CONCATENATE_DATA_H_
// ROS includes
#include <tf/transform_listener.h>
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/pass_through.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
* checks their timestamps, and concatenates their fields together into a single
* PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateDataSynchronizer: public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Empty constructor. */
PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {};
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {};
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateDataSynchronizer () {};
void onInit ();
private:
/** \brief ROS local node handle. */
ros::NodeHandle private_nh_;
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
bool approximate_sync_;
/** \brief A vector of message filters. */
std::vector<boost::shared_ptr<message_filters::Subscriber<PointCloud2> > > filters_;
/** \brief Output TF frame the concatenated points should be transformed to. */
std::string output_frame_;
/** \brief TF listener object. */
tf::TransformListener tf_;
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointCloud2> nf_;
/** \brief Synchronizer.
* \note This will most likely be rewritten soon using the DynamicTimeSynchronizer.
*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_e_;
/** \brief Input point cloud callback.
* Because we want to use the same synchronizer object, we push back
* empty elements with the same timestamp.
*/
inline void
input_callback (const PointCloud2ConstPtr &input)
{
PointCloud2 cloud;
cloud.header.stamp = input->header.stamp;
nf_.add (boost::make_shared<PointCloud2> (cloud));
}
/** \brief Input callback for 8 synchronized topics. */
void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8);
void combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out);
};
}
#endif //#ifndef PCL_ROS_IO_CONCATENATE_H_

100
pcl_ros/include/pcl_ros/io/concatenate_fields.h Normal file
View File

@ -0,0 +1,100 @@
/*
* 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: concatenate_fields.h 35052 2011-01-03 21:04:57Z rusu $
*
*/
#ifndef PCL_IO_CONCATENATE_FIELDS_H_
#define PCL_IO_CONCATENATE_FIELDS_H_
// ROS includes
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
namespace pcl_ros
{
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
* input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
* a single PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateFieldsSynchronizer: public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/** \brief Empty constructor. */
PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {};
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {};
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateFieldsSynchronizer () {};
void onInit ();
void input_callback (const PointCloudConstPtr &cloud);
private:
/** \brief ROS local node handle. */
ros::NodeHandle private_nh_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The number of input messages that we expect on the input topic. */
int input_messages_;
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief The maximum number of seconds to wait until we drop the synchronization. */
double maximum_seconds_;
/** \brief A queue for messages. */
std::map<ros::Time, std::vector<PointCloudConstPtr> > queue_;
};
}
#endif //#ifndef PCL_IO_CONCATENATE_H_

132
pcl_ros/include/pcl_ros/io/pcd_io.h Normal file
View File

@ -0,0 +1,132 @@
/*
* 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: pcd_io.h 35054 2011-01-03 21:16:49Z rusu $
*
*/
#ifndef PCL_ROS_IO_PCD_IO_H_
#define PCL_ROS_IO_PCD_IO_H_
#include <pcl/io/pcd_io.h>
#include "pcl_ros/pcl_nodelet.h"
namespace pcl_ros
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format reader.
* \author Radu Bogdan Rusu
*/
class PCDReader : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Empty constructor. */
PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {};
virtual void onInit ();
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; }
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate () { return (publish_rate_); }
/** \brief Set the TF frame the PointCloud will be published in.
* \param tf_frame the TF frame the PointCloud will be published in
*/
inline void setTFframe (std::string tf_frame) { tf_frame_ = tf_frame; }
/** \brief Get the TF frame the PointCloud is published in. */
inline std::string getTFframe () { return (tf_frame_); }
protected:
/** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
double publish_rate_;
/** \brief The TF frame the data should be published in ("/base_link" by default). */
std::string tf_frame_;
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloud2Ptr output_;
private:
/** \brief The PCL implementation used. */
pcl::PCDReader impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format writer.
* \author Radu Bogdan Rusu
*/
class PCDWriter : public PCLNodelet
{
public:
PCDWriter () : file_name_ (""), binary_mode_ (true) {}
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
virtual void onInit ();
void input_callback (const PointCloud2ConstPtr &cloud);
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
protected:
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief Set to true if the output files should be saved in binary mode (true). */
bool binary_mode_;
private:
/** \brief The PCL implementation used. */
pcl::PCDWriter impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_IO_PCD_IO_H_

228
pcl_ros/include/pcl_ros/pcl_nodelet.h Normal file
View File

@ -0,0 +1,228 @@
/*
* 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: pcl_nodelet.h 33238 2010-03-11 00:46:58Z rusu $
*
*/
/**
\author Radu Bogdan Rusu
**/
#ifndef PCL_NODELET_H_
#define PCL_NODELET_H_
#include <sensor_msgs/PointCloud2.h>
// PCL includes
#include <pcl/PointIndices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include "pcl_ros/point_cloud.h"
// ROS Nodelet includes
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
// Include TF
#include <tf/transform_listener.h>
namespace pcl_ros
{
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */
class PCLNodelet : public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
typedef pcl::PointIndices PointIndices;
typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr;
typedef pcl::ModelCoefficients ModelCoefficients;
typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
/** \brief Empty constructor. */
PCLNodelet () : use_indices_ (false), latched_indices_ (false),
max_queue_size_ (3), approximate_sync_ (false) {};
protected:
/** \brief The ROS NodeHandle used for parameters, publish/subscribe, etc. */
boost::shared_ptr<ros::NodeHandle> pnh_;
/** \brief Set to true if point indices are used.
*
* When receiving a point cloud, if use_indices_ is false, the entire
* point cloud is processed for the given operation. If use_indices_ is
* true, then the ~indices topic is read to get the vector of point
* indices specifying the subset of the point cloud that will be used for
* the operation. In the case where use_indices_ is true, the ~input and
* ~indices topics must be synchronised in time, either exact or within a
* specified jitter. See also @ref latched_indices_ and approximate_sync.
**/
bool use_indices_;
/** \brief Set to true if the indices topic is latched.
*
* If use_indices_ is true, the ~input and ~indices topics generally must
* be synchronised in time. By setting this flag to true, the most recent
* value from ~indices can be used instead of requiring a synchronised
* message.
**/
bool latched_indices_;
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_input_filter_;
/** \brief The message filter subscriber for PointIndices. */
message_filters::Subscriber<PointIndices> sub_indices_filter_;
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The maximum queue size (default: 3). */
int max_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
bool approximate_sync_;
/** \brief TF listener object. */
tf::TransformListener tf_listener_;
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
* \param cloud the point cloud to test
* \param topic_name an optional topic name (only used for printing, defaults to "input")
*/
inline bool
isValid (const PointCloud2::ConstPtr &cloud, const std::string &topic_name = "input")
{
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ())
{
NODELET_WARN ("[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->data.size (), cloud->width, cloud->height, cloud->point_step, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (false);
}
return (true);
}
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
* \param cloud the point cloud to test
* \param topic_name an optional topic name (only used for printing, defaults to "input")
*/
inline bool
isValid (const PointCloudConstPtr &cloud, const std::string &topic_name = "input")
{
if (cloud->width * cloud->height != cloud->points.size ())
{
NODELET_WARN ("[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->points.size (), cloud->width, cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (false);
}
return (true);
}
/** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
* \param indices the point indices message to test
* \param topic_name an optional topic name (only used for printing, defaults to "indices")
*/
inline bool
isValid (const PointIndicesConstPtr &indices, const std::string &topic_name = "indices")
{
/*if (indices->indices.empty ())
{
NODELET_WARN ("[%s] Empty indices (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (true);
}*/
return (true);
}
/** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
* \param model the model coefficients to test
* \param topic_name an optional topic name (only used for printing, defaults to "model")
*/
inline bool
isValid (const ModelCoefficientsConstPtr &model, const std::string &topic_name = "model")
{
/*if (model->values.empty ())
{
NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (false);
}*/
return (true);
}
/** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
virtual void
onInit ()
{
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
// Parameters that we care about only at startup
pnh_->getParam ("max_queue_size", max_queue_size_);
// ---[ Optional parameters
pnh_->getParam ("use_indices", use_indices_);
pnh_->getParam ("latched_indices", latched_indices_);
pnh_->getParam ("approximate_sync", approximate_sync_);
NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
" - approximate_sync : %s\n"
" - use_indices : %s\n"
" - latched_indices : %s\n"
" - max_queue_size : %d",
getName ().c_str (),
(approximate_sync_) ? "true" : "false",
(use_indices_) ? "true" : "false",
(latched_indices_) ? "true" : "false",
max_queue_size_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_NODELET_H_

4
pcl_ros/include/pcl_ros/publisher.h
View File

@ -42,8 +42,8 @@
**/
#ifndef pcl_ros_PUBLISHER_H_
#define pcl_ros_PUBLISHER_H_
#ifndef PCL_ROS_PUBLISHER_H_
#define PCL_ROS_PUBLISHER_H_
#include <ros/ros.h>
#include "pcl/ros/conversions.h"

12
pcl_ros/package.xml
View File

@ -14,18 +14,28 @@
<url>http://ros.org/wiki/perception_pcl</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>eigen</build_depend>
<build_depend>pcl</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>rosbag</build_depend>
<build_depend>tf</build_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>eigen</run_depend>
<run_depend>pcl</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>rosbag</run_depend>
<run_depend>tf</run_depend>
<export>
<rviz plugin="${prefix}/pcl_nodelets.xml"/>
</export>
</package>

86
pcl_ros/pcl_nodelets.xml Normal file
View File

@ -0,0 +1,86 @@
<!-- PCL IO library component -->
<library path="lib/libpcl_ros_io">
<class name="pcl/NodeletMUX" type="NodeletMUX" base_class_type="nodelet::Nodelet">
<description>
NodeletMUX represent a mux nodelet for PointCloud topics: it takes N (up
to 8) input topics, and publishes all of them on one output topic.
</description>
</class>
<class name="pcl/NodeletDEMUX" type="NodeletDEMUX" base_class_type="nodelet::Nodelet">
<description>
NodeletDEMUX represent a demux nodelet for PointCloud topics: it
publishes 1 input topic to N output topics.
</description>
</class>
<class name="pcl/PCDReader" type="PCDReader" base_class_type="nodelet::Nodelet">
<description>
PCDReader reads in a PCD (Point Cloud Data) v.5 file from disk and converts it to a PointCloud message.
</description>
</class>
<class name="pcl/BAGReader" type="BAGReader" base_class_type="nodelet::Nodelet">
<description>
BAGReader reads in sensor_msgs/PointCloud2 messages from BAG files.
</description>
</class>
<class name="pcl/PCDWriter" type="PCDWriter" base_class_type="nodelet::Nodelet">
<description>
PCDWriter writes a PointCloud message to disk in a PCD (Point Cloud Data) v.5 file format.
</description>
</class>
<class name="pcl/PointCloudConcatenateFieldsSynchronizer" type="PointCloudConcatenateFieldsSynchronizer" base_class_type="nodelet::Nodelet">
<description>
PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of input PointCloud messages on the
same topic, checks their timestamps, and concatenates their fields together into a single PointCloud output message.
</description>
</class>
<class name="pcl/PointCloudConcatenateDataSynchronizer" type="PointCloudConcatenateDataSynchronizer" base_class_type="nodelet::Nodelet">
<description>
PointCloudConcatenateDataSynchronizer is a special form of data
synchronizer: it listens for a set of input PointCloud messages on
different topics, and concatenates them together into a single PointCloud
output message.
</description>
</class>
</library>
<!-- PCL Filters library component -->
<library path="lib/libpcl_ros_filters">
<class name="pcl/PassThrough" type="PassThrough" base_class_type="nodelet::Nodelet">
<description>
PassThrough is a filter that uses the basic Filter class mechanisms for passing data around.
</description>
</class>
<class name="pcl/VoxelGrid" type="VoxelGrid" base_class_type="nodelet::Nodelet">
<description>
VoxelGrid assembles a local 3D grid over a given PointCloud, and uses that to downsample the data.
</description>
</class>
<class name="pcl/ProjectInliers" type="ProjectInliers" base_class_type="nodelet::Nodelet">
<description>
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a separate PointCloud.
</description>
</class>
<class name="pcl/ExtractIndices" type="ExtractIndices" base_class_type="nodelet::Nodelet">
<description>
ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
</description>
</class>
<class name="pcl/StatisticalOutlierRemoval" type="StatisticalOutlierRemoval" base_class_type="nodelet::Nodelet">
<description>
StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data.
</description>
</class>
</library>

0
pcl_ros/src/pcl_ros/__init__.py Normal file
View File

70
pcl_ros/src/pcl_ros/filters/extract_indices.cpp Normal file
View File

@ -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);

77
pcl_ros/src/pcl_ros/filters/features/boundary.cpp Normal file
View File

@ -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);

443
pcl_ros/src/pcl_ros/filters/features/feature.cpp Normal file
View File

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

79
pcl_ros/src/pcl_ros/filters/features/fpfh.cpp Normal file
View File

@ -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);

79
pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp Normal file
View File

@ -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);

77
pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp Normal file
View File

@ -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);

77
pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp Normal file
View File

@ -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);

77
pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp Normal file
View File

@ -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);

81
pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp Normal file
View File

@ -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

79
pcl_ros/src/pcl_ros/filters/features/pfh.cpp Normal file
View File

@ -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);

79
pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp Normal file
View File

@ -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);

79
pcl_ros/src/pcl_ros/filters/features/vfh.cpp Normal file
View File

@ -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);

230
pcl_ros/src/pcl_ros/filters/filter.cpp Normal file
View File

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

120
pcl_ros/src/pcl_ros/filters/passthrough.cpp Normal file
View File

@ -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);

144
pcl_ros/src/pcl_ros/filters/project_inliers.cpp Normal file
View File

@ -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);

43
pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp Normal file
View File

@ -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);

81
pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp Normal file
View File

@ -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);

124
pcl_ros/src/pcl_ros/filters/voxel_grid.cpp Normal file
View File

@ -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);

149
pcl_ros/tools/bag_to_pcd.cpp Normal file
View File

@ -0,0 +1,149 @@
/*
* 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: bag_to_pcd.cpp 35812 2011-02-08 00:05:03Z rusu $
*
*/
/**
\author Radu Bogdan Rusu
@b bag_to_pcd is a simple node that reads in a BAG file and saves all the PointCloud messages to disk in PCD (Point
Cloud Data) format.
**/
#include <sstream>
#include <boost/filesystem.hpp>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include "pcl/io/io.h"
#include "pcl/io/pcd_io.h"
#include "pcl_ros/transforms.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/* ---[ */
int
main (int argc, char** argv)
{
ros::init (argc, argv, "bag_to_pcd");
if (argc < 4)
{
std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory>" << std::endl;
std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds" << std::endl;
return (-1);
}
// TF
tf::TransformListener tf_listener;
tf::TransformBroadcaster tf_broadcaster;
rosbag::Bag bag;
rosbag::View view;
rosbag::View::iterator view_it;
try
{
bag.open (argv[1], rosbag::bagmode::Read);
}
catch (rosbag::BagException)
{
std::cerr << "Error opening file " << argv[1] << std::endl;
return (-1);
}
view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2"));
view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage"));
view_it = view.begin ();
std::string output_dir = std::string (argv[3]);
boost::filesystem::path outpath (output_dir);
if (!boost::filesystem::exists (outpath))
{
if (!boost::filesystem::create_directories (outpath))
{
std::cerr << "Error creating directory " << output_dir << std::endl;
return (-1);
}
std::cerr << "Creating directory " << output_dir << std::endl;
}
// Add the PointCloud2 handler
std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl;
PointCloud cloud_t;
ros::Duration r (0.001);
// Loop over the whole bag file
while (view_it != view.end ())
{
// Handle TF messages first
tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> ();
if (tf != NULL)
{
tf_broadcaster.sendTransform (tf->transforms);
ros::spinOnce ();
r.sleep ();
}
else
{
// Get the PointCloud2 message
PointCloudConstPtr cloud = view_it->instantiate<PointCloud> ();
if (cloud == NULL)
{
++view_it;
continue;
}
// Transform it
pcl_ros::transformPointCloud ("/base_link", *cloud, cloud_t, tf_listener);
std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl;
std::stringstream ss;
ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
std::cerr << "Data saved to " << ss.str () << std::endl;
pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
Eigen::Quaternionf::Identity (), true);
}
// Increment the iterator
++view_it;
}
return (0);
}
/* ]--- */

94
pcl_ros/tools/convert_pcd_to_image.cpp Normal file
View File

@ -0,0 +1,94 @@
/*
* 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: surface_convex_hull.cpp 34612 2010-12-08 01:06:27Z rusu $
*
*/
/**
\author Ethan Rublee
@b convert a pcd to an image file
run with:
rosrun pcl convert_pcd_image cloud_00042.pcd
It will publish a ros image message on /pcd/image
View the image with:
rosrun image_view image_view image:=/pcd/image
**/
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
/* ---[ */
int
main (int argc, char **argv)
{
ros::init (argc, argv, "image_publisher");
ros::NodeHandle nh;
ros::Publisher image_pub = nh.advertise <sensor_msgs::Image> ("output", 1);
if (argc != 2)
{
std::cout << "usage:\n" << argv[0] << " cloud.pcd" << std::endl;
return 1;
}
sensor_msgs::Image image;
sensor_msgs::PointCloud2 cloud;
pcl::io::loadPCDFile (std::string (argv[1]), cloud);
try
{
pcl::toROSMsg (cloud, image); //convert the cloud
}
catch (std::runtime_error e)
{
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
return 1; //fail!
}
ros::Rate loop_rate (5);
while (nh.ok ())
{
image_pub.publish (image);
ros::spinOnce ();
loop_rate.sleep ();
}
return (0);
}
/* ]--- */

97
pcl_ros/tools/convert_pointcloud_to_image.cpp Normal file
View File

@ -0,0 +1,97 @@
/*
* 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: surface_convex_hull.cpp 34612 2010-12-08 01:06:27Z rusu $
*
*/
/**
\author Ethan Rublee
**/
// ROS core
#include <ros/ros.h>
//Image message
#include <sensor_msgs/Image.h>
//pcl::toROSMsg
#include <pcl/io/pcd_io.h>
//stl stuff
#include <string>
class PointCloudToImage
{
public:
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
{
if ((cloud->width * cloud->height) == 0)
return; //return if the cloud is not dense!
try
{
pcl::toROSMsg (*cloud, image_); //convert the cloud
}
catch (std::runtime_error e)
{
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
}
image_pub_.publish (image_); //publish our cloud image
}
PointCloudToImage () : cloud_topic_("input"),image_topic_("output")
{
sub_ = nh_.subscribe (cloud_topic_, 30,
&PointCloudToImage::cloud_cb, this);
image_pub_ = nh_.advertise<sensor_msgs::Image> (image_topic_, 30);
//print some info about the node
std::string r_ct = nh_.resolveName (cloud_topic_);
std::string r_it = nh_.resolveName (image_topic_);
ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct );
ROS_INFO_STREAM("Publishing image on topic " << r_it );
}
private:
ros::NodeHandle nh_;
sensor_msgs::Image image_; //cache the image message
std::string cloud_topic_; //default input
std::string image_topic_; //default output
ros::Subscriber sub_; //cloud subscriber
ros::Publisher image_pub_; //image message publisher
};
int
main (int argc, char **argv)
{
ros::init (argc, argv, "convert_pointcloud_to_image");
PointCloudToImage pci; //this loads up the node
ros::spin (); //where she stops nobody knows
return 0;
}

156
pcl_ros/tools/pcd_to_pointcloud.cpp Normal file
View File

@ -0,0 +1,156 @@
/*
* 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: pcd_to_pointcloud.cpp 33238 2010-03-11 00:46:58Z rusu $
*
*/
/**
\author Radu Bogdan Rusu
@b pcd_to_pointcloud is a simple node that loads PCD (Point Cloud Data) files from disk and publishes them as ROS messages on the network.
**/
// ROS core
#include <ros/ros.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include "pcl_ros/publisher.h"
using namespace std;
class PCDGenerator
{
protected:
string tf_frame_;
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
public:
// ROS messages
sensor_msgs::PointCloud2 cloud_;
string file_name_, cloud_topic_;
double rate_;
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
////////////////////////////////////////////////////////////////////////////////
PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~")
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
cloud_topic_ = "cloud_pcd";
pub_.advertise (nh_, cloud_topic_.c_str (), 1);
private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
}
////////////////////////////////////////////////////////////////////////////////
// Start
int
start ()
{
if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1)
return (-1);
cloud_.header.frame_id = tf_frame_;
return (0);
}
////////////////////////////////////////////////////////////////////////////////
// Spin (!)
bool spin ()
{
int nr_points = cloud_.width * cloud_.height;
string fields_list = pcl::getFieldsList (cloud_);
double interval = rate_ * 1e+6;
while (nh_.ok ())
{
ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ());
cloud_.header.stamp = ros::Time::now ();
if (pub_.getNumSubscribers () > 0)
{
ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ());
pub_.publish (cloud_);
}
else
{
ros::Duration (0.001).sleep ();
continue;
}
usleep (interval);
if (interval == 0) // We only publish once if a 0 seconds interval is given
break;
}
ros::Duration (3.0).sleep ();
return (true);
}
};
/* ---[ */
int
main (int argc, char** argv)
{
if (argc < 3)
{
std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
return (-1);
}
ros::init (argc, argv, "pcd_to_pointcloud");
PCDGenerator c;
c.file_name_ = string (argv[1]);
c.rate_ = atof (argv[2]);
if (c.start () == -1)
{
ROS_ERROR ("Could not load file %s. Exiting.", argv[1]);
return (-1);
}
ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ());
c.spin ();
return (0);
}
/* ]--- */

122
pcl_ros/tools/pointcloud_to_pcd.cpp Normal file
View File

@ -0,0 +1,122 @@
/*
* 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: pointcloud_to_pcd.cpp 33238 2010-03-11 00:46:58Z rusu $
*
*/
// ROS core
#include <ros/ros.h>
// PCL includes
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
using namespace std;
/**
\author Radu Bogdan Rusu
@b pointcloud_to_pcd is a simple node that retrieves a ROS point cloud message and saves it to disk into a PCD (Point
Cloud Data) file format.
**/
class PointCloudToPCD
{
protected:
ros::NodeHandle nh_;
private:
std::string prefix_;
public:
string cloud_topic_;
ros::Subscriber sub_;
////////////////////////////////////////////////////////////////////////////////
// Callback
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
{
if ((cloud->width * cloud->height) == 0)
return;
ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
(int)cloud->width * cloud->height,
cloud->header.frame_id.c_str (),
pcl::getFieldsList (*cloud).c_str ());
std::stringstream ss;
ss << prefix_ << cloud->header.stamp << ".pcd";
ROS_INFO ("Data saved to %s", ss.str ().c_str ());
pcl::io::savePCDFile (ss.str (), *cloud, Eigen::Vector4f::Zero (),
Eigen::Quaternionf::Identity (), false);
}
////////////////////////////////////////////////////////////////////////////////
PointCloudToPCD ()
{
// Check if a prefix parameter is defined for output file names.
ros::NodeHandle priv_nh("~");
if (priv_nh.getParam ("prefix", prefix_))
{
ROS_INFO_STREAM ("PCD file prefix is: " << prefix_);
}
else if (nh_.getParam ("prefix", prefix_))
{
ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: "
<< prefix_);
}
cloud_topic_ = "input";
sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this);
ROS_INFO ("Listening for incoming data on topic %s",
nh_.resolveName (cloud_topic_).c_str ());
}
};
/* ---[ */
int
main (int argc, char** argv)
{
ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
PointCloudToPCD b;
ros::spin ();
return (0);
}
/* ]--- */