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

migrating nodelets and tools from fuerte release to pcl_ros

Browse Source
This commit is contained in:
Julius Kammerl
2012-12-17 18:09:15 -08:00
committed by Paul Bovbel
parent d5d9e3816a
commit a3701bb3df
46 changed files with 4331 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pcl_ros/include/pcl_ros/filters/extract_indices.h
+94
View File
@@ -0,0 +1,94 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: extract_indices.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_
#define PCL_ROS_FILTERS_EXTRACTINDICES_H_
// PCL includes
#include <pcl/filters/extract_indices.h>
#include "pcl_ros/filters/filter.h"
#include "pcl_ros/ExtractIndicesConfig.h"
namespace pcl_ros
{
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ExtractIndices : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
impl_.setInputCloud (input);
impl_.setIndices (indices);
impl_.filter (output);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback. */
void
config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::ExtractIndices<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_
pcl_ros/include/pcl_ros/filters/filter.h
+149
View File
@@ -0,0 +1,149 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: filter.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTER_H_
#define PCL_ROS_FILTER_H_
// PCL includes
#include <pcl/filters/filter.h>
#include "pcl_ros/pcl_nodelet.h"
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/FilterConfig.h"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters
* are defined here as static methods.
* \author Radu Bogdan Rusu
*/
class Filter : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
Filter () {}
protected:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
message_filters::Subscriber<PointCloud2> sub_input_filter_;
/** \brief The desired user filter field name. */
std::string filter_field_name_;
/** \brief The minimum allowed filter value a point will be considered from. */
double filter_limit_min_;
/** \brief The maximum allowed filter value a point will be considered from. */
double filter_limit_max_;
/** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
bool filter_limit_negative_;
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
std::string tf_input_frame_;
/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
std::string tf_output_frame_;
/** \brief Internal mutex. */
boost::mutex mutex_;
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init (ros::NodeHandle &nh, bool &has_service)
{
has_service = false;
return (true);
}
/** \brief Virtual abstract filter method. To be implemented by every child.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
* \param output the resultant filtered PointCloud2
*/
virtual void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output) = 0;
/** \brief Nodelet initialization routine. */
virtual void
onInit ();
/** \brief Call the child filter () method, optionally transform the result, and publish it.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
*/
void
computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices);
private:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices> > > sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices> > > sync_input_indices_a_;
/** \brief Dynamic reconfigure service callback. */
virtual void
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
/** \brief PointCloud2 + Indices data callback. */
void
input_indices_callback (const PointCloud2::ConstPtr &cloud,
const PointIndicesConstPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTER_H_
pcl_ros/include/pcl_ros/filters/passthrough.h
+94
View File
@@ -0,0 +1,94 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: passthrough.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_
#define PCL_ROS_FILTERS_PASSTHROUGH_H_
// PCL includes
#include <pcl/filters/passthrough.h>
#include "pcl_ros/filters/filter.h"
namespace pcl_ros
{
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
* constraints.
* \author Radu Bogdan Rusu
*/
class PassThrough : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
impl_.setInputCloud (input);
impl_.setIndices (indices);
impl_.filter (output);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback.
* \param config the dynamic reconfigure FilterConfig object
* \param level the dynamic reconfigure level
*/
void
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::PassThrough<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_
pcl_ros/include/pcl_ros/filters/project_inliers.h
+104
View File
@@ -0,0 +1,104 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: project_inliers.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_PROJECT_INLIERS_H_
#define PCL_ROS_FILTERS_PROJECT_INLIERS_H_
// PCL includes
#include <pcl/filters/project_inliers.h>
#include "pcl_ros/filters/filter.h"
#include <message_filters/subscriber.h>
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
* separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ProjectInliers : public Filter
{
public:
ProjectInliers () : model_ () {}
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
impl_.setInputCloud (input);
impl_.setIndices (indices);
impl_.setModelCoefficients (model_);
impl_.filter (output);
}
private:
/** \brief A pointer to the vector of model coefficients. */
ModelCoefficientsConstPtr model_;
/** \brief The message filter subscriber for model coefficients. */
message_filters::Subscriber<ModelCoefficients> sub_model_;
/** \brief Synchronized input, indices, and model coefficients.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_;
/** \brief The PCL filter implementation used. */
pcl::ProjectInliers<PointCloud2> impl_;
/** \brief Nodelet initialization routine. */
virtual void
onInit ();
/** \brief PointCloud2 + Indices + Model data callback. */
void
input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
const PointIndicesConstPtr &indices,
const ModelCoefficientsConstPtr &model);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_
pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h
+82
View File
@@ -0,0 +1,82 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: radius_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_
#define PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_
// PCL includes
#include <pcl/filters/radius_outlier_removal.h>
#include "pcl_ros/filters/filter.h"
namespace pcl_ros
{
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
* search radius is smaller than a given K.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class RadiusOutlierRemoval : public Filter
{
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
impl_.setInputCloud (input);
impl_.setIndices (indices);
impl_.filter (output);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
*/
virtual inline bool child_init (ros::NodeHandle &nh) { return (true); }
private:
/** \brief The PCL filter implementation used. */
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_
pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h
+102
View File
@@ -0,0 +1,102 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: statistical_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_
#define PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_
// PCL includes
#include <pcl/filters/statistical_outlier_removal.h>
#include "pcl_ros/filters/filter.h"
// Dynamic reconfigure
#include "pcl_ros/StatisticalOutlierRemovalConfig.h"
namespace pcl_ros
{
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
* information check:
* <ul>
* <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
* Towards 3D Point Cloud Based Object Maps for Household Environments
* Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
* </ul>
*
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class StatisticalOutlierRemoval : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
impl_.setInputCloud (input);
impl_.setIndices (indices);
impl_.filter (output);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_
pcl_ros/include/pcl_ros/filters/voxel_grid.h
+89
View File
@@ -0,0 +1,89 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: voxel_grid.h 35876 2011-02-09 01:04:36Z rusu $
*
*/
#ifndef PCL_ROS_FILTERS_VOXEL_H_
#define PCL_ROS_FILTERS_VOXEL_H_
// PCL includes
#include <pcl/filters/voxel_grid.h>
#include "pcl_ros/filters/filter.h"
// Dynamic reconfigure
#include "pcl_ros/VoxelGridConfig.h"
namespace pcl_ros
{
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
* \author Radu Bogdan Rusu
*/
class VoxelGrid : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > srv_;
/** \brief The PCL filter implementation used. */
pcl::VoxelGrid<sensor_msgs::PointCloud2> impl_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
virtual void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output);
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void
config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_
pcl_ros/include/pcl_ros/io/bag_io.h
+130
View File
@@ -0,0 +1,130 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: bag_io.h 35471 2011-01-25 06:50:00Z rusu $
*
*/
#ifndef PCL_ROS_IO_BAG_IO_H_
#define PCL_ROS_IO_BAG_IO_H_
#include <pcl_ros/pcl_nodelet.h>
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
namespace pcl_ros
{
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief BAG PointCloud file format reader.
* \author Radu Bogdan Rusu
*/
class BAGReader: public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/** \brief Empty constructor. */
BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {};
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; }
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate () { return (publish_rate_); }
/** \brief Get the next point cloud dataset in the BAG file.
* \return the next point cloud dataset as read from the file
*/
inline PointCloudConstPtr
getNextCloud ()
{
if (it_ != view_.end ())
{
output_ = it_->instantiate<sensor_msgs::PointCloud2> ();
++it_;
}
return (output_);
}
/** \brief Open a BAG file for reading and select a specified topic
* \param file_name the BAG file to open
* \param topic_name the topic that we want to read data from
*/
bool open (const std::string &file_name, const std::string &topic_name);
/** \brief Close an open BAG file. */
inline void
close ()
{
bag_.close ();
}
/** \brief Nodelet initialization routine. */
virtual void onInit ();
private:
/** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
double publish_rate_;
/** \brief The BAG object. */
rosbag::Bag bag_;
/** \brief The BAG view object. */
rosbag::View view_;
/** \brief The BAG view iterator object. */
rosbag::View::iterator it_;
/** \brief The name of the topic that contains the PointCloud data. */
std::string topic_name_;
/** \brief The name of the BAG file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloudPtr output_;
/** \brief Signals that a new PointCloud2 message has been read from the file. */
//bool cloud_received_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_IO_BAG_IO_H_
pcl_ros/include/pcl_ros/io/concatenate_data.h
+134
View File
@@ -0,0 +1,134 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: concatenate_data.h 35231 2011-01-14 05:33:20Z rusu $
*
*/
#ifndef PCL_IO_CONCATENATE_DATA_H_
#define PCL_IO_CONCATENATE_DATA_H_
// ROS includes
#include <tf/transform_listener.h>
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/pass_through.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
* checks their timestamps, and concatenates their fields together into a single
* PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateDataSynchronizer: public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Empty constructor. */
PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {};
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {};
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateDataSynchronizer () {};
void onInit ();
private:
/** \brief ROS local node handle. */
ros::NodeHandle private_nh_;
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
bool approximate_sync_;
/** \brief A vector of message filters. */
std::vector<boost::shared_ptr<message_filters::Subscriber<PointCloud2> > > filters_;
/** \brief Output TF frame the concatenated points should be transformed to. */
std::string output_frame_;
/** \brief TF listener object. */
tf::TransformListener tf_;
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointCloud2> nf_;
/** \brief Synchronizer.
* \note This will most likely be rewritten soon using the DynamicTimeSynchronizer.
*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_e_;
/** \brief Input point cloud callback.
* Because we want to use the same synchronizer object, we push back
* empty elements with the same timestamp.
*/
inline void
input_callback (const PointCloud2ConstPtr &input)
{
PointCloud2 cloud;
cloud.header.stamp = input->header.stamp;
nf_.add (boost::make_shared<PointCloud2> (cloud));
}
/** \brief Input callback for 8 synchronized topics. */
void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8);
void combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out);
};
}
#endif //#ifndef PCL_ROS_IO_CONCATENATE_H_
pcl_ros/include/pcl_ros/io/concatenate_fields.h
+100
View File
@@ -0,0 +1,100 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: concatenate_fields.h 35052 2011-01-03 21:04:57Z rusu $
*
*/
#ifndef PCL_IO_CONCATENATE_FIELDS_H_
#define PCL_IO_CONCATENATE_FIELDS_H_
// ROS includes
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
namespace pcl_ros
{
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
* input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
* a single PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateFieldsSynchronizer: public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/** \brief Empty constructor. */
PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {};
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {};
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateFieldsSynchronizer () {};
void onInit ();
void input_callback (const PointCloudConstPtr &cloud);
private:
/** \brief ROS local node handle. */
ros::NodeHandle private_nh_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The number of input messages that we expect on the input topic. */
int input_messages_;
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief The maximum number of seconds to wait until we drop the synchronization. */
double maximum_seconds_;
/** \brief A queue for messages. */
std::map<ros::Time, std::vector<PointCloudConstPtr> > queue_;
};
}
#endif //#ifndef PCL_IO_CONCATENATE_H_
pcl_ros/include/pcl_ros/io/pcd_io.h
+132
View File
@@ -0,0 +1,132 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: pcd_io.h 35054 2011-01-03 21:16:49Z rusu $
*
*/
#ifndef PCL_ROS_IO_PCD_IO_H_
#define PCL_ROS_IO_PCD_IO_H_
#include <pcl/io/pcd_io.h>
#include "pcl_ros/pcl_nodelet.h"
namespace pcl_ros
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format reader.
* \author Radu Bogdan Rusu
*/
class PCDReader : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Empty constructor. */
PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {};
virtual void onInit ();
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; }
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate () { return (publish_rate_); }
/** \brief Set the TF frame the PointCloud will be published in.
* \param tf_frame the TF frame the PointCloud will be published in
*/
inline void setTFframe (std::string tf_frame) { tf_frame_ = tf_frame; }
/** \brief Get the TF frame the PointCloud is published in. */
inline std::string getTFframe () { return (tf_frame_); }
protected:
/** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
double publish_rate_;
/** \brief The TF frame the data should be published in ("/base_link" by default). */
std::string tf_frame_;
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloud2Ptr output_;
private:
/** \brief The PCL implementation used. */
pcl::PCDReader impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format writer.
* \author Radu Bogdan Rusu
*/
class PCDWriter : public PCLNodelet
{
public:
PCDWriter () : file_name_ (""), binary_mode_ (true) {}
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
virtual void onInit ();
void input_callback (const PointCloud2ConstPtr &cloud);
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
protected:
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief Set to true if the output files should be saved in binary mode (true). */
bool binary_mode_;
private:
/** \brief The PCL implementation used. */
pcl::PCDWriter impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_IO_PCD_IO_H_
pcl_ros/include/pcl_ros/pcl_nodelet.h
+228
View File
@@ -0,0 +1,228 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: pcl_nodelet.h 33238 2010-03-11 00:46:58Z rusu $
*
*/
/**
\author Radu Bogdan Rusu
**/
#ifndef PCL_NODELET_H_
#define PCL_NODELET_H_
#include <sensor_msgs/PointCloud2.h>
// PCL includes
#include <pcl/PointIndices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include "pcl_ros/point_cloud.h"
// ROS Nodelet includes
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
// Include TF
#include <tf/transform_listener.h>
namespace pcl_ros
{
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */
class PCLNodelet : public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
typedef pcl::PointIndices PointIndices;
typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr;
typedef pcl::ModelCoefficients ModelCoefficients;
typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
/** \brief Empty constructor. */
PCLNodelet () : use_indices_ (false), latched_indices_ (false),
max_queue_size_ (3), approximate_sync_ (false) {};
protected:
/** \brief The ROS NodeHandle used for parameters, publish/subscribe, etc. */
boost::shared_ptr<ros::NodeHandle> pnh_;
/** \brief Set to true if point indices are used.
*
* When receiving a point cloud, if use_indices_ is false, the entire
* point cloud is processed for the given operation. If use_indices_ is
* true, then the ~indices topic is read to get the vector of point
* indices specifying the subset of the point cloud that will be used for
* the operation. In the case where use_indices_ is true, the ~input and
* ~indices topics must be synchronised in time, either exact or within a
* specified jitter. See also @ref latched_indices_ and approximate_sync.
**/
bool use_indices_;
/** \brief Set to true if the indices topic is latched.
*
* If use_indices_ is true, the ~input and ~indices topics generally must
* be synchronised in time. By setting this flag to true, the most recent
* value from ~indices can be used instead of requiring a synchronised
* message.
**/
bool latched_indices_;
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_input_filter_;
/** \brief The message filter subscriber for PointIndices. */
message_filters::Subscriber<PointIndices> sub_indices_filter_;
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
/** \brief The maximum queue size (default: 3). */
int max_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
bool approximate_sync_;
/** \brief TF listener object. */
tf::TransformListener tf_listener_;
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
* \param cloud the point cloud to test
* \param topic_name an optional topic name (only used for printing, defaults to "input")
*/
inline bool
isValid (const PointCloud2::ConstPtr &cloud, const std::string &topic_name = "input")
{
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ())
{
NODELET_WARN ("[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->data.size (), cloud->width, cloud->height, cloud->point_step, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (false);
}
return (true);
}
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
* \param cloud the point cloud to test
* \param topic_name an optional topic name (only used for printing, defaults to "input")
*/
inline bool
isValid (const PointCloudConstPtr &cloud, const std::string &topic_name = "input")
{
if (cloud->width * cloud->height != cloud->points.size ())
{
NODELET_WARN ("[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->points.size (), cloud->width, cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (false);
}
return (true);
}
/** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
* \param indices the point indices message to test
* \param topic_name an optional topic name (only used for printing, defaults to "indices")
*/
inline bool
isValid (const PointIndicesConstPtr &indices, const std::string &topic_name = "indices")
{
/*if (indices->indices.empty ())
{
NODELET_WARN ("[%s] Empty indices (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (true);
}*/
return (true);
}
/** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
* \param model the model coefficients to test
* \param topic_name an optional topic name (only used for printing, defaults to "model")
*/
inline bool
isValid (const ModelCoefficientsConstPtr &model, const std::string &topic_name = "model")
{
/*if (model->values.empty ())
{
NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
return (false);
}*/
return (true);
}
/** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
virtual void
onInit ()
{
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
// Parameters that we care about only at startup
pnh_->getParam ("max_queue_size", max_queue_size_);
// ---[ Optional parameters
pnh_->getParam ("use_indices", use_indices_);
pnh_->getParam ("latched_indices", latched_indices_);
pnh_->getParam ("approximate_sync", approximate_sync_);
NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
" - approximate_sync : %s\n"
" - use_indices : %s\n"
" - latched_indices : %s\n"
" - max_queue_size : %d",
getName ().c_str (),
(approximate_sync_) ? "true" : "false",
(use_indices_) ? "true" : "false",
(latched_indices_) ? "true" : "false",
max_queue_size_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_NODELET_H_
pcl_ros/include/pcl_ros/publisher.h
+2 -2
View File
@@ -42,8 +42,8 @@
**/
#ifndef pcl_ros_PUBLISHER_H_
#define pcl_ros_PUBLISHER_H_
#ifndef PCL_ROS_PUBLISHER_H_
#define PCL_ROS_PUBLISHER_H_
#include <ros/ros.h>
#include "pcl/ros/conversions.h"