From a3701bb3df14ba08df641fd6967264b73e15c060 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 17 Dec 2012 18:09:15 -0800 Subject: [PATCH] migrating nodelets and tools from fuerte release to pcl_ros --- pcl_ros/CMakeLists.txt | 72 ++- pcl_ros/cfg/ExtractIndices.cfg | 14 + pcl_ros/cfg/Filter.cfg | 15 + pcl_ros/cfg/Filter_common.py | 19 + pcl_ros/cfg/Filter_common.pyc | Bin 0 -> 1614 bytes pcl_ros/cfg/StatisticalOutlierRemoval.cfg | 18 + pcl_ros/cfg/VoxelGrid.cfg | 16 + .../include/pcl_ros/filters/extract_indices.h | 94 ++++ pcl_ros/include/pcl_ros/filters/filter.h | 149 ++++++ pcl_ros/include/pcl_ros/filters/passthrough.h | 94 ++++ .../include/pcl_ros/filters/project_inliers.h | 104 ++++ .../pcl_ros/filters/radius_outlier_removal.h | 82 ++++ .../filters/statistical_outlier_removal.h | 102 ++++ pcl_ros/include/pcl_ros/filters/voxel_grid.h | 89 ++++ pcl_ros/include/pcl_ros/io/bag_io.h | 130 +++++ pcl_ros/include/pcl_ros/io/concatenate_data.h | 134 ++++++ .../include/pcl_ros/io/concatenate_fields.h | 100 ++++ pcl_ros/include/pcl_ros/io/pcd_io.h | 132 ++++++ pcl_ros/include/pcl_ros/pcl_nodelet.h | 228 +++++++++ pcl_ros/include/pcl_ros/publisher.h | 4 +- pcl_ros/package.xml | 12 +- pcl_ros/pcl_nodelets.xml | 86 ++++ pcl_ros/src/pcl_ros/__init__.py | 0 .../src/pcl_ros/filters/extract_indices.cpp | 70 +++ .../src/pcl_ros/filters/features/boundary.cpp | 77 +++ .../src/pcl_ros/filters/features/feature.cpp | 443 ++++++++++++++++++ pcl_ros/src/pcl_ros/filters/features/fpfh.cpp | 79 ++++ .../src/pcl_ros/filters/features/fpfh_omp.cpp | 79 ++++ .../filters/features/moment_invariants.cpp | 77 +++ .../pcl_ros/filters/features/normal_3d.cpp | 77 +++ .../filters/features/normal_3d_omp.cpp | 77 +++ .../filters/features/normal_3d_tbb.cpp | 81 ++++ pcl_ros/src/pcl_ros/filters/features/pfh.cpp | 79 ++++ .../filters/features/principal_curvatures.cpp | 79 ++++ pcl_ros/src/pcl_ros/filters/features/vfh.cpp | 79 ++++ pcl_ros/src/pcl_ros/filters/filter.cpp | 230 +++++++++ pcl_ros/src/pcl_ros/filters/passthrough.cpp | 120 +++++ .../src/pcl_ros/filters/project_inliers.cpp | 144 ++++++ .../filters/radius_outlier_removal.cpp | 43 ++ .../filters/statistical_outlier_removal.cpp | 81 ++++ pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 124 +++++ pcl_ros/tools/bag_to_pcd.cpp | 149 ++++++ pcl_ros/tools/convert_pcd_to_image.cpp | 94 ++++ pcl_ros/tools/convert_pointcloud_to_image.cpp | 97 ++++ pcl_ros/tools/pcd_to_pointcloud.cpp | 156 ++++++ pcl_ros/tools/pointcloud_to_pcd.cpp | 122 +++++ 46 files changed, 4331 insertions(+), 20 deletions(-) create mode 100755 pcl_ros/cfg/ExtractIndices.cfg create mode 100755 pcl_ros/cfg/Filter.cfg create mode 100644 pcl_ros/cfg/Filter_common.py create mode 100644 pcl_ros/cfg/Filter_common.pyc create mode 100755 pcl_ros/cfg/StatisticalOutlierRemoval.cfg create mode 100755 pcl_ros/cfg/VoxelGrid.cfg create mode 100644 pcl_ros/include/pcl_ros/filters/extract_indices.h create mode 100644 pcl_ros/include/pcl_ros/filters/filter.h create mode 100644 pcl_ros/include/pcl_ros/filters/passthrough.h create mode 100644 pcl_ros/include/pcl_ros/filters/project_inliers.h create mode 100644 pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h create mode 100644 pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h create mode 100644 pcl_ros/include/pcl_ros/filters/voxel_grid.h create mode 100644 pcl_ros/include/pcl_ros/io/bag_io.h create mode 100644 pcl_ros/include/pcl_ros/io/concatenate_data.h create mode 100644 pcl_ros/include/pcl_ros/io/concatenate_fields.h create mode 100644 pcl_ros/include/pcl_ros/io/pcd_io.h create mode 100644 pcl_ros/include/pcl_ros/pcl_nodelet.h create mode 100644 pcl_ros/pcl_nodelets.xml create mode 100644 pcl_ros/src/pcl_ros/__init__.py create mode 100644 pcl_ros/src/pcl_ros/filters/extract_indices.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/boundary.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/feature.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/fpfh.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/pfh.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/features/vfh.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/filter.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/passthrough.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/project_inliers.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp create mode 100644 pcl_ros/src/pcl_ros/filters/voxel_grid.cpp create mode 100644 pcl_ros/tools/bag_to_pcd.cpp create mode 100644 pcl_ros/tools/convert_pcd_to_image.cpp create mode 100644 pcl_ros/tools/convert_pointcloud_to_image.cpp create mode 100644 pcl_ros/tools/pcd_to_pointcloud.cpp create mode 100644 pcl_ros/tools/pointcloud_to_pcd.cpp diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 3f4aac2e..53275398 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -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} ) diff --git a/pcl_ros/cfg/ExtractIndices.cfg b/pcl_ros/cfg/ExtractIndices.cfg new file mode 100755 index 00000000..b3ed9cde --- /dev/null +++ b/pcl_ros/cfg/ExtractIndices.cfg @@ -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")) diff --git a/pcl_ros/cfg/Filter.cfg b/pcl_ros/cfg/Filter.cfg new file mode 100755 index 00000000..bcda6baf --- /dev/null +++ b/pcl_ros/cfg/Filter.cfg @@ -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")) + diff --git a/pcl_ros/cfg/Filter_common.py b/pcl_ros/cfg/Filter_common.py new file mode 100644 index 00000000..b414c378 --- /dev/null +++ b/pcl_ros/cfg/Filter_common.py @@ -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.", "") + diff --git a/pcl_ros/cfg/Filter_common.pyc b/pcl_ros/cfg/Filter_common.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9ccbd7322de5f0d9972c36eb357be0a1b44e81e8 GIT binary patch literal 1614 zcmcIkOK%i85OzWq(!mxw>XPtSr5J`))fI-P6PWJ&bS%LJRQ> zG7-8bY$Bc|f)cUc6C}iGB9dsB5q3uG>RIsuduQXuoz*!UOJZmdR&@A*-`W zm1Lk4;TFX19?4cOqh2$}ux03kFHK>rDGg_(slwe30CQ6|rBv3sQ3QyN ziR5e5#|VPj&Q*KHuNvYjE9> zGtR1_p<;7L;VCRT#FCw4k2FjnMa;ZD2RRw zmKhufQk$Ycja1xc_!PVh0g}HBWvu+MSX6=1{)te=jbsL)D6Khm6>ya?$F8AWGq8lk zJQf4zET(bCrm}&?ttK8EQniXd(Tea0vvf2}13P~^-9J!eN#3SU9`B^NV%$v{e4f_i zb6mJ_RfYzZc18wPR78IEOghn5uQ + +#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 > 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 impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ + diff --git a/pcl_ros/include/pcl_ros/filters/filter.h b/pcl_ros/include/pcl_ros/filters/filter.h new file mode 100644 index 00000000..24fca0b0 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/filter.h @@ -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 +#include "pcl_ros/pcl_nodelet.h" + +// Dynamic reconfigure +#include +#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 > IndicesPtr; + typedef boost::shared_ptr > IndicesConstPtr; + + Filter () {} + + protected: + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + message_filters::Subscriber 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 > srv_; + + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr > > sync_input_indices_e_; + boost::shared_ptr > > 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_ diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.h b/pcl_ros/include/pcl_ros/filters/passthrough.h new file mode 100644 index 00000000..c21a7d1b --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/passthrough.h @@ -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 +#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 > 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 impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.h b/pcl_ros/include/pcl_ros/filters/project_inliers.h new file mode 100644 index 00000000..200ef10e --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.h @@ -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 +#include "pcl_ros/filters/filter.h" + +#include + +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 sub_model_; + + /** \brief Synchronized input, indices, and model coefficients.*/ + boost::shared_ptr > > sync_input_indices_model_e_; + boost::shared_ptr > > sync_input_indices_model_a_; + /** \brief The PCL filter implementation used. */ + pcl::ProjectInliers 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_ diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h new file mode 100644 index 00000000..fc316a83 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h @@ -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 +#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 impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h new file mode 100644 index 00000000..e97735d5 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h @@ -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 +#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: + *
    + *
  • 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. + *
+ * + * \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 > 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 impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.h b/pcl_ros/include/pcl_ros/filters/voxel_grid.h new file mode 100644 index 00000000..95942259 --- /dev/null +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.h @@ -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 +#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 > srv_; + + /** \brief The PCL filter implementation used. */ + pcl::VoxelGrid 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_ diff --git a/pcl_ros/include/pcl_ros/io/bag_io.h b/pcl_ros/include/pcl_ros/io/bag_io.h new file mode 100644 index 00000000..18f53de2 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/bag_io.h @@ -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 +#include +#include +#include + +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 (); + ++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_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.h b/pcl_ros/include/pcl_ros/io/concatenate_data.h new file mode 100644 index 00000000..7f00c6d5 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/concatenate_data.h @@ -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 +#include +#include +#include +#include +#include +#include + +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 > > 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 nf_; + + /** \brief Synchronizer. + * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer. + */ + boost::shared_ptr > > ts_a_; + boost::shared_ptr > > 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 (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_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.h b/pcl_ros/include/pcl_ros/io/concatenate_fields.h new file mode 100644 index 00000000..f6252b68 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.h @@ -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 +#include +#include +#include +#include + +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 > queue_; + }; +} + +#endif //#ifndef PCL_IO_CONCATENATE_H_ diff --git a/pcl_ros/include/pcl_ros/io/pcd_io.h b/pcl_ros/include/pcl_ros/io/pcd_io.h new file mode 100644 index 00000000..89663451 --- /dev/null +++ b/pcl_ros/include/pcl_ros/io/pcd_io.h @@ -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 +#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_ diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.h new file mode 100644 index 00000000..9fef5ba7 --- /dev/null +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.h @@ -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 +// PCL includes +#include +#include +#include +#include "pcl_ros/point_cloud.h" +// ROS Nodelet includes +#include +#include +#include +#include +#include + +// Include TF +#include + +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 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 > IndicesPtr; + typedef boost::shared_ptr > 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 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 sub_input_filter_; + + /** \brief The message filter subscriber for PointIndices. */ + message_filters::Subscriber 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_ diff --git a/pcl_ros/include/pcl_ros/publisher.h b/pcl_ros/include/pcl_ros/publisher.h index e97d2ea1..7ebc0984 100644 --- a/pcl_ros/include/pcl_ros/publisher.h +++ b/pcl_ros/include/pcl_ros/publisher.h @@ -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 #include "pcl/ros/conversions.h" diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 4f92ec85..86be1cdc 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -14,18 +14,28 @@ http://ros.org/wiki/perception_pcl catkin - + + dynamic_reconfigure eigen pcl roscpp sensor_msgs std_msgs + rosbag tf + dynamic_reconfigure eigen pcl roscpp sensor_msgs std_msgs + rosbag tf + + + + + + diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml new file mode 100644 index 00000000..e6cd0f90 --- /dev/null +++ b/pcl_ros/pcl_nodelets.xml @@ -0,0 +1,86 @@ + + + + + + + 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. + + + + + + NodeletDEMUX represent a demux nodelet for PointCloud topics: it + publishes 1 input topic to N output topics. + + + + + + PCDReader reads in a PCD (Point Cloud Data) v.5 file from disk and converts it to a PointCloud message. + + + + + + BAGReader reads in sensor_msgs/PointCloud2 messages from BAG files. + + + + + + PCDWriter writes a PointCloud message to disk in a PCD (Point Cloud Data) v.5 file format. + + + + + + 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. + + + + + 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. + + + + + + + + + PassThrough is a filter that uses the basic Filter class mechanisms for passing data around. + + + + + + VoxelGrid assembles a local 3D grid over a given PointCloud, and uses that to downsample the data. + + + + + + ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a separate PointCloud. + + + + + + ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. + + + + + + StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. + + + + + diff --git a/pcl_ros/src/pcl_ros/__init__.py b/pcl_ros/src/pcl_ros/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp new file mode 100644 index 00000000..2d596cd9 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -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 +#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 > (nh); + dynamic_reconfigure::Server::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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp new file mode 100644 index 00000000..0e17c5fd --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp @@ -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 +#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); diff --git a/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/pcl_ros/src/pcl_ros/filters/features/feature.cpp new file mode 100644 index 00000000..434707ee --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -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 +// 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 +#include "pcl_ros/features/feature.h" +#include + +//////////////////////////////////////////////////////////////////////////////////////////// +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 >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("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 > (*pnh_); + dynamic_reconfigure::Server::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 > >(max_queue_size_); + else + sync_input_surface_indices_e_ = boost::make_shared > >(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 ("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 (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 >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("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 > (*pnh_); + dynamic_reconfigure::Server::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 > > (max_queue_size_); + else + sync_input_normals_surface_indices_e_ = boost::make_shared > > (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 (indices->indices)); + + computePublish (cloud, cloud_normals, cloud_surface, vindices); +} + diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp new file mode 100644 index 00000000..7782fde9 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp new file mode 100644 index 00000000..51880a9f --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp new file mode 100644 index 00000000..85bd209a --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp new file mode 100644 index 00000000..a512c41f --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp new file mode 100644 index 00000000..e53fe9c7 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp new file mode 100644 index 00000000..d6636f73 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp @@ -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 +#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 + diff --git a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp new file mode 100644 index 00000000..f40e0bab --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp new file mode 100644 index 00000000..d6431160 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp new file mode 100644 index 00000000..f13028e3 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp @@ -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 +#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); + diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp new file mode 100644 index 00000000..9fcf844c --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -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 +#include "pcl_ros/transforms.h" +#include "pcl_ros/filters/filter.h" + +/*//#include +//#include +*/ + +/*//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 ("output", max_queue_size_); + + // Enable the dynamic reconfigure service + if (!has_service) + { + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::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 > >(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 > >(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 ("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 (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 (indices->indices)); + + computePublish (cloud_tf, vindices); +} + diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp new file mode 100644 index 00000000..e4f54f4b --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -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 +#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 > (nh); + dynamic_reconfigure::Server::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); + diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp new file mode 100644 index 00000000..f498ba65 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -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 +#include "pcl_ros/filters/project_inliers.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ProjectInliers::onInit () +{ + // No need to call the super onInit as we are overwriting everything + //Filter::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 ("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 > > (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 > > (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 (indices->indices)); + + model_ = model; + computePublish (cloud, vindices); +} + +typedef pcl_ros::ProjectInliers ProjectInliers; +PLUGINLIB_DECLARE_CLASS (pcl, ProjectInliers, ProjectInliers, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp new file mode 100644 index 00000000..6aaf2fbb --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -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 +#include "pcl_ros/filters/radius_outlier_removal.h" + +typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; +PLUGINLIB_DECLARE_CLASS (pcl, RadiusOutlierRemoval, RadiusOutlierRemoval, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp new file mode 100644 index 00000000..74288f56 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -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 +#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 > (nh); + dynamic_reconfigure::Server::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); + diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp new file mode 100644 index 00000000..55181f75 --- /dev/null +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -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 +#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 > (nh); + dynamic_reconfigure::Server::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); + diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp new file mode 100644 index 00000000..0a753d3a --- /dev/null +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -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 +#include +#include +#include +#include "pcl/io/io.h" +#include "pcl/io/pcd_io.h" +#include "pcl_ros/transforms.h" +#include +#include + +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] << " " << 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 (); + if (tf != NULL) + { + tf_broadcaster.sendTransform (tf->transforms); + ros::spinOnce (); + r.sleep (); + } + else + { + // Get the PointCloud2 message + PointCloudConstPtr cloud = view_it->instantiate (); + 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); +} +/* ]--- */ diff --git a/pcl_ros/tools/convert_pcd_to_image.cpp b/pcl_ros/tools/convert_pcd_to_image.cpp new file mode 100644 index 00000000..2325f513 --- /dev/null +++ b/pcl_ros/tools/convert_pcd_to_image.cpp @@ -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 +#include +#include +#include +#include + +/* ---[ */ +int +main (int argc, char **argv) +{ + ros::init (argc, argv, "image_publisher"); + ros::NodeHandle nh; + ros::Publisher image_pub = nh.advertise ("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); +} + +/* ]--- */ diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp new file mode 100644 index 00000000..ebadd99d --- /dev/null +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -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 +//Image message +#include +//pcl::toROSMsg +#include +//stl stuff +#include + +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 (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; +} diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp new file mode 100644 index 00000000..b707557b --- /dev/null +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -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 +#include +#include +#include + +#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 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] << " [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); +} +/* ]--- */ diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp new file mode 100644 index 00000000..9b52113c --- /dev/null +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -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 +// PCL includes +#include +#include +#include + +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); +} +/* ]--- */