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

Apply ament_uncrustify --reformat (#297)

Browse Source 
Signed-off-by: Sean Kelly <sean@seankelly.dev>
This commit is contained in:
Sean Kelly 2020-08-06 15:28:29 -04:00 committed by GitHub
parent 0ac6810688
commit 63cee139f1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
88 changed files with 6019 additions and 5318 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
pcl_ros/include/pcl_ros/features/boundary.hpp
View File

@ -45,44 +45,43 @@
namespace pcl_ros namespace pcl_ros
{ {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle /** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle
* criterion. The code makes use of the estimated surface normals at each point in the input dataset. * criterion. The code makes use of the estimated surface normals at each point in the input dataset.
* *
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class BoundaryEstimation: public FeatureFromNormals class BoundaryEstimation : public FeatureFromNormals
{
private:
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
typedef pcl::PointCloud<pcl::Boundary> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::Boundary> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_BOUNDARY_H_ #endif //#ifndef PCL_ROS_BOUNDARY_H_

351
pcl_ros/include/pcl_ros/features/feature.hpp
View File

@ -54,198 +54,209 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
/////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Feature represents the base feature class. Some generic 3D operations that /** \brief @b Feature represents the base feature class. Some generic 3D operations that
* are applicable to all features are defined here as static methods. * are applicable to all features are defined here as static methods.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class Feature : public PCLNodelet
{
public:
typedef pcl::KdTree<pcl::PointXYZ> KdTree;
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
/** \brief Empty constructor. */
Feature()
: /*input_(), indices_(), surface_(), */ tree_(), k_(0), search_radius_(0),
use_surface_(false), spatial_locator_type_(-1)
{}
protected:
/** \brief The input point cloud dataset. */
//PointCloudInConstPtr input_;
/** \brief A pointer to the vector of point indices to use. */
//IndicesConstPtr indices_;
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
//PointCloudInConstPtr surface_;
/** \brief A pointer to the spatial search object. */
KdTreePtr tree_;
/** \brief The number of K nearest neighbors to use for each point. */
int k_;
/** \brief The nearest neighbors search radius for each point. */
double search_radius_;
// ROS nodelet attributes
/** \brief The surface PointCloud subscriber filter. */
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */
bool use_surface_;
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
* 2: Organized spatial dataset index
*/ */
class Feature : public PCLNodelet int spatial_locator_type_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<FeatureConfig>> srv_;
/** \brief Child initialization routine. Internal method. */
virtual bool childInit(ros::NodeHandle & nh) = 0;
/** \brief Publish an empty point cloud of the feature output type. */
virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
/** \brief Compute the feature and publish it. Internal method. */
virtual void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices) = 0;
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(FeatureConfig & config, uint32_t level);
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointIndices> nf_pi_;
message_filters::PassThrough<PointCloudIn> nf_pc_;
/** \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 PointCloudInConstPtr & input)
{ {
public: PointIndices indices;
typedef pcl::KdTree<pcl::PointXYZ> KdTree; indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr; PointCloudIn cloud;
cloud.header.stamp = input->header.stamp;
nf_pc_.add(ros_ptr(cloud.makeShared()));
nf_pi_.add(boost::make_shared<PointIndices>(indices));
}
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn; private:
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr; /** \brief Synchronized input, surface, and point indices.*/
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr; boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudIn, PointIndices>>> sync_input_surface_indices_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudIn, PointIndices>>> sync_input_surface_indices_e_;
typedef pcl::IndicesPtr IndicesPtr; /** \brief Nodelet initialization routine. */
typedef pcl::IndicesConstPtr IndicesConstPtr; virtual void onInit();
/** \brief Empty constructor. */ /** \brief NodeletLazy connection routine. */
Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0), virtual void subscribe();
use_surface_(false), spatial_locator_type_(-1) virtual void unsubscribe();
{};
protected: /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
/** \brief The input point cloud dataset. */ * \param cloud the pointer to the input point cloud
//PointCloudInConstPtr input_; * \param cloud_surface the pointer to the surface point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_surface_indices_callback(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & cloud_surface,
const PointIndicesConstPtr & indices);
/** \brief A pointer to the vector of point indices to use. */ public:
//IndicesConstPtr indices_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ //////////////////////////////////////////////////////////////////////////////////////////
//PointCloudInConstPtr surface_; //////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
class FeatureFromNormals : public Feature
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
/** \brief A pointer to the spatial search object. */ typedef pcl::PointCloud<pcl::Normal> PointCloudN;
KdTreePtr tree_; typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
/** \brief The number of K nearest neighbors to use for each point. */ FeatureFromNormals()
int k_; : normals_() {}
/** \brief The nearest neighbors search radius for each point. */ protected:
double search_radius_; /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
PointCloudNConstPtr normals_;
// ROS nodelet attributes /** \brief Child initialization routine. Internal method. */
/** \brief The surface PointCloud subscriber filter. */ virtual bool childInit(ros::NodeHandle & nh) = 0;
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
/** \brief The input PointCloud subscriber. */ /** \brief Publish an empty point cloud of the feature output type. */
ros::Subscriber sub_input_; virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */ /** \brief Compute the feature and publish it. */
bool use_surface_; virtual void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices) = 0;
/** \brief Parameter for the spatial locator tree. By convention, the values represent: private:
* 0: ANN (Approximate Nearest Neigbor library) kd-tree /** \brief The normals PointCloud subscriber filter. */
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree message_filters::Subscriber<PointCloudN> sub_normals_filter_;
* 2: Organized spatial dataset index
*/
int spatial_locator_type_;
/** \brief Pointer to a dynamic reconfigure service. */ /** \brief Synchronized input, normals, surface, and point indices.*/
boost::shared_ptr <dynamic_reconfigure::Server<FeatureConfig> > srv_; boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_e_;
/** \brief Child initialization routine. Internal method. */ /** \brief Internal method. */
virtual bool childInit (ros::NodeHandle &nh) = 0; void computePublish(
const PointCloudInConstPtr &,
const PointCloudInConstPtr &,
const IndicesPtr &) {} // This should never be called
/** \brief Publish an empty point cloud of the feature output type. */ /** \brief Nodelet initialization routine. */
virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0; virtual void onInit();
/** \brief Compute the feature and publish it. Internal method. */ /** \brief NodeletLazy connection routine. */
virtual void computePublish (const PointCloudInConstPtr &cloud, virtual void subscribe();
const PointCloudInConstPtr &surface, virtual void unsubscribe();
const IndicesPtr &indices) = 0;
/** \brief Dynamic reconfigure callback /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
* \param config the config object * \param cloud the pointer to the input point cloud
* \param level the dynamic reconfigure level * \param cloud_normals the pointer to the input point cloud normals
*/ * \param cloud_surface the pointer to the surface point cloud
void config_callback (FeatureConfig &config, uint32_t level); * \param indices the pointer to the input point cloud indices
*/
void input_normals_surface_indices_callback(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & cloud_normals,
const PointCloudInConstPtr & cloud_surface,
const PointIndicesConstPtr & indices);
/** \brief Null passthrough filter, used for pushing empty elements in the public:
* synchronizer */ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
message_filters::PassThrough<PointIndices> nf_pi_; };
message_filters::PassThrough<PointCloudIn> nf_pc_;
/** \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 PointCloudInConstPtr &input)
{
PointIndices indices;
indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
PointCloudIn cloud;
cloud.header.stamp = input->header.stamp;
nf_pc_.add (ros_ptr(cloud.makeShared ()));
nf_pi_.add (boost::make_shared<PointIndices> (indices));
}
private:
/** \brief Synchronized input, surface, and point indices.*/
boost::shared_ptr <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_a_;
boost::shared_ptr <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_e_;
/** \brief Nodelet initialization routine. */
virtual void onInit ();
/** \brief NodeletLazy connection routine. */
virtual void subscribe ();
virtual void unsubscribe ();
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
* \param cloud the pointer to the input point cloud
* \param cloud_surface the pointer to the surface point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_surface_indices_callback (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &cloud_surface,
const PointIndicesConstPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
class FeatureFromNormals : public Feature
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
FeatureFromNormals () : normals_() {};
protected:
/** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
PointCloudNConstPtr normals_;
/** \brief Child initialization routine. Internal method. */
virtual bool childInit (ros::NodeHandle &nh) = 0;
/** \brief Publish an empty point cloud of the feature output type. */
virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
/** \brief Compute the feature and publish it. */
virtual void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices) = 0;
private:
/** \brief The normals PointCloud subscriber filter. */
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
/** \brief Synchronized input, normals, surface, and point indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_e_;
/** \brief Internal method. */
void computePublish (const PointCloudInConstPtr &,
const PointCloudInConstPtr &,
const IndicesPtr &) {} // This should never be called
/** \brief Nodelet initialization routine. */
virtual void onInit ();
/** \brief NodeletLazy connection routine. */
virtual void subscribe ();
virtual void unsubscribe ();
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
* \param cloud the pointer to the input point cloud
* \param cloud_normals the pointer to the input point cloud normals
* \param cloud_surface the pointer to the surface point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &cloud_normals,
const PointCloudInConstPtr &cloud_surface,
const PointIndicesConstPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_FEATURE_H_ #endif //#ifndef PCL_ROS_FEATURE_H_

95
pcl_ros/include/pcl_ros/features/fpfh.hpp
View File

@ -43,58 +43,57 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud /** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud
* dataset containing points and normals. * dataset containing points and normals.
* *
* @note If you use this code in any academic work, please cite: * @note If you use this code in any academic work, please cite:
* *
* <ul> * <ul>
* <li> R.B. Rusu, N. Blodow, M. Beetz. * <li> R.B. Rusu, N. Blodow, M. Beetz.
* Fast Point Feature Histograms (FPFH) for 3D Registration. * Fast Point Feature Histograms (FPFH) for 3D Registration.
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
* Kobe, Japan, May 12-17 2009. * Kobe, Japan, May 12-17 2009.
* </li> * </li>
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. * <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
* Fast Geometric Point Labeling using Conditional Random Fields. * Fast Geometric Point Labeling using Conditional Random Fields.
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
* St. Louis, MO, USA, October 11-15 2009. * St. Louis, MO, USA, October 11-15 2009.
* </li> * </li>
* </ul> * </ul>
* *
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class FPFHEstimation : public FeatureFromNormals class FPFHEstimation : public FeatureFromNormals
{
private:
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_FPFH_H_ #endif //#ifndef PCL_FPFH_H_

88
pcl_ros/include/pcl_ros/features/fpfh_omp.hpp
View File

@ -43,54 +43,54 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud /** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
* dataset containing points and normals, in parallel, using the OpenMP standard. * dataset containing points and normals, in parallel, using the OpenMP standard.
* *
* @note If you use this code in any academic work, please cite: * @note If you use this code in any academic work, please cite:
* *
* <ul> * <ul>
* <li> R.B. Rusu, N. Blodow, M. Beetz. * <li> R.B. Rusu, N. Blodow, M. Beetz.
* Fast Point Feature Histograms (FPFH) for 3D Registration. * Fast Point Feature Histograms (FPFH) for 3D Registration.
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
* Kobe, Japan, May 12-17 2009. * Kobe, Japan, May 12-17 2009.
* </li> * </li>
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. * <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
* Fast Geometric Point Labeling using Conditional Random Fields. * Fast Geometric Point Labeling using Conditional Random Fields.
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
* St. Louis, MO, USA, October 11-15 2009. * St. Louis, MO, USA, October 11-15 2009.
* </li> * </li>
* </ul> * </ul>
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class FPFHEstimationOMP : public FeatureFromNormals class FPFHEstimationOMP : public FeatureFromNormals
{
private:
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_FPFH_OMP_H_ #endif //#ifndef PCL_ROS_FPFH_OMP_H_

61
pcl_ros/include/pcl_ros/features/moment_invariants.hpp
View File

@ -43,41 +43,40 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. /** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
* *
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class MomentInvariantsEstimation: public Feature class MomentInvariantsEstimation : public Feature
{
private:
pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudInConstPtr & surface,
// Create the output publisher const IndicesPtr & indices);
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ #endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_

68
pcl_ros/include/pcl_ros/features/normal_3d.hpp
View File

@ -43,44 +43,44 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and /** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
* curvatures. * curvatures.
* *
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations. * \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class NormalEstimation: public Feature class NormalEstimation : public Feature
{
private:
/** \brief PCL implementation object. */
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
/** \brief PCL implementation object. */ pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_; return true;
}
typedef pcl::PointCloud<pcl::Normal> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type.
* \param cloud the input point cloud to copy the header from.
*/
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudInConstPtr & surface,
// Create the output publisher const IndicesPtr & indices);
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. public:
* \param cloud the input point cloud to copy the header from. EIGEN_MAKE_ALIGNED_OPERATOR_NEW
*/ };
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_NORMAL_3D_H_ #endif //#ifndef PCL_ROS_NORMAL_3D_H_

55
pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp
View File

@ -43,37 +43,38 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and /** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
* curvatures, in parallel, using the OpenMP standard. * curvatures, in parallel, using the OpenMP standard.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class NormalEstimationOMP: public Feature class NormalEstimationOMP : public Feature
{
private:
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::Normal> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudInConstPtr & surface,
// Create the output publisher const IndicesPtr & indices);
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_ #endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_

57
pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp
View File

@ -46,41 +46,40 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and /** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and
* curvatures, in parallel, using Intel's Threading Building Blocks library. * curvatures, in parallel, using Intel's Threading Building Blocks library.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class NormalEstimationTBB: public Feature class NormalEstimationTBB : public Feature
{
private:
pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_; pub_output_ = advertise<PointCloud>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::Normal> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudInConstPtr & surface,
// Create the output publisher const IndicesPtr & indices);
pub_output_ = advertise<PointCloud> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
//#endif // HAVE_TBB //#endif // HAVE_TBB
#endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_ #endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_

95
pcl_ros/include/pcl_ros/features/pfh.hpp
View File

@ -43,58 +43,57 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset /** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
* containing points and normals. * containing points and normals.
* *
* @note If you use this code in any academic work, please cite: * @note If you use this code in any academic work, please cite:
* *
* <ul> * <ul>
* <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz. * <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
* Aligning Point Cloud Views using Persistent Feature Histograms. * Aligning Point Cloud Views using Persistent Feature Histograms.
* In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), * In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
* Nice, France, September 22-26 2008. * Nice, France, September 22-26 2008.
* </li> * </li>
* <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz. * <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
* Learning Informative Point Classes for the Acquisition of Object Model Maps. * Learning Informative Point Classes for the Acquisition of Object Model Maps.
* In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV), * In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV),
* Hanoi, Vietnam, December 17-20 2008. * Hanoi, Vietnam, December 17-20 2008.
* </li> * </li>
* </ul> * </ul>
* *
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class PFHEstimation : public FeatureFromNormals class PFHEstimation : public FeatureFromNormals
{
private:
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_PFH_H_ #endif //#ifndef PCL_ROS_PFH_H_

63
pcl_ros/include/pcl_ros/features/principal_curvatures.hpp
View File

@ -44,41 +44,42 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of /** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
* principal surface curvatures for a given point cloud dataset containing points and normals. * principal surface curvatures for a given point cloud dataset containing points and normals.
* *
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations. * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu, Jared Glover * \author Radu Bogdan Rusu, Jared Glover
*/ */
class PrincipalCurvaturesEstimation : public FeatureFromNormals class PrincipalCurvaturesEstimation : public FeatureFromNormals
{
private:
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ #endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_

57
pcl_ros/include/pcl_ros/features/shot.hpp
View File

@ -42,39 +42,38 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b SHOTEstimation estimates SHOT descriptor. /** \brief @b SHOTEstimation estimates SHOT descriptor.
* *
*/ */
class SHOTEstimation : public FeatureFromNormals class SHOTEstimation : public FeatureFromNormals
{
private:
pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_SHOT_H_ #endif //#ifndef PCL_SHOT_H_

54
pcl_ros/include/pcl_ros/features/shot_omp.hpp
View File

@ -42,37 +42,37 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP. /** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP.
*/ */
class SHOTEstimationOMP : public FeatureFromNormals class SHOTEstimationOMP : public FeatureFromNormals
{
private:
pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_SHOT_OMP_H_ #endif //#ifndef PCL_ROS_SHOT_OMP_H_

63
pcl_ros/include/pcl_ros/features/vfh.hpp
View File

@ -43,41 +43,42 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud /** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
* dataset containing points and normals. * dataset containing points and normals.
* *
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class VFHEstimation : public FeatureFromNormals class VFHEstimation : public FeatureFromNormals
{
private:
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut;
/** \brief Child initialization routine. Internal method. */
inline bool
childInit(ros::NodeHandle & nh)
{ {
private: // Create the output publisher
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_; pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
return true;
}
typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut; /** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */ /** \brief Compute the feature and publish it. */
inline bool void computePublish(
childInit (ros::NodeHandle &nh) const PointCloudInConstPtr & cloud,
{ const PointCloudNConstPtr & normals,
// Create the output publisher const PointCloudInConstPtr & surface,
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_); const IndicesPtr & indices);
return (true);
}
/** \brief Publish an empty point cloud of the feature output type. */ public:
void emptyPublish (const PointCloudInConstPtr &cloud); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_FEATURES_VFH_H_ #endif //#ifndef PCL_ROS_FEATURES_VFH_H_

92
pcl_ros/include/pcl_ros/filters/crop_box.hpp
View File

@ -48,57 +48,59 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box. /** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box.
* *
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
* \author Justin Rosen * \author Justin Rosen
* \author Marti Morta Garriga * \author Marti Morta Garriga
*/
class CropBox : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>> srv_; // TODO
/** \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
*/ */
class CropBox : public Filter inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{ {
protected: boost::mutex::scoped_lock lock(mutex_);
/** \brief Pointer to a dynamic reconfigure service. */ pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::CropBoxConfig> > srv_; // TODO pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Call the actual filter. /** \brief Child initialization routine.
* \param input the input point cloud dataset * \param nh ROS node handle
* \param indices the input set of indices to use from \a input * \param has_service set to true if the child has a Dynamic Reconfigure service
* \param output the resultant filtered dataset */
*/ bool
inline void child_init(ros::NodeHandle & nh, bool & has_service);
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Child initialization routine. /** \brief Dynamic reconfigure service callback.
* \param nh ROS node handle * \param config the dynamic reconfigure CropBoxConfig object
* \param has_service set to true if the child has a Dynamic Reconfigure service * \param level the dynamic reconfigure level
*/ */
bool void
child_init (ros::NodeHandle &nh, bool &has_service); config_callback(pcl_ros::CropBoxConfig & config, uint32_t level);
/** \brief Dynamic reconfigure service callback. private:
* \param config the dynamic reconfigure CropBoxConfig object /** \brief The PCL filter implementation used. */
* \param level the dynamic reconfigure level pcl::CropBox<pcl::PCLPointCloud2> impl_;
*/
void
config_callback (pcl_ros::CropBoxConfig &config, uint32_t level);
private: public:
/** \brief The PCL filter implementation used. */ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
pcl::CropBox<pcl::PCLPointCloud2> impl_; };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_ #endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_

83
pcl_ros/include/pcl_ros/filters/extract_indices.hpp
View File

@ -46,53 +46,54 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. /** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu * \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
*/ */
class ExtractIndices : public Filter inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{ {
protected: boost::mutex::scoped_lock lock(mutex_);
/** \brief Pointer to a dynamic reconfigure service. */ pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > srv_; pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Call the actual filter. /** \brief Child initialization routine.
* \param input the input point cloud dataset * \param nh ROS node handle
* \param indices the input set of indices to use from \a input * \param has_service set to true if the child has a Dynamic Reconfigure service
* \param output the resultant filtered dataset */
*/ virtual bool
inline void child_init(ros::NodeHandle & nh, bool & has_service);
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Child initialization routine. /** \brief Dynamic reconfigure service callback. */
* \param nh ROS node handle void
* \param has_service set to true if the child has a Dynamic Reconfigure service config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level);
*/
virtual bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback. */ private:
void /** \brief The PCL filter implementation used. */
config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level); pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
private: public:
/** \brief The PCL filter implementation used. */ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_; };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ #endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_

187
pcl_ros/include/pcl_ros/filters/filter.hpp
View File

@ -48,110 +48,115 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; 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 /** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters
* are defined here as static methods. * are defined here as static methods.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class Filter : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr 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
*/ */
class Filter : public PCLNodelet virtual bool
child_init(ros::NodeHandle & nh, bool & has_service)
{ {
public: has_service = false;
typedef sensor_msgs::PointCloud2 PointCloud2; return true;
}
typedef pcl::IndicesPtr IndicesPtr; /** \brief Virtual abstract filter method. To be implemented by every child.
typedef pcl::IndicesConstPtr IndicesConstPtr; * \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;
Filter () {} /** \brief Lazy transport subscribe routine. */
virtual void
subscribe();
protected: /** \brief Lazy transport unsubscribe routine. */
/** \brief The input PointCloud subscriber. */ virtual void
ros::Subscriber sub_input_; unsubscribe();
message_filters::Subscriber<PointCloud2> sub_input_filter_; /** \brief Nodelet initialization routine. */
virtual void
onInit();
/** \brief The desired user filter field name. */ /** \brief Call the child filter () method, optionally transform the result, and publish it.
std::string filter_field_name_; * \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);
/** \brief The minimum allowed filter value a point will be considered from. */ private:
double filter_limit_min_; /** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
/** \brief The maximum allowed filter value a point will be considered from. */ /** \brief Synchronized input, and indices.*/
double filter_limit_max_; 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 Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ /** \brief Dynamic reconfigure service callback. */
bool filter_limit_negative_; virtual void
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ /** \brief PointCloud2 + Indices data callback. */
std::string tf_input_frame_; void
input_indices_callback(
const PointCloud2::ConstPtr & cloud,
const PointIndicesConstPtr & indices);
/** \brief The original data input TF frame. */ public:
std::string tf_input_orig_frame_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \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 Lazy transport subscribe routine. */
virtual void
subscribe();
/** \brief Lazy transport unsubscribe routine. */
virtual void
unsubscribe();
/** \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_ #endif //#ifndef PCL_ROS_FILTER_H_

88
pcl_ros/include/pcl_ros/filters/passthrough.hpp
View File

@ -44,55 +44,57 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given /** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
* constraints. * constraints.
* \author Radu Bogdan Rusu * \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
*/ */
class PassThrough : public Filter inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{ {
protected: boost::mutex::scoped_lock lock(mutex_);
/** \brief Pointer to a dynamic reconfigure service. */ pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_; pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Call the actual filter. /** \brief Child initialization routine.
* \param input the input point cloud dataset * \param nh ROS node handle
* \param indices the input set of indices to use from \a input * \param has_service set to true if the child has a Dynamic Reconfigure service
* \param output the resultant filtered dataset */
*/ bool
inline void child_init(ros::NodeHandle & nh, bool & has_service);
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Child initialization routine. /** \brief Dynamic reconfigure service callback.
* \param nh ROS node handle * \param config the dynamic reconfigure FilterConfig object
* \param has_service set to true if the child has a Dynamic Reconfigure service * \param level the dynamic reconfigure level
*/ */
bool void
child_init (ros::NodeHandle &nh, bool &has_service); config_callback(pcl_ros::FilterConfig & config, uint32_t level);
/** \brief Dynamic reconfigure service callback. private:
* \param config the dynamic reconfigure FilterConfig object /** \brief The PCL filter implementation used. */
* \param level the dynamic reconfigure level pcl::PassThrough<pcl::PCLPointCloud2> impl_;
*/
void
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
private: public:
/** \brief The PCL filter implementation used. */ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
pcl::PassThrough<pcl::PCLPointCloud2> impl_; };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ #endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_

112
pcl_ros/include/pcl_ros/filters/project_inliers.hpp
View File

@ -46,69 +46,75 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; 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 /** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
* separate PointCloud. * separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu * \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
*/ */
class ProjectInliers : public Filter inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{ {
public: pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
ProjectInliers () : model_ () {} pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients);
pcl_conversions::toPCL(*(model_), *(pcl_model));
impl_.setModelCoefficients(pcl_model);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
protected: private:
/** \brief Call the actual filter. /** \brief A pointer to the vector of model coefficients. */
* \param input the input point cloud dataset ModelCoefficientsConstPtr model_;
* \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)
{
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients);
pcl_conversions::toPCL(*(model_), *(pcl_model));
impl_.setModelCoefficients (pcl_model);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
private: /** \brief The message filter subscriber for model coefficients. */
/** \brief A pointer to the vector of model coefficients. */ message_filters::Subscriber<ModelCoefficients> sub_model_;
ModelCoefficientsConstPtr model_;
/** \brief The message filter subscriber for model coefficients. */ /** \brief Synchronized input, indices, and model coefficients.*/
message_filters::Subscriber<ModelCoefficients> sub_model_; 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<pcl::PCLPointCloud2> impl_;
/** \brief Synchronized input, indices, and model coefficients.*/ /** \brief Nodelet initialization routine. */
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_; virtual void
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_; onInit();
/** \brief The PCL filter implementation used. */
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
/** \brief Nodelet initialization routine. */ /** \brief NodeletLazy connection routine. */
virtual void void subscribe();
onInit (); void unsubscribe();
/** \brief NodeletLazy connection routine. */ /** \brief PointCloud2 + Indices + Model data callback. */
void subscribe (); void
void unsubscribe (); input_indices_model_callback(
const PointCloud2::ConstPtr & cloud,
const PointIndicesConstPtr & indices,
const ModelCoefficientsConstPtr & model);
/** \brief PointCloud2 + Indices + Model data callback. */ public:
void EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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_ #endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_

85
pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp
View File

@ -47,54 +47,55 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain /** \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. * search radius is smaller than a given K.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class RadiusOutlierRemoval : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>> 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
*/ */
class RadiusOutlierRemoval : public Filter inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{ {
protected: pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
/** \brief Pointer to a dynamic reconfigure service. */ pcl_conversions::toPCL(*(input), *(pcl_input));
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig> > srv_; impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Call the actual filter. /** \brief Child initialization routine.
* \param input the input point cloud dataset * \param nh ROS node handle
* \param indices the input set of indices to use from \a input * \param has_service set to true if the child has a Dynamic Reconfigure service
* \param output the resultant filtered dataset */
*/ virtual inline bool child_init(ros::NodeHandle & nh, bool & has_service);
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Child initialization routine. /** \brief Dynamic reconfigure callback
* \param nh ROS node handle * \param config the config object
* \param has_service set to true if the child has a Dynamic Reconfigure service * \param level the dynamic reconfigure level
*/ */
virtual inline bool child_init (ros::NodeHandle &nh, bool &has_service); void config_callback(pcl_ros::RadiusOutlierRemovalConfig & config, uint32_t level);
/** \brief Dynamic reconfigure callback private:
* \param config the config object /** \brief The PCL filter implementation used. */
* \param level the dynamic reconfigure level pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
*/
void config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level);
public:
private: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** \brief The PCL filter implementation used. */ };
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ #endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_

98
pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp
View File

@ -47,60 +47,62 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
* information check: * information check:
* <ul> * <ul>
* <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. * <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
* Towards 3D Point Cloud Based Object Maps for Household Environments * Towards 3D Point Cloud Based Object Maps for Household Environments
* Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
* </ul> * </ul>
* *
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu * \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
*/ */
class StatisticalOutlierRemoval : public Filter inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{ {
protected: boost::mutex::scoped_lock lock(mutex_);
/** \brief Pointer to a dynamic reconfigure service. */ pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > srv_; pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Call the actual filter. /** \brief Child initialization routine.
* \param input the input point cloud dataset * \param nh ROS node handle
* \param indices the input set of indices to use from \a input * \param has_service set to true if the child has a Dynamic Reconfigure service
* \param output the resultant filtered dataset */
*/ bool child_init(ros::NodeHandle & nh, bool & has_service);
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Child initialization routine. /** \brief Dynamic reconfigure callback
* \param nh ROS node handle * \param config the config object
* \param has_service set to true if the child has a Dynamic Reconfigure service * \param level the dynamic reconfigure level
*/ */
bool child_init (ros::NodeHandle &nh, bool &has_service); void config_callback(pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level);
/** \brief Dynamic reconfigure callback private:
* \param config the config object /** \brief The PCL filter implementation used. */
* \param level the dynamic reconfigure level pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
*/
void config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level);
private: public:
/** \brief The PCL filter implementation used. */ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_; };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ #endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_

68
pcl_ros/include/pcl_ros/filters/voxel_grid.hpp
View File

@ -47,43 +47,45 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
* \author Radu Bogdan Rusu * \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<pcl::PCLPointCloud2> 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
*/ */
class VoxelGrid : public Filter virtual void
{ filter(
protected: const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
/** \brief Pointer to a dynamic reconfigure service. */ PointCloud2 & output);
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > srv_;
/** \brief The PCL filter implementation used. */ /** \brief Child initialization routine.
pcl::VoxelGrid<pcl::PCLPointCloud2> impl_; * \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 Call the actual filter. /** \brief Dynamic reconfigure callback
* \param input the input point cloud dataset * \param config the config object
* \param indices the input set of indices to use from \a input * \param level the dynamic reconfigure level
* \param output the resultant filtered dataset */
*/ void
virtual void config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level);
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output);
/** \brief Child initialization routine. public:
* \param nh ROS node handle EIGEN_MAKE_ALIGNED_OPERATOR_NEW
* \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_ #endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_

210
pcl_ros/include/pcl_ros/impl/transforms.hpp
View File

@ -46,178 +46,176 @@ using pcl_conversions::toPCL;
namespace pcl_ros namespace pcl_ros
{ {
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void template<typename PointT>
transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in, void
pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform) transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform)
{ {
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
// of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
// this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
// the conversion of the point cloud anyway. Idem for the origin. // the conversion of the point cloud anyway. Idem for the origin.
tf::Quaternion q = transform.getRotation (); tf::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin (); tf::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin (v.x (), v.y (), v.z ()); Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v); // Eigen::Translation3f translation(v);
// Assemble an Eigen Transform // Assemble an Eigen Transform
//Eigen::Transform3f t; //Eigen::Transform3f t;
//t = translation * rotation; //t = translation * rotation;
transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation); transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void template<typename PointT>
transformPointCloud (const pcl::PointCloud <PointT> &cloud_in, void
pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform) transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform)
{ {
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
// of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
// this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
// the conversion of the point cloud anyway. Idem for the origin. // the conversion of the point cloud anyway. Idem for the origin.
tf::Quaternion q = transform.getRotation (); tf::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w) Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin (); tf::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin (v.x (), v.y (), v.z ()); Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v); // Eigen::Translation3f translation(v);
// Assemble an Eigen Transform // Assemble an Eigen Transform
//Eigen::Transform3f t; //Eigen::Transform3f t;
//t = translation * rotation; //t = translation * rotation;
transformPointCloud (cloud_in, cloud_out, origin, rotation); transformPointCloud(cloud_in, cloud_out, origin, rotation);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool template<typename PointT>
transformPointCloudWithNormals (const std::string &target_frame, bool
const pcl::PointCloud <PointT> &cloud_in, transformPointCloudWithNormals(
pcl::PointCloud <PointT> &cloud_out, const std::string & target_frame,
const tf::TransformListener &tf_listener) const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener)
{ {
if (cloud_in.header.frame_id == target_frame) if (cloud_in.header.frame_id == target_frame) {
{
cloud_out = cloud_in; cloud_out = cloud_in;
return (true); return true;
} }
tf::StampedTransform transform; tf::StampedTransform transform;
try try {
{ tf_listener.lookupTransform(
tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); target_frame, cloud_in.header.frame_id, fromPCL(
} cloud_in.header).stamp, transform);
catch (tf::LookupException &e) } catch (tf::LookupException & e) {
{ ROS_ERROR("%s", e.what());
ROS_ERROR ("%s", e.what ()); return false;
return (false); } catch (tf::ExtrapolationException & e) {
} ROS_ERROR("%s", e.what());
catch (tf::ExtrapolationException &e) return false;
{
ROS_ERROR ("%s", e.what ());
return (false);
} }
transformPointCloudWithNormals (cloud_in, cloud_out, transform); transformPointCloudWithNormals(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool template<typename PointT>
transformPointCloud (const std::string &target_frame, bool
const pcl::PointCloud <PointT> &cloud_in, transformPointCloud(
pcl::PointCloud <PointT> &cloud_out, const std::string & target_frame,
const tf::TransformListener &tf_listener) const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener)
{ {
if (cloud_in.header.frame_id == target_frame) if (cloud_in.header.frame_id == target_frame) {
{
cloud_out = cloud_in; cloud_out = cloud_in;
return (true); return true;
} }
tf::StampedTransform transform; tf::StampedTransform transform;
try try {
{ tf_listener.lookupTransform(
tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform); target_frame, cloud_in.header.frame_id, fromPCL(
cloud_in.header).stamp, transform);
} catch (tf::LookupException & e) {
ROS_ERROR("%s", e.what());
return false;
} catch (tf::ExtrapolationException & e) {
ROS_ERROR("%s", e.what());
return false;
} }
catch (tf::LookupException &e) transformPointCloud(cloud_in, cloud_out, transform);
{
ROS_ERROR ("%s", e.what ());
return (false);
}
catch (tf::ExtrapolationException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
transformPointCloud (cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool template<typename PointT>
transformPointCloud (const std::string &target_frame, bool
const ros::Time & target_time, transformPointCloud(
const pcl::PointCloud <PointT> &cloud_in, const std::string & target_frame,
const std::string &fixed_frame, const ros::Time & target_time,
pcl::PointCloud <PointT> &cloud_out, const pcl::PointCloud<PointT> & cloud_in,
const tf::TransformListener &tf_listener) const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener)
{ {
tf::StampedTransform transform; tf::StampedTransform transform;
try try {
{ tf_listener.lookupTransform(
tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); target_frame, target_time, cloud_in.header.frame_id,
} fromPCL(cloud_in.header).stamp, fixed_frame, transform);
catch (tf::LookupException &e) } catch (tf::LookupException & e) {
{ ROS_ERROR("%s", e.what());
ROS_ERROR ("%s", e.what ()); return false;
return (false); } catch (tf::ExtrapolationException & e) {
} ROS_ERROR("%s", e.what());
catch (tf::ExtrapolationException &e) return false;
{
ROS_ERROR ("%s", e.what ());
return (false);
} }
transformPointCloud (cloud_in, cloud_out, transform); transformPointCloud(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
std_msgs::Header header; std_msgs::Header header;
header.stamp = target_time; header.stamp = target_time;
cloud_out.header = toPCL(header); cloud_out.header = toPCL(header);
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool template<typename PointT>
transformPointCloudWithNormals (const std::string &target_frame, bool
const ros::Time & target_time, transformPointCloudWithNormals(
const pcl::PointCloud <PointT> &cloud_in, const std::string & target_frame,
const std::string &fixed_frame, const ros::Time & target_time,
pcl::PointCloud <PointT> &cloud_out, const pcl::PointCloud<PointT> & cloud_in,
const tf::TransformListener &tf_listener) const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener)
{ {
tf::StampedTransform transform; tf::StampedTransform transform;
try try {
{ tf_listener.lookupTransform(
tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform); target_frame, target_time, cloud_in.header.frame_id,
} fromPCL(cloud_in.header).stamp, fixed_frame, transform);
catch (tf::LookupException &e) } catch (tf::LookupException & e) {
{ ROS_ERROR("%s", e.what());
ROS_ERROR ("%s", e.what ()); return false;
return (false); } catch (tf::ExtrapolationException & e) {
} ROS_ERROR("%s", e.what());
catch (tf::ExtrapolationException &e) return false;
{
ROS_ERROR ("%s", e.what ());
return (false);
} }
transformPointCloudWithNormals (cloud_in, cloud_out, transform); transformPointCloudWithNormals(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
std_msgs::Header header; std_msgs::Header header;
header.stamp = target_time; header.stamp = target_time;
cloud_out.header = toPCL(header); cloud_out.header = toPCL(header);
return (true); return true;
} }
} // namespace pcl_ros } // namespace pcl_ros

131
pcl_ros/include/pcl_ros/io/bag_io.hpp
View File

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

160
pcl_ros/include/pcl_ros/io/concatenate_data.hpp
View File

@ -49,88 +49,94 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic, * 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 * checks their timestamps, and concatenates their fields together into a single
* PointCloud output message. * PointCloud output message.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class PointCloudConcatenateDataSynchronizer : public nodelet_topic_tools::NodeletLazy
{
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
*/ */
class PointCloudConcatenateDataSynchronizer: public nodelet_topic_tools::NodeletLazy PointCloudConcatenateDataSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), approximate_sync_(false) {}
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateDataSynchronizer() {}
void onInit();
void subscribe();
void unsubscribe();
private:
/** \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 Input point cloud topics. */
XmlRpc::XmlRpcValue input_topics_;
/** \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)
{ {
public: PointCloud2 cloud;
typedef sensor_msgs::PointCloud2 PointCloud2; cloud.header.stamp = input->header.stamp;
typedef PointCloud2::Ptr PointCloud2Ptr; nf_.add(boost::make_shared<PointCloud2>(cloud));
typedef PointCloud2::ConstPtr PointCloud2ConstPtr; }
/** \brief Empty constructor. */ /** \brief Input callback for 8 synchronized topics. */
PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {}; 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);
/** \brief Empty constructor. void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out);
* \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 ();
void subscribe ();
void unsubscribe ();
private:
/** \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 Input point cloud topics. */
XmlRpc::XmlRpcValue input_topics_;
/** \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_ #endif //#ifndef PCL_ROS_IO_CONCATENATE_H_

78
pcl_ros/include/pcl_ros/io/concatenate_fields.hpp
View File

@ -49,53 +49,55 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of /** \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 * input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
* a single PointCloud output message. * a single PointCloud output message.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class PointCloudConcatenateFieldsSynchronizer : public nodelet_topic_tools::NodeletLazy
{
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
*/ */
class PointCloudConcatenateFieldsSynchronizer: public nodelet_topic_tools::NodeletLazy PointCloudConcatenateFieldsSynchronizer(int queue_size)
{ : maximum_queue_size_(queue_size), maximum_seconds_(0) {}
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/** \brief Empty constructor. */ /** \brief Empty destructor. */
PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {}; virtual ~PointCloudConcatenateFieldsSynchronizer() {}
/** \brief Empty constructor. void onInit();
* \param queue_size the maximum queue size void subscribe();
*/ void unsubscribe();
PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {}; void input_callback(const PointCloudConstPtr & cloud);
/** \brief Empty destructor. */ private:
virtual ~PointCloudConcatenateFieldsSynchronizer () {}; /** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
void onInit (); /** \brief The output PointCloud publisher. */
void subscribe (); ros::Publisher pub_output_;
void unsubscribe ();
void input_callback (const PointCloudConstPtr &cloud);
private: /** \brief The number of input messages that we expect on the input topic. */
/** \brief The input PointCloud subscriber. */ int input_messages_;
ros::Subscriber sub_input_;
/** \brief The output PointCloud publisher. */ /** \brief The maximum number of messages that we can store in the queue. */
ros::Publisher pub_output_; int maximum_queue_size_;
/** \brief The number of input messages that we expect on the input topic. */ /** \brief The maximum number of seconds to wait until we drop the synchronization. */
int input_messages_; double maximum_seconds_;
/** \brief The maximum number of messages that we can store in the queue. */ /** \brief A queue for messages. */
int maximum_queue_size_; std::map<ros::Time, std::vector<PointCloudConstPtr>> queue_;
};
/** \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_ #endif //#ifndef PCL_IO_CONCATENATE_H_

152
pcl_ros/include/pcl_ros/io/pcd_io.hpp
View File

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

309
pcl_ros/include/pcl_ros/pcl_nodelet.hpp
View File

@ -66,168 +66,177 @@ using pcl_conversions::fromPCL;
namespace pcl_ros namespace pcl_ros
{ {
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */ /** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */
class PCLNodelet : public nodelet_topic_tools::NodeletLazy class PCLNodelet : public nodelet_topic_tools::NodeletLazy
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
typedef pcl_msgs::PointIndices PointIndices;
typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr;
typedef pcl_msgs::ModelCoefficients ModelCoefficients;
typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
/** \brief Empty constructor. */
PCLNodelet()
: use_indices_(false), latched_indices_(false),
max_queue_size_(3), approximate_sync_(false) {}
protected:
/** \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")
{ {
public: if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
typedef sensor_msgs::PointCloud2 PointCloud2; 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());
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; return false;
typedef boost::shared_ptr<PointCloud> PointCloudPtr; }
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; return true;
}
typedef pcl_msgs::PointIndices PointIndices; /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
typedef PointIndices::Ptr PointIndicesPtr; * \param cloud the point cloud to test
typedef PointIndices::ConstPtr PointIndicesConstPtr; * \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,
fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
pnh_->resolveName(topic_name).c_str());
typedef pcl_msgs::ModelCoefficients ModelCoefficients; return false;
typedef ModelCoefficients::Ptr ModelCoefficientsPtr; }
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; return true;
}
typedef pcl::IndicesPtr IndicesPtr; /** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
typedef pcl::IndicesConstPtr IndicesConstPtr; * \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 Empty constructor. */ /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
PCLNodelet () : use_indices_ (false), latched_indices_ (false), * \param model the model coefficients to test
max_queue_size_ (3), approximate_sync_ (false) {}; * \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;
}
protected: /** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */
/** \brief Set to true if point indices are used. virtual void subscribe() {}
* virtual void unsubscribe() {}
* 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. */ /** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
message_filters::Subscriber<PointCloud> sub_input_filter_; virtual void
onInit()
{
nodelet_topic_tools::NodeletLazy::onInit();
/** \brief The message filter subscriber for PointIndices. */ // Parameters that we care about only at startup
message_filters::Subscriber<PointIndices> sub_indices_filter_; pnh_->getParam("max_queue_size", max_queue_size_);
/** \brief The output PointCloud publisher. */ // ---[ Optional parameters
ros::Publisher pub_output_; pnh_->getParam("use_indices", use_indices_);
pnh_->getParam("latched_indices", latched_indices_);
pnh_->getParam("approximate_sync", approximate_sync_);
/** \brief The maximum queue size (default: 3). */ NODELET_DEBUG(
int max_queue_size_; "[%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_);
}
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ public:
bool approximate_sync_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \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, fromPCL(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 Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */
virtual void subscribe () {}
virtual void unsubscribe () {}
/** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
virtual void
onInit ()
{
nodelet_topic_tools::NodeletLazy::onInit();
// 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_ #endif //#ifndef PCL_NODELET_H_

655
pcl_ros/include/pcl_ros/point_cloud.hpp
View File

@ -13,284 +13,293 @@
namespace pcl namespace pcl
{ {
namespace detail namespace detail
{
template<typename Stream, typename PointT>
struct FieldStreamer
{
FieldStreamer(Stream & stream)
: stream_(stream) {}
template<typename U>
void operator()()
{ {
template<typename Stream, typename PointT> const char * name = traits::name<PointT, U>::value;
struct FieldStreamer std::uint32_t name_length = strlen(name);
{ stream_.next(name_length);
FieldStreamer(Stream& stream) : stream_(stream) {} if (name_length > 0) {
memcpy(stream_.advance(name_length), name, name_length);
}
template<typename U> void operator() () std::uint32_t offset = traits::offset<PointT, U>::value;
{ stream_.next(offset);
const char* name = traits::name<PointT, U>::value;
std::uint32_t name_length = strlen(name);
stream_.next(name_length);
if (name_length > 0)
memcpy(stream_.advance(name_length), name, name_length);
std::uint32_t offset = traits::offset<PointT, U>::value; std::uint8_t datatype = traits::datatype<PointT, U>::value;
stream_.next(offset); stream_.next(datatype);
std::uint8_t datatype = traits::datatype<PointT, U>::value; std::uint32_t count = traits::datatype<PointT, U>::size;
stream_.next(datatype); stream_.next(count);
}
std::uint32_t count = traits::datatype<PointT, U>::size; Stream & stream_;
stream_.next(count); };
}
Stream& stream_; template<typename PointT>
}; struct FieldsLength
{
FieldsLength()
: length(0) {}
template<typename PointT> template<typename U>
struct FieldsLength void operator()()
{ {
FieldsLength() : length(0) {} std::uint32_t name_length = strlen(traits::name<PointT, U>::value);
length += name_length + 13;
}
template<typename U> void operator() () std::uint32_t length;
{ };
std::uint32_t name_length = strlen(traits::name<PointT, U>::value); } // namespace pcl::detail
length += name_length + 13;
}
std::uint32_t length;
};
} // namespace pcl::detail
} // namespace pcl } // namespace pcl
namespace ros namespace ros
{ {
// In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects // In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
// on the subscriber side. This allows us to generate the mapping between message // on the subscriber side. This allows us to generate the mapping between message
// data and object fields only once and reuse it. // data and object fields only once and reuse it.
#if ROS_VERSION_MINIMUM(1, 3, 1) #if ROS_VERSION_MINIMUM(1, 3, 1)
template<typename T> template<typename T>
struct DefaultMessageCreator<pcl::PointCloud<T> > struct DefaultMessageCreator<pcl::PointCloud<T>>
{
boost::shared_ptr<pcl::MsgFieldMap> mapping_;
DefaultMessageCreator()
: mapping_(boost::make_shared<pcl::MsgFieldMap>() )
{ {
boost::shared_ptr<pcl::MsgFieldMap> mapping_; }
DefaultMessageCreator() boost::shared_ptr<pcl::PointCloud<T>> operator()()
: mapping_( boost::make_shared<pcl::MsgFieldMap>() ) {
{ boost::shared_ptr<pcl::PointCloud<T>> msg(new pcl::PointCloud<T>());
} pcl::detail::getMapping(*msg) = mapping_;
return msg;
boost::shared_ptr<pcl::PointCloud<T> > operator() () }
{ };
boost::shared_ptr<pcl::PointCloud<T> > msg (new pcl::PointCloud<T> ());
pcl::detail::getMapping(*msg) = mapping_;
return msg;
}
};
#endif #endif
namespace message_traits namespace message_traits
{
template<typename T>
struct MD5Sum<pcl::PointCloud<T>>
{
static const char * value() {return MD5Sum<sensor_msgs::PointCloud2>::value();}
static const char * value(const pcl::PointCloud<T> &) {return value();}
static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
// If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
};
template<typename T>
struct DataType<pcl::PointCloud<T>>
{
static const char * value() {return DataType<sensor_msgs::PointCloud2>::value();}
static const char * value(const pcl::PointCloud<T> &) {return value();}
};
template<typename T>
struct Definition<pcl::PointCloud<T>>
{
static const char * value() {return Definition<sensor_msgs::PointCloud2>::value();}
static const char * value(const pcl::PointCloud<T> &) {return value();}
};
// pcl point clouds message don't have a ROS compatible header
// the specialized meta functions below (TimeStamp and FrameId)
// can be used to get the header data.
template<typename T>
struct HasHeader<pcl::PointCloud<T>>: FalseType {};
template<typename T>
struct TimeStamp<pcl::PointCloud<T>>
{
// This specialization could be dangerous, but it's the best I can do.
// If this TimeStamp struct is destroyed before they are done with the
// pointer returned by the first functions may go out of scope, but there
// isn't a lot I can do about that. This is a good reason to refuse to
// returning pointers like this...
static ros::Time * pointer(typename pcl::PointCloud<T> & m)
{ {
template<typename T> struct MD5Sum<pcl::PointCloud<T> > header_.reset(new std_msgs::Header());
{ pcl_conversions::fromPCL(m.header, *(header_));
static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); } return &(header_->stamp);
static const char* value(const pcl::PointCloud<T>&) { return value(); } }
static ros::Time const * pointer(const typename pcl::PointCloud<T> & m)
static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
// If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
};
template<typename T> struct DataType<pcl::PointCloud<T> >
{
static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
static const char* value(const pcl::PointCloud<T>&) { return value(); }
};
template<typename T> struct Definition<pcl::PointCloud<T> >
{
static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
static const char* value(const pcl::PointCloud<T>&) { return value(); }
};
// pcl point clouds message don't have a ROS compatible header
// the specialized meta functions below (TimeStamp and FrameId)
// can be used to get the header data.
template<typename T> struct HasHeader<pcl::PointCloud<T> > : FalseType {};
template<typename T>
struct TimeStamp<pcl::PointCloud<T> >
{
// This specialization could be dangerous, but it's the best I can do.
// If this TimeStamp struct is destroyed before they are done with the
// pointer returned by the first functions may go out of scope, but there
// isn't a lot I can do about that. This is a good reason to refuse to
// returning pointers like this...
static ros::Time* pointer(typename pcl::PointCloud<T> &m) {
header_.reset(new std_msgs::Header());
pcl_conversions::fromPCL(m.header, *(header_));
return &(header_->stamp);
}
static ros::Time const* pointer(const typename pcl::PointCloud<T>& m) {
header_const_.reset(new std_msgs::Header());
pcl_conversions::fromPCL(m.header, *(header_const_));
return &(header_const_->stamp);
}
static ros::Time value(const typename pcl::PointCloud<T>& m) {
return pcl_conversions::fromPCL(m.header).stamp;
}
private:
static boost::shared_ptr<std_msgs::Header> header_;
static boost::shared_ptr<std_msgs::Header> header_const_;
};
template<typename T>
struct FrameId<pcl::PointCloud<T> >
{
static std::string* pointer(pcl::PointCloud<T>& m) { return &m.header.frame_id; }
static std::string const* pointer(const pcl::PointCloud<T>& m) { return &m.header.frame_id; }
static std::string value(const pcl::PointCloud<T>& m) { return m.header.frame_id; }
};
} // namespace ros::message_traits
namespace serialization
{ {
template<typename T> header_const_.reset(new std_msgs::Header());
struct Serializer<pcl::PointCloud<T> > pcl_conversions::fromPCL(m.header, *(header_const_));
return &(header_const_->stamp);
}
static ros::Time value(const typename pcl::PointCloud<T> & m)
{
return pcl_conversions::fromPCL(m.header).stamp;
}
private:
static boost::shared_ptr<std_msgs::Header> header_;
static boost::shared_ptr<std_msgs::Header> header_const_;
};
template<typename T>
struct FrameId<pcl::PointCloud<T>>
{
static std::string * pointer(pcl::PointCloud<T> & m) {return &m.header.frame_id;}
static std::string const * pointer(const pcl::PointCloud<T> & m) {return &m.header.frame_id;}
static std::string value(const pcl::PointCloud<T> & m) {return m.header.frame_id;}
};
} // namespace ros::message_traits
namespace serialization
{
template<typename T>
struct Serializer<pcl::PointCloud<T>>
{
template<typename Stream>
inline static void write(Stream & stream, const pcl::PointCloud<T> & m)
{
stream.next(m.header);
// Ease the user's burden on specifying width/height for unorganized datasets
uint32_t height = m.height, width = m.width;
if (height == 0 && width == 0) {
width = m.points.size();
height = 1;
}
stream.next(height);
stream.next(width);
// Stream out point field metadata
typedef typename pcl::traits::fieldList<T>::type FieldList;
uint32_t fields_size = boost::mpl::size<FieldList>::value;
stream.next(fields_size);
pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
// Assume little-endian...
uint8_t is_bigendian = false;
stream.next(is_bigendian);
// Write out point data as binary blob
uint32_t point_step = sizeof(T);
stream.next(point_step);
uint32_t row_step = point_step * width;
stream.next(row_step);
uint32_t data_size = row_step * height;
stream.next(data_size);
memcpy(stream.advance(data_size), &m.points[0], data_size);
uint8_t is_dense = m.is_dense;
stream.next(is_dense);
}
template<typename Stream>
inline static void read(Stream & stream, pcl::PointCloud<T> & m)
{
std_msgs::Header header;
stream.next(header);
pcl_conversions::toPCL(header, m.header);
stream.next(m.height);
stream.next(m.width);
/// @todo Check that fields haven't changed!
std::vector<sensor_msgs::PointField> fields;
stream.next(fields);
// Construct field mapping if deserializing for the first time
boost::shared_ptr<pcl::MsgFieldMap> & mapping_ptr = pcl::detail::getMapping(m);
if (!mapping_ptr) {
// This normally should get allocated by DefaultMessageCreator, but just in case
mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
}
pcl::MsgFieldMap & mapping = *mapping_ptr;
if (mapping.empty()) {
pcl::createMapping<T>(fields, mapping);
}
uint8_t is_bigendian;
stream.next(is_bigendian); // ignoring...
uint32_t point_step, row_step;
stream.next(point_step);
stream.next(row_step);
// Copy point data
uint32_t data_size;
stream.next(data_size);
assert(data_size == m.height * m.width * point_step);
m.points.resize(m.height * m.width);
uint8_t * m_data = reinterpret_cast<uint8_t *>(&m.points[0]);
// If the data layouts match, can copy a whole row in one memcpy
if (mapping.size() == 1 &&
mapping[0].serialized_offset == 0 &&
mapping[0].struct_offset == 0 &&
point_step == sizeof(T))
{ {
template<typename Stream> uint32_t m_row_step = sizeof(T) * m.width;
inline static void write(Stream& stream, const pcl::PointCloud<T>& m) // And if the row steps match, can copy whole point cloud in one memcpy
{ if (m_row_step == row_step) {
stream.next(m.header); memcpy(m_data, stream.advance(data_size), data_size);
} else {
// Ease the user's burden on specifying width/height for unorganized datasets for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) {
uint32_t height = m.height, width = m.width; memcpy(m_data, stream.advance(row_step), m_row_step);
if (height == 0 && width == 0) {
width = m.points.size();
height = 1;
} }
stream.next(height);
stream.next(width);
// Stream out point field metadata
typedef typename pcl::traits::fieldList<T>::type FieldList;
uint32_t fields_size = boost::mpl::size<FieldList>::value;
stream.next(fields_size);
pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
// Assume little-endian...
uint8_t is_bigendian = false;
stream.next(is_bigendian);
// Write out point data as binary blob
uint32_t point_step = sizeof(T);
stream.next(point_step);
uint32_t row_step = point_step * width;
stream.next(row_step);
uint32_t data_size = row_step * height;
stream.next(data_size);
memcpy(stream.advance(data_size), &m.points[0], data_size);
uint8_t is_dense = m.is_dense;
stream.next(is_dense);
} }
} else {
template<typename Stream> // If not, do a lot of memcpys to copy over the fields
inline static void read(Stream& stream, pcl::PointCloud<T>& m) for (uint32_t row = 0; row < m.height; ++row) {
{ const uint8_t * stream_data = stream.advance(row_step);
std_msgs::Header header; for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
stream.next(header); BOOST_FOREACH(const pcl::detail::FieldMapping & fm, mapping) {
pcl_conversions::toPCL(header, m.header); memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
stream.next(m.height);
stream.next(m.width);
/// @todo Check that fields haven't changed!
std::vector<sensor_msgs::PointField> fields;
stream.next(fields);
// Construct field mapping if deserializing for the first time
boost::shared_ptr<pcl::MsgFieldMap>& mapping_ptr = pcl::detail::getMapping(m);
if (!mapping_ptr)
{
// This normally should get allocated by DefaultMessageCreator, but just in case
mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
}
pcl::MsgFieldMap& mapping = *mapping_ptr;
if (mapping.empty())
pcl::createMapping<T> (fields, mapping);
uint8_t is_bigendian;
stream.next(is_bigendian); // ignoring...
uint32_t point_step, row_step;
stream.next(point_step);
stream.next(row_step);
// Copy point data
uint32_t data_size;
stream.next(data_size);
assert(data_size == m.height * m.width * point_step);
m.points.resize(m.height * m.width);
uint8_t* m_data = reinterpret_cast<uint8_t*>(&m.points[0]);
// If the data layouts match, can copy a whole row in one memcpy
if (mapping.size() == 1 &&
mapping[0].serialized_offset == 0 &&
mapping[0].struct_offset == 0 &&
point_step == sizeof(T))
{
uint32_t m_row_step = sizeof(T) * m.width;
// And if the row steps match, can copy whole point cloud in one memcpy
if (m_row_step == row_step)
{
memcpy (m_data, stream.advance(data_size), data_size);
}
else
{
for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step)
memcpy (m_data, stream.advance(row_step), m_row_step);
} }
m_data += sizeof(T);
} }
else
{
// If not, do a lot of memcpys to copy over the fields
for (uint32_t row = 0; row < m.height; ++row) {
const uint8_t* stream_data = stream.advance(row_step);
for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) {
memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
}
m_data += sizeof(T);
}
}
}
uint8_t is_dense;
stream.next(is_dense);
m.is_dense = is_dense;
} }
}
inline static uint32_t serializedLength(const pcl::PointCloud<T>& m) uint8_t is_dense;
{ stream.next(is_dense);
uint32_t length = 0; m.is_dense = is_dense;
}
length += serializationLength(m.header); inline static uint32_t serializedLength(const pcl::PointCloud<T> & m)
length += 8; // height/width {
uint32_t length = 0;
pcl::detail::FieldsLength<T> fl; length += serializationLength(m.header);
typedef typename pcl::traits::fieldList<T>::type FieldList; length += 8; // height/width
pcl::for_each_type<FieldList>(boost::ref(fl));
length += 4; // size of 'fields'
length += fl.length;
length += 1; // is_bigendian pcl::detail::FieldsLength<T> fl;
length += 4; // point_step typedef typename pcl::traits::fieldList<T>::type FieldList;
length += 4; // row_step pcl::for_each_type<FieldList>(boost::ref(fl));
length += 4; // size of 'data' length += 4; // size of 'fields'
length += m.points.size() * sizeof(T); // data length += fl.length;
length += 1; // is_dense
return length; length += 1; // is_bigendian
} length += 4; // point_step
}; length += 4; // row_step
} // namespace ros::serialization length += 4; // size of 'data'
length += m.points.size() * sizeof(T); // data
length += 1; // is_dense
/// @todo Printer specialization in message_operations return length;
}
};
} // namespace ros::serialization
/// @todo Printer specialization in message_operations
} // namespace ros } // namespace ros
@ -315,103 +324,101 @@ namespace ros
namespace pcl namespace pcl
{ {
namespace detail namespace detail
{ {
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#if PCL_VERSION_COMPARE(>=, 1, 10, 0) #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
template <class T> template<class T>
constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>, constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>,
pcl::shared_ptr<T>>::value; pcl::shared_ptr<T>>::value;
#else #else
template <class T> template<class T>
constexpr static bool pcl_uses_boost = true; constexpr static bool pcl_uses_boost = true;
#endif #endif
template<class SharedPointer> struct Holder template<class SharedPointer>
{ struct Holder
SharedPointer p; {
SharedPointer p;
Holder(const SharedPointer &p) : p(p) {} Holder(const SharedPointer & p)
Holder(const Holder &other) : p(other.p) {} : p(p) {}
Holder(Holder &&other) : p(std::move(other.p)) {} Holder(const Holder & other)
: p(other.p) {}
Holder(Holder && other)
: p(std::move(other.p)) {}
void operator () (...) { p.reset(); } void operator()(...) {p.reset();}
}; };
template<class T> template<class T>
inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> &p) inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> & p)
{ {
typedef Holder<std::shared_ptr<T>> H; typedef Holder<std::shared_ptr<T>> H;
if(H *h = boost::get_deleter<H>(p)) if (H * h = boost::get_deleter<H>(p)) {
{ return h->p;
return h->p; } else {
} return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
else }
{ }
return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
}
}
template<class T> template<class T>
inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> &p) inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
{ {
typedef Holder<boost::shared_ptr<T>> H; typedef Holder<boost::shared_ptr<T>> H;
if(H * h = std::get_deleter<H>(p)) if (H * h = std::get_deleter<H>(p)) {
{ return h->p;
return h->p; } else {
} return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
else }
{ }
return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
}
}
#endif #endif
} // namespace pcl::detail } // namespace pcl::detail
// add functions to convert to smart pointer used by ROS // add functions to convert to smart pointer used by ROS
template <class T> template<class T>
inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> &p) inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> & p)
{ {
return p; return p;
} }
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
template <class T> template<class T>
inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> &p) inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> & p)
{ {
return detail::to_boost_ptr(p); return detail::to_boost_ptr(p);
} }
// add functions to convert to smart pointer used by PCL, based on PCL's own pointer // add functions to convert to smart pointer used by PCL, based on PCL's own pointer
template <class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type> template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
inline std::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> &p) inline std::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
{ {
return p; return p;
} }
template <class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type> template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
inline std::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> &p) inline std::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
{ {
return detail::to_std_ptr(p); return detail::to_std_ptr(p);
} }
template <class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type> template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
inline boost::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> &p) inline boost::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
{ {
return detail::to_boost_ptr(p); return detail::to_boost_ptr(p);
} }
template <class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type> template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> &p) inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
{ {
return p; return p;
} }
#else #else
template <class T> template<class T>
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> &p) inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
{ {
return p; return p;
} }
#endif #endif
} // namespace pcl } // namespace pcl

166
pcl_ros/include/pcl_ros/publisher.hpp
View File

@ -53,97 +53,97 @@
namespace pcl_ros namespace pcl_ros
{ {
class BasePublisher class BasePublisher
{
public:
void
advertise(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
{ {
public: pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
void }
advertise (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
{
pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
}
std::string std::string
getTopic () getTopic()
{
return (pub_.getTopic ());
}
uint32_t
getNumSubscribers () const
{
return (pub_.getNumSubscribers ());
}
void
shutdown ()
{
pub_.shutdown ();
}
operator void*() const
{
return (pub_) ? (void*)1 : (void*)0;
}
protected:
ros::Publisher pub_;
};
template <typename PointT>
class Publisher : public BasePublisher
{ {
public: return pub_.getTopic();
Publisher () {} }
Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) uint32_t
{ getNumSubscribers() const
advertise (nh, topic, queue_size);
}
~Publisher () {}
inline void
publish (const boost::shared_ptr<const pcl::PointCloud<PointT> > &point_cloud) const
{
publish (*point_cloud);
}
inline void
publish (const pcl::PointCloud<PointT>& point_cloud) const
{
// Fill point cloud binary data
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg (point_cloud, msg);
pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (msg));
}
};
template <>
class Publisher<sensor_msgs::PointCloud2> : public BasePublisher
{ {
public: return pub_.getNumSubscribers();
Publisher () {} }
Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size) void
{ shutdown()
advertise (nh, topic, queue_size); {
} pub_.shutdown();
}
~Publisher () {} operator void *() const
{
return (pub_) ? (void *)1 : (void *)0;
}
void protected:
publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const ros::Publisher pub_;
{ };
pub_.publish (point_cloud);
//pub_.publish (*point_cloud);
}
void template<typename PointT>
publish (const sensor_msgs::PointCloud2& point_cloud) const class Publisher : public BasePublisher
{ {
pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (point_cloud)); public:
//pub_.publish (point_cloud); Publisher() {}
}
}; Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
{
advertise(nh, topic, queue_size);
}
~Publisher() {}
inline void
publish(const boost::shared_ptr<const pcl::PointCloud<PointT>> & point_cloud) const
{
publish(*point_cloud);
}
inline void
publish(const pcl::PointCloud<PointT> & point_cloud) const
{
// Fill point cloud binary data
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(point_cloud, msg);
pub_.publish(boost::make_shared<const sensor_msgs::PointCloud2>(msg));
}
};
template<>
class Publisher<sensor_msgs::PointCloud2>: public BasePublisher
{
public:
Publisher() {}
Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
{
advertise(nh, topic, queue_size);
}
~Publisher() {}
void
publish(const sensor_msgs::PointCloud2Ptr & point_cloud) const
{
pub_.publish(point_cloud);
//pub_.publish (*point_cloud);
}
void
publish(const sensor_msgs::PointCloud2 & point_cloud) const
{
pub_.publish(boost::make_shared<const sensor_msgs::PointCloud2>(point_cloud));
//pub_.publish (point_cloud);
}
};
} }
#endif //#ifndef PCL_ROS_PUBLISHER_H_ #endif //#ifndef PCL_ROS_PUBLISHER_H_

105
pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp
View File

@ -47,63 +47,68 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class EuclideanClusterExtraction : public PCLNodelet
{
public:
/** \brief Empty constructor. */
EuclideanClusterExtraction()
: publish_indices_(false), max_clusters_(std::numeric_limits<int>::max()) {}
protected:
// ROS nodelet attributes
/** \brief Publish indices or convert to PointCloud clusters. Default: false */
bool publish_indices_;
/** \brief Maximum number of clusters to publish. */
int max_clusters_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>> srv_;
/** \brief Nodelet initialization routine. */
void onInit();
/** \brief LazyNodelet connection routine. */
void subscribe();
void unsubscribe();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/ */
class EuclideanClusterExtraction : public PCLNodelet void config_callback(EuclideanClusterExtractionConfig & config, uint32_t level);
{
public:
/** \brief Empty constructor. */
EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits<int>::max ()) {};
protected: /** \brief Input point cloud callback.
// ROS nodelet attributes * \param cloud the pointer to the input point cloud
/** \brief Publish indices or convert to PointCloud clusters. Default: false */ * \param indices the pointer to the input point cloud indices
bool publish_indices_; */
void input_indices_callback(
const PointCloudConstPtr & cloud,
const PointIndicesConstPtr & indices);
/** \brief Maximum number of clusters to publish. */ private:
int max_clusters_; /** \brief The PCL implementation used. */
pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
/** \brief Pointer to a dynamic reconfigure service. */ /** \brief The input PointCloud subscriber. */
boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > srv_; ros::Subscriber sub_input_;
/** \brief Nodelet initialization routine. */ /** \brief Synchronized input, and indices.*/
void onInit (); boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>> sync_input_indices_a_;
/** \brief LazyNodelet connection routine. */ public:
void subscribe (); EIGEN_MAKE_ALIGNED_OPERATOR_NEW
void unsubscribe (); };
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (EuclideanClusterExtractionConfig &config, uint32_t level);
/** \brief Input point cloud callback.
* \param cloud the pointer to the input point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
private:
/** \brief The PCL implementation used. */
pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ #endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_

132
pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp
View File

@ -52,79 +52,83 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
/** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with /** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with
* a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying * a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
* inside it. * inside it.
* *
* An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane). * An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane).
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class ExtractPolygonalPrismData : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
protected:
/** \brief The output PointIndices publisher. */
ros::Publisher pub_output_;
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_hull_filter_;
/** \brief Synchronized input, planar hull, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud,
PointIndices>>> sync_input_hull_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud, PointIndices>>> sync_input_hull_indices_a_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>> srv_;
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointIndices> nf_;
/** \brief Input point cloud callback.
* Because we want to use the same synchronizer object, we push back
* empty elements with the same timestamp.
*/ */
class ExtractPolygonalPrismData : public PCLNodelet inline void
input_callback(const PointCloudConstPtr & input)
{ {
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; PointIndices cloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr; cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; nf_.add(boost::make_shared<PointIndices>(cloud));
}
protected: /** \brief Nodelet initialization routine. */
/** \brief The output PointIndices publisher. */ void onInit();
ros::Publisher pub_output_;
/** \brief The message filter subscriber for PointCloud2. */ /** \brief LazyNodelet connection routine. */
message_filters::Subscriber<PointCloud> sub_hull_filter_; void subscribe();
void unsubscribe();
/** \brief Synchronized input, planar hull, and indices.*/ /** \brief Dynamic reconfigure callback
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_e_; * \param config the config object
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_a_; * \param level the dynamic reconfigure level
*/
void config_callback(ExtractPolygonalPrismDataConfig & config, uint32_t level);
/** \brief Pointer to a dynamic reconfigure service. */ /** \brief Input point cloud callback. Used when \a use_indices is set.
boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > srv_; * \param cloud the pointer to the input point cloud
* \param hull the pointer to the planar hull point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_hull_indices_callback(
const PointCloudConstPtr & cloud,
const PointCloudConstPtr & hull,
const PointIndicesConstPtr & indices);
/** \brief Null passthrough filter, used for pushing empty elements in the private:
* synchronizer */ /** \brief The PCL implementation used. */
message_filters::PassThrough<PointIndices> nf_; pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
/** \brief Input point cloud callback. public:
* Because we want to use the same synchronizer object, we push back EIGEN_MAKE_ALIGNED_OPERATOR_NEW
* empty elements with the same timestamp. };
*/
inline void
input_callback (const PointCloudConstPtr &input)
{
PointIndices cloud;
cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
nf_.add (boost::make_shared<PointIndices> (cloud));
}
/** \brief Nodelet initialization routine. */
void onInit ();
/** \brief LazyNodelet connection routine. */
void subscribe ();
void unsubscribe ();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level);
/** \brief Input point cloud callback. Used when \a use_indices is set.
* \param cloud the pointer to the input point cloud
* \param hull the pointer to the planar hull point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_hull_indices_callback (const PointCloudConstPtr &cloud,
const PointCloudConstPtr &hull,
const PointIndicesConstPtr &indices);
private:
/** \brief The PCL implementation used. */
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ #endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_

414
pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp
View File

@ -51,235 +51,243 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in /** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in
* the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. * the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class SACSegmentation : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
public:
/** \brief Constructor. */
SACSegmentation()
: min_inliers_(0) {}
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
*/ */
class SACSegmentation : public PCLNodelet inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
public: /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
/** \brief Constructor. */ inline std::string getInputTFframe() {return tf_input_frame_;}
SACSegmentation () : min_inliers_(0) {}
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. /** \brief Set the output TF frame the data should be transformed into after processing.
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing * \param tf_frame the TF frame the PointCloud should be transformed into after processing
*/
inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
inline std::string getInputTFframe () { return (tf_input_frame_); }
/** \brief Set the output TF frame the data should be transformed into after processing.
* \param tf_frame the TF frame the PointCloud should be transformed into after processing
*/
inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
inline std::string getOutputTFframe () { return (tf_output_frame_); }
protected:
// The minimum number of inliers a model must have in order to be considered valid.
int min_inliers_;
// ROS nodelet attributes
/** \brief The output PointIndices publisher. */
ros::Publisher pub_indices_;
/** \brief The output ModelCoefficients publisher. */
ros::Publisher pub_model_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig> > srv_;
/** \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 Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<pcl_msgs::PointIndices> nf_pi_;
/** \brief Nodelet initialization routine. */
virtual void onInit ();
/** \brief LazyNodelet connection routine. */
virtual void subscribe ();
virtual void unsubscribe ();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (SACSegmentationConfig &config, uint32_t level);
/** \brief Input point cloud callback. Used when \a use_indices is set.
* \param cloud the pointer to the input point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
/** \brief Pointer to a set of indices stored internally.
* (used when \a latched_indices_ is set).
*/
PointIndices indices_;
/** \brief Indices callback. Used when \a latched_indices_ is set.
* \param indices the pointer to the input point cloud indices
*/
inline void
indices_callback (const PointIndicesConstPtr &indices)
{
indices_ = *indices;
}
/** \brief Input callback. Used when \a latched_indices_ is set.
* \param input the pointer to the input point cloud
*/
inline void
input_callback (const PointCloudConstPtr &input)
{
indices_.header = fromPCL(input->header);
PointIndicesConstPtr indices;
indices.reset (new PointIndices (indices_));
nf_pi_.add (indices);
}
private:
/** \brief Internal mutex. */
boost::mutex mutex_;
/** \brief The PCL implementation used. */
pcl::SACSegmentation<pcl::PointXYZ> impl_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
* models that require the use of surface normals for estimation.
*/ */
class SACSegmentationFromNormals: public SACSegmentation inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
inline std::string getOutputTFframe() {return tf_output_frame_;}
protected:
// The minimum number of inliers a model must have in order to be considered valid.
int min_inliers_;
// ROS nodelet attributes
/** \brief The output PointIndices publisher. */
ros::Publisher pub_indices_;
/** \brief The output ModelCoefficients publisher. */
ros::Publisher pub_model_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig>> srv_;
/** \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 Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<pcl_msgs::PointIndices> nf_pi_;
/** \brief Nodelet initialization routine. */
virtual void onInit();
/** \brief LazyNodelet connection routine. */
virtual void subscribe();
virtual void unsubscribe();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(SACSegmentationConfig & config, uint32_t level);
/** \brief Input point cloud callback. Used when \a use_indices is set.
* \param cloud the pointer to the input point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_indices_callback(
const PointCloudConstPtr & cloud,
const PointIndicesConstPtr & indices);
/** \brief Pointer to a set of indices stored internally.
* (used when \a latched_indices_ is set).
*/
PointIndices indices_;
/** \brief Indices callback. Used when \a latched_indices_ is set.
* \param indices the pointer to the input point cloud indices
*/
inline void
indices_callback(const PointIndicesConstPtr & indices)
{ {
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; indices_ = *indices;
typedef boost::shared_ptr<PointCloud> PointCloudPtr; }
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
typedef pcl::PointCloud<pcl::Normal> PointCloudN; /** \brief Input callback. Used when \a latched_indices_ is set.
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr; * \param input the pointer to the input point cloud
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr; */
inline void
input_callback(const PointCloudConstPtr & input)
{
indices_.header = fromPCL(input->header);
PointIndicesConstPtr indices;
indices.reset(new PointIndices(indices_));
nf_pi_.add(indices);
}
public: private:
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. /** \brief Internal mutex. */
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing boost::mutex mutex_;
*/
inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ /** \brief The PCL implementation used. */
inline std::string getInputTFframe () { return (tf_input_frame_); } pcl::SACSegmentation<pcl::PointXYZ> impl_;
/** \brief Set the output TF frame the data should be transformed into after processing. /** \brief Synchronized input, and indices.*/
* \param tf_frame the TF frame the PointCloud should be transformed into after processing boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
*/ PointIndices>>> sync_input_indices_e_;
inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; } boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>> sync_input_indices_a_;
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */ public:
inline std::string getOutputTFframe () { return (tf_output_frame_); } EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
protected: ////////////////////////////////////////////////////////////////////////////////////////////
// ROS nodelet attributes /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
/** \brief The normals PointCloud subscriber filter. */ * models that require the use of surface normals for estimation.
message_filters::Subscriber<PointCloudN> sub_normals_filter_; */
class SACSegmentationFromNormals : public SACSegmentation
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
/** \brief The input PointCloud subscriber. */ typedef pcl::PointCloud<pcl::Normal> PointCloudN;
ros::Subscriber sub_axis_; typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
/** \brief Pointer to a dynamic reconfigure service. */ public:
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > srv_; /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
*/
inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
/** \brief Input point cloud callback. /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
* Because we want to use the same synchronizer object, we push back inline std::string getInputTFframe() {return tf_input_frame_;}
* empty elements with the same timestamp.
*/
inline void
input_callback (const PointCloudConstPtr &cloud)
{
PointIndices indices;
indices.header.stamp = fromPCL(cloud->header).stamp;
nf_.add (boost::make_shared<PointIndices> (indices));
}
/** \brief Null passthrough filter, used for pushing empty elements in the /** \brief Set the output TF frame the data should be transformed into after processing.
* synchronizer */ * \param tf_frame the TF frame the PointCloud should be transformed into after processing
message_filters::PassThrough<PointIndices> nf_; */
inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ /** \brief Get the TF frame the PointCloud should be transformed into after processing. */
std::string tf_input_frame_; inline std::string getOutputTFframe() {return tf_output_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 Nodelet initialization routine. */ protected:
virtual void onInit (); // ROS nodelet attributes
/** \brief The normals PointCloud subscriber filter. */
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
/** \brief LazyNodelet connection routine. */ /** \brief The input PointCloud subscriber. */
virtual void subscribe (); ros::Subscriber sub_axis_;
virtual void unsubscribe ();
/** \brief Model callback /** \brief Pointer to a dynamic reconfigure service. */
* \param model the sample consensus model found boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>> srv_;
*/
void axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model);
/** \brief Dynamic reconfigure callback /** \brief Input point cloud callback.
* \param config the config object * Because we want to use the same synchronizer object, we push back
* \param level the dynamic reconfigure level * empty elements with the same timestamp.
*/ */
void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level); inline void
input_callback(const PointCloudConstPtr & cloud)
{
PointIndices indices;
indices.header.stamp = fromPCL(cloud->header).stamp;
nf_.add(boost::make_shared<PointIndices>(indices));
}
/** \brief Input point cloud callback. /** \brief Null passthrough filter, used for pushing empty elements in the
* \param cloud the pointer to the input point cloud * synchronizer */
* \param cloud_normals the pointer to the input point cloud normals message_filters::PassThrough<PointIndices> nf_;
* \param indices the pointer to the input point cloud indices
*/
void input_normals_indices_callback (const PointCloudConstPtr &cloud,
const PointCloudNConstPtr &cloud_normals,
const PointIndicesConstPtr &indices);
private: /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
/** \brief Internal mutex. */ std::string tf_input_frame_;
boost::mutex mutex_; /** \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 The PCL implementation used. */ /** \brief Nodelet initialization routine. */
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_; virtual void onInit();
/** \brief Synchronized input, normals, and indices.*/ /** \brief LazyNodelet connection routine. */
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_a_; virtual void subscribe();
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_e_; virtual void unsubscribe();
public: /** \brief Model callback
EIGEN_MAKE_ALIGNED_OPERATOR_NEW * \param model the sample consensus model found
}; */
void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level);
/** \brief Input point cloud callback.
* \param cloud the pointer to the input point cloud
* \param cloud_normals the pointer to the input point cloud normals
* \param indices the pointer to the input point cloud indices
*/
void input_normals_indices_callback(
const PointCloudConstPtr & cloud,
const PointCloudNConstPtr & cloud_normals,
const PointIndicesConstPtr & indices);
private:
/** \brief Internal mutex. */
boost::mutex mutex_;
/** \brief The PCL implementation used. */
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
/** \brief Synchronized input, normals, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloudN, PointIndices>>> sync_input_normals_indices_a_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN,
PointIndices>>> sync_input_normals_indices_e_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ #endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_

107
pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp
View File

@ -48,63 +48,66 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the /** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
* difference between them for a maximum given distance threshold. * difference between them for a maximum given distance threshold.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class SegmentDifferences : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
public:
/** \brief Empty constructor. */
SegmentDifferences() {}
protected:
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_target_filter_;
/** \brief Synchronized input, and planar hull.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloud>>> sync_input_target_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud>>> sync_input_target_a_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig>> srv_;
/** \brief Nodelet initialization routine. */
void onInit();
/** \brief LazyNodelet connection routine. */
void subscribe();
void unsubscribe();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/ */
class SegmentDifferences : public PCLNodelet void config_callback(SegmentDifferencesConfig & config, uint32_t level);
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
public: /** \brief Input point cloud callback.
/** \brief Empty constructor. */ * \param cloud the pointer to the input point cloud
SegmentDifferences () {}; * \param cloud_target the pointcloud that we want to segment \a cloud from
*/
void input_target_callback(
const PointCloudConstPtr & cloud,
const PointCloudConstPtr & cloud_target);
protected: private:
/** \brief The message filter subscriber for PointCloud2. */ /** \brief The PCL implementation used. */
message_filters::Subscriber<PointCloud> sub_target_filter_; pcl::SegmentDifferences<pcl::PointXYZ> impl_;
/** \brief Synchronized input, and planar hull.*/ public:
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > sync_input_target_e_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > sync_input_target_a_; };
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig> > srv_;
/** \brief Nodelet initialization routine. */
void onInit ();
/** \brief LazyNodelet connection routine. */
void subscribe ();
void unsubscribe ();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (SegmentDifferencesConfig &config, uint32_t level);
/** \brief Input point cloud callback.
* \param cloud the pointer to the input point cloud
* \param cloud_target the pointcloud that we want to segment \a cloud from
*/
void input_target_callback (const PointCloudConstPtr &cloud,
const PointCloudConstPtr &cloud_target);
private:
/** \brief The PCL implementation used. */
pcl::SegmentDifferences<pcl::PointXYZ> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ #endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_

75
pcl_ros/include/pcl_ros/surface/convex_hull.hpp
View File

@ -48,49 +48,52 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation. /** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/
class ConvexHull2D : public PCLNodelet
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
private:
/** \brief Nodelet initialization routine. */
virtual void onInit();
/** \brief LazyNodelet connection routine. */
virtual void subscribe();
virtual void unsubscribe();
/** \brief Input point cloud callback.
* \param cloud the pointer to the input point cloud
* \param indices the pointer to the input point cloud indices
*/ */
class ConvexHull2D : public PCLNodelet void input_indices_callback(
{ const PointCloudConstPtr & cloud,
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; const PointIndicesConstPtr & indices);
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
private: private:
/** \brief Nodelet initialization routine. */ /** \brief The PCL implementation used. */
virtual void onInit (); pcl::ConvexHull<pcl::PointXYZ> impl_;
/** \brief LazyNodelet connection routine. */ /** \brief The input PointCloud subscriber. */
virtual void subscribe (); ros::Subscriber sub_input_;
virtual void unsubscribe ();
/** \brief Input point cloud callback. /** \brief Publisher for PolygonStamped. */
* \param cloud the pointer to the input point cloud ros::Publisher pub_plane_;
* \param indices the pointer to the input point cloud indices
*/
void input_indices_callback (const PointCloudConstPtr &cloud,
const PointIndicesConstPtr &indices);
private: /** \brief Synchronized input, and indices.*/
/** \brief The PCL implementation used. */ boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
pcl::ConvexHull<pcl::PointXYZ> impl_; PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>> sync_input_indices_a_;
/** \brief The input PointCloud subscriber. */ public:
ros::Subscriber sub_input_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Publisher for PolygonStamped. */
ros::Publisher pub_plane_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ #endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_

168
pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp
View File

@ -50,101 +50,103 @@
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation. /** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
* The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters. * The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters.
* Normals are estimated at each point as well and published on a separate topic. * Normals are estimated at each point as well and published on a separate topic.
* \author Radu Bogdan Rusu, Zoltan-Csaba Marton * \author Radu Bogdan Rusu, Zoltan-Csaba Marton
*/
class MovingLeastSquares : public PCLNodelet
{
typedef pcl::PointXYZ PointIn;
typedef pcl::PointNormal NormalOut;
typedef pcl::PointCloud<PointIn> PointCloudIn;
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
typedef pcl::PointCloud<NormalOut> NormalCloudOut;
typedef pcl::KdTree<PointIn> KdTree;
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
protected:
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
PointCloudInConstPtr surface_;
/** \brief A pointer to the spatial search object. */
KdTreePtr tree_;
/** \brief The nearest neighbors search radius for each point. */
double search_radius_;
/** \brief The number of K nearest neighbors to use for each point. */
//int k_;
/** \brief Whether to use a polynomial fit. */
bool use_polynomial_fit_;
/** \brief The order of the polynomial to be fit. */
int polynomial_order_;
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */
double gaussian_parameter_;
// ROS nodelet attributes
/** \brief The surface PointCloud subscriber filter. */
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
* 2: Organized spatial dataset index
*/ */
class MovingLeastSquares : public PCLNodelet int spatial_locator_type_;
{
typedef pcl::PointXYZ PointIn;
typedef pcl::PointNormal NormalOut;
typedef pcl::PointCloud<PointIn> PointCloudIn; /** \brief Pointer to a dynamic reconfigure service. */
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr; boost::shared_ptr<dynamic_reconfigure::Server<MLSConfig>> srv_;
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
typedef pcl::PointCloud<NormalOut> NormalCloudOut;
typedef pcl::KdTree<PointIn> KdTree; /** \brief Dynamic reconfigure callback
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr; * \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(MLSConfig & config, uint32_t level);
protected: /** \brief Nodelet initialization routine. */
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ virtual void onInit();
PointCloudInConstPtr surface_;
/** \brief A pointer to the spatial search object. */ /** \brief LazyNodelet connection routine. */
KdTreePtr tree_; virtual void subscribe();
virtual void unsubscribe();
/** \brief The nearest neighbors search radius for each point. */ private:
double search_radius_; /** \brief Input point cloud callback.
* \param cloud the pointer to the input point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_indices_callback(
const PointCloudInConstPtr & cloud,
const PointIndicesConstPtr & indices);
/** \brief The number of K nearest neighbors to use for each point. */ private:
//int k_; /** \brief The PCL implementation used. */
pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
/** \brief Whether to use a polynomial fit. */ /** \brief The input PointCloud subscriber. */
bool use_polynomial_fit_; ros::Subscriber sub_input_;
/** \brief The order of the polynomial to be fit. */ /** \brief The output PointCloud (containing the normals) publisher. */
int polynomial_order_; ros::Publisher pub_normals_;
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */ /** \brief Synchronized input, and indices.*/
double gaussian_parameter_; boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointIndices>>> sync_input_indices_a_;
// ROS nodelet attributes public:
/** \brief The surface PointCloud subscriber filter. */ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
message_filters::Subscriber<PointCloudIn> sub_surface_filter_; };
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
* 2: Organized spatial dataset index
*/
int spatial_locator_type_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<MLSConfig> > srv_;
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (MLSConfig &config, uint32_t level);
/** \brief Nodelet initialization routine. */
virtual void onInit ();
/** \brief LazyNodelet connection routine. */
virtual void subscribe ();
virtual void unsubscribe ();
private:
/** \brief Input point cloud callback.
* \param cloud the pointer to the input point cloud
* \param indices the pointer to the input point cloud indices
*/
void input_indices_callback (const PointCloudInConstPtr &cloud,
const PointIndicesConstPtr &indices);
private:
/** \brief The PCL implementation used. */
pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief The output PointCloud (containing the normals) publisher. */
ros::Publisher pub_normals_;
/** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointIndices> > > sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointIndices> > > sync_input_indices_a_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} }
#endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ #endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_

232
pcl_ros/include/pcl_ros/transforms.hpp
View File

@ -44,124 +44,138 @@
namespace pcl_ros namespace pcl_ros
{ {
/** \brief Transform a point cloud and rotate its normals using an Eigen transform. /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param cloud_out the input point cloud * \param cloud_out the input point cloud
* \param transform a rigid transformation from tf * \param transform a rigid transformation from tf
* \note calls the Eigen version * \note calls the Eigen version
*/ */
template <typename PointT> void template<typename PointT>
transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in, void
pcl::PointCloud <PointT> &cloud_out, transformPointCloudWithNormals(
const tf::Transform &transform); const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf::Transform & transform);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener /** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to * \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param cloud_out the input point cloud * \param cloud_out the input point cloud
* \param tf_listener a TF listener object * \param tf_listener a TF listener object
*/ */
template <typename PointT> bool template<typename PointT>
transformPointCloudWithNormals (const std::string &target_frame, bool
const pcl::PointCloud <PointT> &cloud_in, transformPointCloudWithNormals(
pcl::PointCloud <PointT> &cloud_out, const std::string & target_frame,
const tf::TransformListener &tf_listener); const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener /** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to * \param target_frame the target TF frame the point cloud should be transformed to
* \param target_time the target timestamp * \param target_time the target timestamp
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame * \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud * \param cloud_out the input point cloud
* \param tf_listener a TF listener object * \param tf_listener a TF listener object
*/ */
template <typename PointT> bool template<typename PointT>
transformPointCloudWithNormals (const std::string &target_frame, bool
const ros::Time & target_time, transformPointCloudWithNormals(
const pcl::PointCloud <PointT> &cloud_in, const std::string & target_frame,
const std::string &fixed_frame, const ros::Time & target_time,
pcl::PointCloud <PointT> &cloud_out, const pcl::PointCloud<PointT> & cloud_in,
const tf::TransformListener &tf_listener); const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener);
/** \brief Apply a rigid transform defined by a 3D offset and a quaternion /** \brief Apply a rigid transform defined by a 3D offset and a quaternion
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param cloud_out the input point cloud * \param cloud_out the input point cloud
* \param transform a rigid transformation from tf * \param transform a rigid transformation from tf
* \note calls the Eigen version * \note calls the Eigen version
*/ */
template <typename PointT> void template<typename PointT>
transformPointCloud (const pcl::PointCloud <PointT> &cloud_in, void
pcl::PointCloud <PointT> &cloud_out, transformPointCloud(
const tf::Transform &transform); const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf::Transform & transform);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener /** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to * \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param cloud_out the input point cloud * \param cloud_out the input point cloud
* \param tf_listener a TF listener object * \param tf_listener a TF listener object
*/ */
template <typename PointT> bool template<typename PointT>
transformPointCloud (const std::string &target_frame, bool
const pcl::PointCloud <PointT> &cloud_in, transformPointCloud(
pcl::PointCloud <PointT> &cloud_out, const std::string & target_frame,
const tf::TransformListener &tf_listener); const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener /** \brief Transforms a point cloud in a given target TF frame using a TransformListener
* \param target_frame the target TF frame the point cloud should be transformed to * \param target_frame the target TF frame the point cloud should be transformed to
* \param target_time the target timestamp * \param target_time the target timestamp
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame * \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud * \param cloud_out the input point cloud
* \param tf_listener a TF listener object * \param tf_listener a TF listener object
*/ */
template <typename PointT> bool template<typename PointT>
transformPointCloud (const std::string &target_frame, const ros::Time & target_time, bool
const pcl::PointCloud <PointT> &cloud_in, transformPointCloud(
const std::string &fixed_frame, const std::string & target_frame, const ros::Time & target_time,
pcl::PointCloud <PointT> &cloud_out, const pcl::PointCloud<PointT> & cloud_in,
const tf::TransformListener &tf_listener); const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener);
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame * \param target_frame the target TF frame
* \param in the input PointCloud2 dataset * \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset
* \param tf_listener a TF listener object * \param tf_listener a TF listener object
*/ */
bool bool
transformPointCloud (const std::string &target_frame, transformPointCloud(
const sensor_msgs::PointCloud2 &in, const std::string & target_frame,
sensor_msgs::PointCloud2 &out, const sensor_msgs::PointCloud2 & in,
const tf::TransformListener &tf_listener); sensor_msgs::PointCloud2 & out,
const tf::TransformListener & tf_listener);
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame * \param target_frame the target TF frame
* \param net_transform the TF transformer object * \param net_transform the TF transformer object
* \param in the input PointCloud2 dataset * \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset
*/ */
void void
transformPointCloud (const std::string &target_frame, transformPointCloud(
const tf::Transform &net_transform, const std::string & target_frame,
const sensor_msgs::PointCloud2 &in, const tf::Transform & net_transform,
sensor_msgs::PointCloud2 &out); const sensor_msgs::PointCloud2 & in,
sensor_msgs::PointCloud2 & out);
/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix. /** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
* \param transform the transformation to use on the points * \param transform the transformation to use on the points
* \param in the input PointCloud2 dataset * \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset
*/ */
void void
transformPointCloud (const Eigen::Matrix4f &transform, transformPointCloud(
const sensor_msgs::PointCloud2 &in, const Eigen::Matrix4f & transform,
sensor_msgs::PointCloud2 &out); const sensor_msgs::PointCloud2 & in,
sensor_msgs::PointCloud2 & out);
/** \brief Obtain the transformation matrix from TF into an Eigen form /** \brief Obtain the transformation matrix from TF into an Eigen form
* \param bt the TF transformation * \param bt the TF transformation
* \param out_mat the Eigen transformation * \param out_mat the Eigen transformation
*/ */
void void
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat); transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat);
} }
#endif // PCL_ROS_TRANSFORMS_H_ #endif // PCL_ROS_TRANSFORMS_H_

29
pcl_ros/src/pcl_ros/features/boundary.cpp
View File

@ -39,35 +39,36 @@
#include "pcl_ros/features/boundary.hpp" #include "pcl_ros/features/boundary.hpp"
void void
pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::BoundaryEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::BoundaryEstimation BoundaryEstimation; typedef pcl_ros::BoundaryEstimation BoundaryEstimation;

686
pcl_ros/src/pcl_ros/features/feature.cpp
View File

@ -53,219 +53,263 @@
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Feature::onInit () pcl_ros::Feature::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
// Call the child init // Call the child init
childInit (*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
{ NODELET_ERROR(
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 ()); "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
getName().c_str());
return; return;
} }
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("use_surface", use_surface_); pnh_->getParam("use_surface", use_surface_);
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &Feature::config_callback, this, _1, _2);
srv_->setCallback(f);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - use_surface : %s\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - k_search : %d\n" " - use_surface : %s\n"
" - radius_search : %f\n" " - k_search : %d\n"
" - spatial_locator: %d", " - radius_search : %f\n"
getName ().c_str (), " - spatial_locator: %d",
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); getName().c_str(),
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
onInitPostProcess (); onInitPostProcess();
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Feature::subscribe () pcl_ros::Feature::subscribe()
{ {
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
if (use_indices_ || use_surface_) if (use_indices_ || use_surface_) {
{
// Create the objects here // Create the objects here
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_); sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
else PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_); } else {
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudIn, PointIndices>>>(max_queue_size_);
}
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
if (use_indices_) if (use_indices_) {
{
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) // Use both indices and surface if (use_surface_) { // Use both indices and surface
{
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // 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_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); sync_input_surface_indices_a_->connectInput(
else sub_input_filter_, sub_surface_filter_,
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); sub_indices_filter_);
} } else {
else // Use only indices sync_input_surface_indices_e_->connectInput(
{ sub_input_filter_, sub_surface_filter_,
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); 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 // surface not enabled, connect the input-indices duo and register
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); sync_input_surface_indices_a_->connectInput(
else sub_input_filter_, nf_pc_,
sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); sub_indices_filter_);
} else {
sync_input_surface_indices_e_->connectInput(
sub_input_filter_, nf_pc_,
sub_indices_filter_);
}
} }
} } else { // Use only surface
else // Use only surface sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
{
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
// indices not enabled, connect the input-surface duo and register // indices not enabled, connect the input-surface duo and register
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
else } else {
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
}
} }
// Register callbacks // Register callbacks
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); sync_input_surface_indices_a_->registerCallback(
else bind(
sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); &Feature::input_surface_indices_callback,
} this, _1, _2, _3));
else } else {
// Subscribe in an old fashion to input only (no filters) sync_input_surface_indices_e_->registerCallback(
sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); bind(
} &Feature::input_surface_indices_callback,
this, _1, _2, _3));
////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Feature::unsubscribe ()
{
if (use_indices_ || use_surface_)
{
sub_input_filter_.unsubscribe ();
if (use_indices_)
{
sub_indices_filter_.unsubscribe ();
if (use_surface_)
sub_surface_filter_.unsubscribe ();
} }
else } else {
sub_surface_filter_.unsubscribe (); // Subscribe in an old fashion to input only (no filters)
sub_input_ =
pnh_->subscribe<PointCloudIn>(
"input", max_queue_size_,
bind(
&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
PointIndicesConstPtr()));
} }
else
sub_input_.shutdown ();
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level) pcl_ros::Feature::unsubscribe()
{ {
if (k_ != config.k_search) if (use_indices_ || use_surface_) {
{ sub_input_filter_.unsubscribe();
k_ = config.k_search; if (use_indices_) {
NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); sub_indices_filter_.unsubscribe();
} if (use_surface_) {
if (search_radius_ != config.radius_search) sub_surface_filter_.unsubscribe();
{ }
search_radius_ = config.radius_search; } else {
NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_); sub_surface_filter_.unsubscribe();
}
} else {
sub_input_.shutdown();
} }
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) {
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 // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0) {
return; return;
}
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid (cloud)) if (!isValid(cloud)) {
{ NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
// If surface is given, check if it's valid // If surface is given, check if it's valid
if (cloud_surface && !isValid (cloud_surface, "surface")) if (cloud_surface && !isValid(cloud_surface, "surface")) {
{ NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid(indices)) {
{ NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
/// DEBUG /// DEBUG
if (cloud_surface) if (cloud_surface) {
if (indices) if (indices) {
NODELET_DEBUG ("[input_surface_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[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.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
else "input").c_str(),
NODELET_DEBUG ("[input_surface_indices_callback]\n" cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" *cloud_surface).c_str(), fromPCL(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", cloud_surface->header).stamp.toSec(),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ()); indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
else } else {
if (indices) NODELET_DEBUG(
NODELET_DEBUG ("[input_surface_indices_callback]\n" "[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.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
else "input").c_str(),
NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
*cloud_surface).c_str(), fromPCL(
cloud_surface->header).stamp.toSec(),
cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
}
} else if (indices) {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
}
/// ///
if ((int)(cloud->width * cloud->height) < k_) if ((int)(cloud->width * cloud->height) < k_) {
{ NODELET_ERROR(
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)); "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_,
emptyPublish (cloud); (int)(cloud->width * cloud->height));
emptyPublish(cloud);
return; return;
} }
// If indices given... // If indices given...
IndicesPtr vindices; IndicesPtr vindices;
if (indices && !indices->header.frame_id.empty ()) if (indices && !indices->header.frame_id.empty()) {
vindices.reset (new std::vector<int> (indices->indices)); vindices.reset(new std::vector<int>(indices->indices));
}
computePublish (cloud, cloud_surface, vindices); computePublish(cloud, cloud_surface, vindices);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
@ -274,223 +318,289 @@ pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cl
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::FeatureFromNormals::onInit () pcl_ros::FeatureFromNormals::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
// Call the child init // Call the child init
childInit (*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
{ NODELET_ERROR(
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 ()); "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
getName().c_str());
return; return;
} }
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("use_surface", use_surface_); pnh_->getParam("use_surface", use_surface_);
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &FeatureFromNormals::config_callback, this, _1, _2);
srv_->setCallback(f);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - use_surface : %s\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - k_search : %d\n" " - use_surface : %s\n"
" - radius_search : %f\n" " - k_search : %d\n"
" - spatial_locator: %d", " - radius_search : %f\n"
getName ().c_str (), " - spatial_locator: %d",
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); getName().c_str(),
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
onInitPostProcess (); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::FeatureFromNormals::subscribe () pcl_ros::FeatureFromNormals::subscribe()
{ {
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
// Create the objects here // Create the objects here
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_); sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
else PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_); } else {
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
}
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
if (use_indices_ || use_surface_) if (use_indices_ || use_surface_) {
{ if (use_indices_) {
if (use_indices_)
{
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) // Use both indices and surface if (use_surface_) { // Use both indices and surface
{
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // 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_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_,
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); sub_normals_filter_,
} sub_surface_filter_,
else // Use only indices sub_indices_filter_);
{ } else {
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); sync_input_normals_surface_indices_e_->connectInput(
if (approximate_sync_) 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 // 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_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
} else {
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
}
} }
} } else { // Use only surface
else // Use only surface
{
// indices not enabled, connect the input-surface duo and register // indices not enabled, connect the input-surface duo and register
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_, sub_normals_filter_,
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); 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 {
else sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
{
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_, sub_normals_filter_,
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); nf_pc_, nf_pi_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_, sub_normals_filter_,
nf_pc_, nf_pi_);
}
} }
// Register callbacks // Register callbacks
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); sync_input_normals_surface_indices_a_->registerCallback(
else bind(
sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); &FeatureFromNormals::
} input_normals_surface_indices_callback, this, _1, _2, _3, _4));
} else {
////////////////////////////////////////////////////////////////////////////////////////////// sync_input_normals_surface_indices_e_->registerCallback(
void bind(
pcl_ros::FeatureFromNormals::unsubscribe () &FeatureFromNormals::
{ input_normals_surface_indices_callback, this, _1, _2, _3, _4));
sub_input_filter_.unsubscribe ();
sub_normals_filter_.unsubscribe ();
if (use_indices_ || use_surface_)
{
if (use_indices_)
{
sub_indices_filter_.unsubscribe ();
if (use_surface_)
sub_surface_filter_.unsubscribe ();
}
else
sub_surface_filter_.unsubscribe ();
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( pcl_ros::FeatureFromNormals::unsubscribe()
const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, {
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) sub_input_filter_.unsubscribe();
sub_normals_filter_.unsubscribe();
if (use_indices_ || use_surface_) {
if (use_indices_) {
sub_indices_filter_.unsubscribe();
if (use_surface_) {
sub_surface_filter_.unsubscribe();
}
} else {
sub_surface_filter_.unsubscribe();
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
{ {
// No subscribers, no work // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0) {
return; return;
}
// If cloud+normals is given, check if it's valid // If cloud+normals is given, check if it's valid
if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
{ NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
// If surface is given, check if it's valid // If surface is given, check if it's valid
if (cloud_surface && !isValid (cloud_surface, "surface")) if (cloud_surface && !isValid(cloud_surface, "surface")) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ()); "[%s::input_normals_surface_indices_callback] Invalid input surface!",
emptyPublish (cloud); getName().c_str());
emptyPublish(cloud);
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid(indices)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ()); "[%s::input_normals_surface_indices_callback] Invalid input indices!",
emptyPublish (cloud); getName().c_str());
emptyPublish(cloud);
return; return;
} }
/// DEBUG /// DEBUG
if (cloud_surface) if (cloud_surface) {
if (indices) if (indices) {
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%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" " - 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.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), getName().c_str(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); "input").c_str(),
else cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" *cloud_surface).c_str(), fromPCL(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" cloud_surface->header).stamp.toSec(),
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
getName ().c_str (), *cloud_normals).c_str(), fromPCL(
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), cloud_normals->header).stamp.toSec(),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (), cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); indices->indices.size(), indices->header.stamp.toSec(),
else indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
if (indices) } else {
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%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.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), getName().c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
else "input").c_str(),
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" *cloud_surface).c_str(), fromPCL(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", cloud_surface->header).stamp.toSec(),
getName ().c_str (), cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ()); *cloud_normals).c_str(), fromPCL(
cloud_normals->header).stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
}
} else if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), fromPCL(
cloud_normals->header).stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), fromPCL(
cloud_normals->header).stamp.toSec(),
cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
}
/// ///
if ((int)(cloud->width * cloud->height) < k_) if ((int)(cloud->width * cloud->height) < k_) {
{ NODELET_ERROR(
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)); "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!",
emptyPublish (cloud); getName().c_str(), k_, (int)(cloud->width * cloud->height));
emptyPublish(cloud);
return; return;
} }
// If indices given... // If indices given...
IndicesPtr vindices; IndicesPtr vindices;
if (indices && !indices->header.frame_id.empty ()) if (indices && !indices->header.frame_id.empty()) {
vindices.reset (new std::vector<int> (indices->indices)); vindices.reset(new std::vector<int>(indices->indices));
}
computePublish (cloud, cloud_normals, cloud_surface, vindices); computePublish(cloud, cloud_normals, cloud_surface, vindices);
} }

30
pcl_ros/src/pcl_ros/features/fpfh.cpp
View File

@ -39,38 +39,38 @@
#include "pcl_ros/features/fpfh.hpp" #include "pcl_ros/features/fpfh.hpp"
void void
pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::FPFHEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::FPFHEstimation FPFHEstimation; typedef pcl_ros::FPFHEstimation FPFHEstimation;
PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet)

30
pcl_ros/src/pcl_ros/features/fpfh_omp.cpp
View File

@ -39,38 +39,38 @@
#include "pcl_ros/features/fpfh_omp.hpp" #include "pcl_ros/features/fpfh_omp.hpp"
void void
pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::FPFHEstimationOMP::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet)

26
pcl_ros/src/pcl_ros/features/moment_invariants.cpp
View File

@ -39,36 +39,36 @@
#include "pcl_ros/features/moment_invariants.hpp" #include "pcl_ros/features/moment_invariants.hpp"
void void
pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::MomentInvariantsEstimation::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet)

26
pcl_ros/src/pcl_ros/features/normal_3d.cpp
View File

@ -39,36 +39,36 @@
#include "pcl_ros/features/normal_3d.hpp" #include "pcl_ros/features/normal_3d.hpp"
void void
pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::NormalEstimation::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::NormalEstimation NormalEstimation; typedef pcl_ros::NormalEstimation NormalEstimation;
PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet)

26
pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp
View File

@ -39,36 +39,36 @@
#include "pcl_ros/features/normal_3d_omp.hpp" #include "pcl_ros/features/normal_3d_omp.hpp"
void void
pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::NormalEstimationOMP::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet)

30
pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp
View File

@ -41,41 +41,41 @@
#if defined HAVE_TBB #if defined HAVE_TBB
void void
pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloud output; PointCloud output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::NormalEstimationTBB::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet)
#endif // HAVE_TBB #endif // HAVE_TBB

30
pcl_ros/src/pcl_ros/features/pfh.cpp
View File

@ -39,38 +39,38 @@
#include "pcl_ros/features/pfh.hpp" #include "pcl_ros/features/pfh.hpp"
void void
pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::PFHEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::PFHEstimation PFHEstimation; typedef pcl_ros::PFHEstimation PFHEstimation;
PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet)

30
pcl_ros/src/pcl_ros/features/principal_curvatures.cpp
View File

@ -39,38 +39,38 @@
#include "pcl_ros/features/principal_curvatures.hpp" #include "pcl_ros/features/principal_curvatures.hpp"
void void
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::PrincipalCurvaturesEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet)

30
pcl_ros/src/pcl_ros/features/shot.cpp
View File

@ -38,38 +38,38 @@
#include "pcl_ros/features/shot.hpp" #include "pcl_ros/features/shot.hpp"
void void
pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::SHOTEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::SHOTEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::SHOTEstimation SHOTEstimation; typedef pcl_ros::SHOTEstimation SHOTEstimation;
PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet)

30
pcl_ros/src/pcl_ros/features/shot_omp.cpp
View File

@ -38,38 +38,38 @@
#include "pcl_ros/features/shot_omp.hpp" #include "pcl_ros/features/shot_omp.hpp"
void void
pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::SHOTEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::SHOTEstimationOMP::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP; typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP;
PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet)

30
pcl_ros/src/pcl_ros/features/vfh.cpp
View File

@ -39,38 +39,38 @@
#include "pcl_ros/features/vfh.hpp" #include "pcl_ros/features/vfh.hpp"
void void
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
void void
pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::VFHEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Set the inputs // Set the inputs
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (pcl_ptr(surface)); impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals)); impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::VFHEstimation VFHEstimation; typedef pcl_ros::VFHEstimation VFHEstimation;
PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet)

66
pcl_ros/src/pcl_ros/filters/crop_box.cpp
View File

@ -41,24 +41,25 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
pcl_ros::CropBox::child_init (ros::NodeHandle &nh, bool &has_service) pcl_ros::CropBox::child_init(ros::NodeHandle & nh, bool & has_service)
{ {
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
has_service = true; has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::CropBoxConfig> > (nh); srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>::CallbackType f = boost::bind (&CropBox::config_callback, this, _1, _2); dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &CropBox::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t level) pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t level)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock(mutex_);
Eigen::Vector4f min_point,max_point; Eigen::Vector4f min_point, max_point;
min_point = impl_.getMin(); min_point = impl_.getMin();
max_point = impl_.getMax(); max_point = impl_.getMax();
@ -67,49 +68,54 @@ pcl_ros::CropBox::config_callback (pcl_ros::CropBoxConfig &config, uint32_t leve
new_max_point << config.max_x, config.max_y, config.max_z, 0.0; new_max_point << config.max_x, config.max_y, config.max_z, 0.0;
// Check the current values for minimum point // Check the current values for minimum point
if (min_point != new_min_point) if (min_point != new_min_point) {
{ NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the minimum point to: %f %f %f.", getName ().c_str (), new_min_point(0),new_min_point(1),new_min_point(2)); "[%s::config_callback] Setting the minimum point to: %f %f %f.",
getName().c_str(), new_min_point(0), new_min_point(1), new_min_point(2));
// Set the filter min point if different // Set the filter min point if different
impl_.setMin(new_min_point); impl_.setMin(new_min_point);
} }
// Check the current values for the maximum point // Check the current values for the maximum point
if (max_point != new_max_point) if (max_point != new_max_point) {
{ NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the maximum point to: %f %f %f.", getName ().c_str (), new_max_point(0),new_max_point(1),new_max_point(2)); "[%s::config_callback] Setting the maximum point to: %f %f %f.",
getName().c_str(), new_max_point(0), new_max_point(1), new_max_point(2));
// Set the filter max point if different // Set the filter max point if different
impl_.setMax(new_max_point); impl_.setMax(new_max_point);
} }
// Check the current value for keep_organized // Check the current value for keep_organized
if (impl_.getKeepOrganized () != config.keep_organized) if (impl_.getKeepOrganized() != config.keep_organized) {
{ NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); "[%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 // Call the virtual method in the child
impl_.setKeepOrganized (config.keep_organized); impl_.setKeepOrganized(config.keep_organized);
} }
// Check the current value for the negative flag // Check the current value for the negative flag
if (impl_.getNegative() != config.negative) if (impl_.getNegative() != config.negative) {
{ NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.negative ? "true" : "false"); "[%s::config_callback] Setting the filter negative flag to: %s.",
getName().c_str(), config.negative ? "true" : "false");
// Call the virtual method in the child // Call the virtual method in the child
impl_.setNegative(config.negative); impl_.setNegative(config.negative);
} }
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame) if (tf_input_frame_ != config.input_frame) {
{
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); NODELET_DEBUG(
"[%s::config_callback] Setting the input TF frame to: %s.",
getName().c_str(), tf_input_frame_.c_str());
} }
if (tf_output_frame_ != config.output_frame) if (tf_output_frame_ != config.output_frame) {
{
tf_output_frame_ = config.output_frame; tf_output_frame_ = config.output_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); NODELET_DEBUG(
"[%s::config_callback] Setting the output TF frame to: %s.",
getName().c_str(), tf_output_frame_.c_str());
} }
} }
typedef pcl_ros::CropBox CropBox; typedef pcl_ros::CropBox CropBox;
PLUGINLIB_EXPORT_CLASS(CropBox,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet);

27
pcl_ros/src/pcl_ros/filters/extract_indices.cpp
View File

@ -40,31 +40,32 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
pcl_ros::ExtractIndices::child_init (ros::NodeHandle &nh, bool &has_service) pcl_ros::ExtractIndices::child_init(ros::NodeHandle & nh, bool & has_service)
{ {
has_service = true; has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > (nh); srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind (&ExtractIndices::config_callback, this, _1, _2); dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &ExtractIndices::config_callback, this, _1, _2);
srv_->setCallback(f);
use_indices_ = true; use_indices_ = true;
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ExtractIndices::config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level) pcl_ros::ExtractIndices::config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock(mutex_);
if (impl_.getNegative () != config.negative) if (impl_.getNegative() != config.negative) {
{ impl_.setNegative(config.negative);
impl_.setNegative (config.negative); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the extraction to: %s.", getName ().c_str (), (config.negative ? "indices" : "everything but the indices")); "[%s::config_callback] Setting the extraction to: %s.", getName().c_str(),
(config.negative ? "indices" : "everything but the indices"));
} }
} }
typedef pcl_ros::ExtractIndices ExtractIndices; typedef pcl_ros::ExtractIndices ExtractIndices;
PLUGINLIB_EXPORT_CLASS(ExtractIndices,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet);

35
pcl_ros/src/pcl_ros/filters/features/boundary.cpp
View File

@ -39,39 +39,40 @@
#include "pcl_ros/features/boundary.hpp" #include "pcl_ros/features/boundary.hpp"
void void
pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::BoundaryEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
impl_.setInputNormals (normals); impl_.setInputNormals(normals);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::BoundaryEstimation BoundaryEstimation; typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
PLUGINLIB_EXPORT_CLASS(BoundaryEstimation,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet);

600
pcl_ros/src/pcl_ros/filters/features/feature.cpp
View File

@ -53,192 +53,237 @@
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Feature::onInit () pcl_ros::Feature::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
// Call the child init // Call the child init
childInit (*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
{ NODELET_ERROR(
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 ()); "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
getName().c_str());
return; return;
} }
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("use_surface", use_surface_); pnh_->getParam("use_surface", use_surface_);
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &Feature::config_callback, this, _1, _2);
srv_->setCallback(f);
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
if (use_indices_ || use_surface_) if (use_indices_ || use_surface_) {
{
// Create the objects here // Create the objects here
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_); sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
else PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_); } else {
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudIn, PointIndices>>>(max_queue_size_);
}
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
if (use_indices_) if (use_indices_) {
{
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) // Use both indices and surface if (use_surface_) { // Use both indices and surface
{
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // 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_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); sync_input_surface_indices_a_->connectInput(
else sub_input_filter_, sub_surface_filter_,
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_); sub_indices_filter_);
} } else {
else // Use only indices sync_input_surface_indices_e_->connectInput(
{ sub_input_filter_, sub_surface_filter_,
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1)); 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 // surface not enabled, connect the input-indices duo and register
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); sync_input_surface_indices_a_->connectInput(
else sub_input_filter_, nf_pc_,
sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_); sub_indices_filter_);
} else {
sync_input_surface_indices_e_->connectInput(
sub_input_filter_, nf_pc_,
sub_indices_filter_);
}
} }
} } else { // Use only surface
else // Use only surface sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
{
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
// indices not enabled, connect the input-surface duo and register // indices not enabled, connect the input-surface duo and register
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
else } else {
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_); sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
}
} }
// Register callbacks // Register callbacks
if (approximate_sync_) if (approximate_sync_) {
sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); sync_input_surface_indices_a_->registerCallback(
else bind(
sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3)); &Feature::input_surface_indices_callback,
} this, _1, _2, _3));
else } 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) // Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); sub_input_ =
pnh_->subscribe<PointCloudIn>(
"input", max_queue_size_,
bind(
&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
PointIndicesConstPtr()));
}
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - use_surface : %s\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - k_search : %d\n" " - use_surface : %s\n"
" - radius_search : %f\n" " - k_search : %d\n"
" - spatial_locator: %d", " - radius_search : %f\n"
getName ().c_str (), " - spatial_locator: %d",
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); getName().c_str(),
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level) pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
{ {
if (k_ != config.k_search) if (k_ != config.k_search) {
{
k_ = config.k_search; k_ = config.k_search;
NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_); NODELET_DEBUG(
"[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
} }
if (search_radius_ != config.radius_search) if (search_radius_ != config.radius_search) {
{
search_radius_ = config.radius_search; search_radius_ = config.radius_search;
NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_); NODELET_DEBUG(
"[config_callback] Setting the nearest neighbors search radius for each point: %f.",
search_radius_);
} }
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, pcl_ros::Feature::input_surface_indices_callback(
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
{ {
// No subscribers, no work // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0) {
return; return;
}
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid (cloud)) if (!isValid(cloud)) {
{ NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
// If surface is given, check if it's valid // If surface is given, check if it's valid
if (cloud_surface && !isValid (cloud_surface, "surface")) if (cloud_surface && !isValid(cloud_surface, "surface")) {
{ NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid(indices)) {
{ NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
/// DEBUG /// DEBUG
if (cloud_surface) if (cloud_surface) {
if (indices) if (indices) {
NODELET_DEBUG ("[input_surface_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[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.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
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->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
else "input").c_str(),
NODELET_DEBUG ("[input_surface_indices_callback]\n" cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" *cloud_surface).c_str(),
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", cloud_surface->header.stamp.toSec(),
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->header.frame_id.c_str(), pnh_->resolveName("surface").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 } else {
if (indices) NODELET_DEBUG(
NODELET_DEBUG ("[input_surface_indices_callback]\n" "[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.", " - 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->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
else "input").c_str(),
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 ()); 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_) if ((int)(cloud->width * cloud->height) < k_) {
{ NODELET_ERROR(
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)); "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_,
emptyPublish (cloud); (int)(cloud->width * cloud->height));
emptyPublish(cloud);
return; return;
} }
// If indices given... // If indices given...
IndicesPtr vindices; IndicesPtr vindices;
if (indices && !indices->header.frame_id.empty ()) if (indices && !indices->header.frame_id.empty()) {
vindices.reset (new std::vector<int> (indices->indices)); vindices.reset(new std::vector<int>(indices->indices));
}
computePublish (cloud, cloud_surface, vindices); computePublish(cloud, cloud_surface, vindices);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
@ -247,197 +292,264 @@ pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cl
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::FeatureFromNormals::onInit () pcl_ros::FeatureFromNormals::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
// Call the child init // Call the child init
childInit (*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
{ NODELET_ERROR(
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 ()); "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
getName().c_str());
return; return;
} }
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("use_surface", use_surface_); pnh_->getParam("use_surface", use_surface_);
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_); sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &FeatureFromNormals::config_callback, this, _1, _2);
srv_->setCallback(f);
// Create the objects here // Create the objects here
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_); sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
else PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_); } else {
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
}
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
if (use_indices_ || use_surface_) if (use_indices_ || use_surface_) {
{ if (use_indices_) {
if (use_indices_)
{
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) // Use both indices and surface if (use_surface_) { // Use both indices and surface
{
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // 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_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_,
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_); sub_normals_filter_,
} sub_surface_filter_,
else // Use only indices sub_indices_filter_);
{ } else {
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); sync_input_normals_surface_indices_e_->connectInput(
if (approximate_sync_) 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 // 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_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
} else {
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_); sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_,
sub_normals_filter_, nf_pc_,
sub_indices_filter_);
}
} }
} } else { // Use only surface
else // Use only surface
{
// indices not enabled, connect the input-surface duo and register // indices not enabled, connect the input-surface duo and register
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1)); sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_, sub_normals_filter_,
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_); 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 {
else sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
{
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); sync_input_normals_surface_indices_a_->connectInput(
else sub_input_filter_, sub_normals_filter_,
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_); nf_pc_, nf_pi_);
} else {
sync_input_normals_surface_indices_e_->connectInput(
sub_input_filter_, sub_normals_filter_,
nf_pc_, nf_pi_);
}
} }
// Register callbacks // Register callbacks
if (approximate_sync_) if (approximate_sync_) {
sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); sync_input_normals_surface_indices_a_->registerCallback(
else bind(
sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4)); &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" NODELET_DEBUG(
" - use_surface : %s\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - k_search : %d\n" " - use_surface : %s\n"
" - radius_search : %f\n" " - k_search : %d\n"
" - spatial_locator: %d", " - radius_search : %f\n"
getName ().c_str (), " - spatial_locator: %d",
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); getName().c_str(),
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback ( pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices) const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
{ {
// No subscribers, no work // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0) {
return; return;
}
// If cloud+normals is given, check if it's valid // If cloud+normals is given, check if it's valid
if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
{ NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ()); emptyPublish(cloud);
emptyPublish (cloud);
return; return;
} }
// If surface is given, check if it's valid // If surface is given, check if it's valid
if (cloud_surface && !isValid (cloud_surface, "surface")) if (cloud_surface && !isValid(cloud_surface, "surface")) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ()); "[%s::input_normals_surface_indices_callback] Invalid input surface!",
emptyPublish (cloud); getName().c_str());
emptyPublish(cloud);
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid(indices)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ()); "[%s::input_normals_surface_indices_callback] Invalid input indices!",
emptyPublish (cloud); getName().c_str());
emptyPublish(cloud);
return; return;
} }
/// DEBUG /// DEBUG
if (cloud_surface) if (cloud_surface) {
if (indices) if (indices) {
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%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" " - 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.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - 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 (), getName().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->width * cloud->height, pcl::getFieldsList(*cloud).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 (), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); "input").c_str(),
else cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" *cloud_surface).c_str(),
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" cloud_surface->header.stamp.toSec(),
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
getName ().c_str (), *cloud_normals).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->header.stamp.toSec(),
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->header.frame_id.c_str(), pnh_->resolveName("normals").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(),
else indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
if (indices) } else {
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%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.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - 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 (), getName().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 (), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
else "input").c_str(),
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n" cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" *cloud_surface).c_str(),
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", cloud_surface->header.stamp.toSec(),
getName ().c_str (), cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").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->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 ()); *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_) if ((int)(cloud->width * cloud->height) < k_) {
{ NODELET_ERROR(
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)); "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!",
emptyPublish (cloud); getName().c_str(), k_, (int)(cloud->width * cloud->height));
emptyPublish(cloud);
return; return;
} }
// If indices given... // If indices given...
IndicesPtr vindices; IndicesPtr vindices;
if (indices && !indices->header.frame_id.empty ()) if (indices && !indices->header.frame_id.empty()) {
vindices.reset (new std::vector<int> (indices->indices)); vindices.reset(new std::vector<int>(indices->indices));
}
computePublish (cloud, cloud_normals, cloud_surface, vindices); computePublish(cloud, cloud_normals, cloud_surface, vindices);
} }

36
pcl_ros/src/pcl_ros/filters/features/fpfh.cpp
View File

@ -39,41 +39,41 @@
#include "pcl_ros/features/fpfh.hpp" #include "pcl_ros/features/fpfh.hpp"
void void
pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::FPFHEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
impl_.setInputNormals (normals); impl_.setInputNormals(normals);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::FPFHEstimation FPFHEstimation; typedef pcl_ros::FPFHEstimation FPFHEstimation;
PLUGINLIB_EXPORT_CLASS(FPFHEstimation,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet);

36
pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp
View File

@ -39,41 +39,41 @@
#include "pcl_ros/features/fpfh_omp.hpp" #include "pcl_ros/features/fpfh_omp.hpp"
void void
pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::FPFHEstimationOMP::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
impl_.setInputNormals (normals); impl_.setInputNormals(normals);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP; typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet);

32
pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp
View File

@ -39,39 +39,39 @@
#include "pcl_ros/features/moment_invariants.hpp" #include "pcl_ros/features/moment_invariants.hpp"
void void
pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::MomentInvariantsEstimation::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation; typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet);

32
pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp
View File

@ -39,39 +39,39 @@
#include "pcl_ros/features/normal_3d.hpp" #include "pcl_ros/features/normal_3d.hpp"
void void
pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::NormalEstimation::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::NormalEstimation NormalEstimation; typedef pcl_ros::NormalEstimation NormalEstimation;
PLUGINLIB_EXPORT_CLASS(NormalEstimation,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet);

32
pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp
View File

@ -39,39 +39,39 @@
#include "pcl_ros/features/normal_3d_omp.hpp" #include "pcl_ros/features/normal_3d_omp.hpp"
void void
pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::NormalEstimationOMP::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP; typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet);

32
pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp
View File

@ -41,41 +41,41 @@
#if defined HAVE_TBB #if defined HAVE_TBB
void void
pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloud output; PointCloud output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::NormalEstimationTBB::computePublish(
const PointCloudInConstPtr &surface, const PointCloudInConstPtr & cloud,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB; typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet);
#endif // HAVE_TBB #endif // HAVE_TBB

36
pcl_ros/src/pcl_ros/filters/features/pfh.cpp
View File

@ -39,41 +39,41 @@
#include "pcl_ros/features/pfh.hpp" #include "pcl_ros/features/pfh.hpp"
void void
pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::PFHEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
impl_.setInputNormals (normals); impl_.setInputNormals(normals);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::PFHEstimation PFHEstimation; typedef pcl_ros::PFHEstimation PFHEstimation;
PLUGINLIB_EXPORT_CLASS(PFHEstimation,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet);

36
pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp
View File

@ -39,41 +39,41 @@
#include "pcl_ros/features/principal_curvatures.hpp" #include "pcl_ros/features/principal_curvatures.hpp"
void void
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::PrincipalCurvaturesEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
impl_.setInputNormals (normals); impl_.setInputNormals(normals);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation; typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet);

36
pcl_ros/src/pcl_ros/filters/features/vfh.cpp
View File

@ -39,41 +39,41 @@
#include "pcl_ros/features/vfh.hpp" #include "pcl_ros/features/vfh.hpp"
void void
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud) pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{ {
PointCloudOut output; PointCloudOut output;
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
void void
pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud, pcl_ros::VFHEstimation::computePublish(
const PointCloudNConstPtr &normals, const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr &surface, const PointCloudNConstPtr & normals,
const IndicesPtr &indices) const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{ {
// Set the parameters // Set the parameters
impl_.setKSearch (k_); impl_.setKSearch(k_);
impl_.setRadiusSearch (search_radius_); impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator // Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_); initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_); impl_.setSearchMethod(tree_);
// Set the inputs // Set the inputs
impl_.setInputCloud (cloud); impl_.setInputCloud(cloud);
impl_.setIndices (indices); impl_.setIndices(indices);
impl_.setSearchSurface (surface); impl_.setSearchSurface(surface);
impl_.setInputNormals (normals); impl_.setInputNormals(normals);
// Estimate the feature // Estimate the feature
PointCloudOut output; PointCloudOut output;
impl_.compute (output); impl_.compute(output);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (output.makeShared ()); pub_output_.publish(output.makeShared());
} }
typedef pcl_ros::VFHEstimation VFHEstimation; typedef pcl_ros::VFHEstimation VFHEstimation;
PLUGINLIB_EXPORT_CLASS(VFHEstimation,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet);

194
pcl_ros/src/pcl_ros/filters/filter.cpp
View File

@ -61,45 +61,52 @@
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices) pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices)
{ {
PointCloud2 output; PointCloud2 output;
// Call the virtual method in the child // Call the virtual method in the child
filter (input, indices, output); filter(input, indices, output);
PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default PointCloud2::Ptr cloud_tf(new PointCloud2(output)); // set the output by default
// Check whether the user has given a different output TF frame // Check whether the user has given a different output TF frame
if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_) if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) {
{ NODELET_DEBUG(
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 ()); "[%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 // Convert the cloud into the different frame
PointCloud2 cloud_transformed; PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_)) if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_listener_)) {
{ NODELET_ERROR(
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 ()); "[%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; return;
} }
cloud_tf.reset (new PointCloud2 (cloud_transformed)); cloud_tf.reset(new PointCloud2(cloud_transformed));
} }
if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_) 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 // no tf_output_frame given, transform the dataset to its original frame
{ NODELET_DEBUG(
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 ()); "[%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 // Convert the cloud into the different frame
PointCloud2 cloud_transformed; PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_)) if (!pcl_ros::transformPointCloud(
tf_input_orig_frame_, output, cloud_transformed,
tf_listener_))
{ {
NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ()); NODELET_ERROR(
"[%s::computePublish] Error converting output dataset from %s back to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
return; return;
} }
cloud_tf.reset (new PointCloud2 (cloud_transformed)); cloud_tf.reset(new PointCloud2(cloud_transformed));
} }
// Copy timestamp to keep it // Copy timestamp to keep it
cloud_tf->header.stamp = input->header.stamp; cloud_tf->header.stamp = input->header.stamp;
// Publish a boost shared ptr // Publish a boost shared ptr
pub_output_.publish (cloud_tf); pub_output_.publish(cloud_tf);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
@ -107,141 +114,152 @@ void
pcl_ros::Filter::subscribe() pcl_ros::Filter::subscribe()
{ {
// If we're supposed to look for PointIndices (indices) // If we're supposed to look for PointIndices (indices)
if (use_indices_) if (use_indices_) {
{
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
{ sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_); pcl_msgs::PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
pcl_msgs::PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
} }
else } else {
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
}
}
else
// Subscribe in an old fashion to input only (no filters) // Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ())); sub_input_ =
pnh_->subscribe<sensor_msgs::PointCloud2>(
"input", max_queue_size_,
bind(&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr()));
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::unsubscribe() pcl_ros::Filter::unsubscribe()
{ {
if (use_indices_) if (use_indices_) {
{
sub_input_filter_.unsubscribe(); sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe(); sub_indices_filter_.unsubscribe();
} } else {
else
sub_input_.shutdown(); sub_input_.shutdown();
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::onInit () pcl_ros::Filter::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
// Call the child's local init // Call the child's local init
bool has_service = false; bool has_service = false;
if (!child_init (*pnh_, has_service)) if (!child_init(*pnh_, has_service)) {
{ NODELET_ERROR("[%s::onInit] Initialization failed.", getName().c_str());
NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
return; return;
} }
pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
if (!has_service) if (!has_service) {
{ srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(*pnh_);
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_); dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2); &Filter::config_callback, this, _1, _2);
srv_->setCallback (f); srv_->setCallback(f);
} }
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ()); NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str());
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level) pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
{ {
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame) if (tf_input_frame_ != config.input_frame) {
{
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); NODELET_DEBUG(
"[%s::config_callback] Setting the input TF frame to: %s.",
getName().c_str(), tf_input_frame_.c_str());
} }
if (tf_output_frame_ != config.output_frame) if (tf_output_frame_ != config.output_frame) {
{
tf_output_frame_ = config.output_frame; tf_output_frame_ = config.output_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); NODELET_DEBUG(
"[%s::config_callback] Setting the output TF frame to: %s.",
getName().c_str(), tf_output_frame_.c_str());
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices) pcl_ros::Filter::input_indices_callback(
const PointCloud2::ConstPtr & cloud,
const PointIndicesConstPtr & indices)
{ {
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid (cloud)) if (!isValid(cloud)) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid(indices)) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
return; return;
} }
/// DEBUG /// DEBUG
if (indices) if (indices) {
NODELET_DEBUG ("[%s::input_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%s::input_indices_callback]\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - 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 (), getName().c_str(),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
else cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
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 ()); "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 // Check whether the user has given a different input TF frame
tf_input_orig_frame_ = cloud->header.frame_id; tf_input_orig_frame_ = cloud->header.frame_id;
PointCloud2::ConstPtr cloud_tf; PointCloud2::ConstPtr cloud_tf;
if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) {
{ NODELET_DEBUG(
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 ()); "[%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 // Save the original frame ID
// Convert the cloud into the different frame // Convert the cloud into the different frame
PointCloud2 cloud_transformed; PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) {
{ NODELET_ERROR(
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 ()); "[%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; return;
} }
cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed); cloud_tf = boost::make_shared<PointCloud2>(cloud_transformed);
} } else {
else
cloud_tf = cloud; cloud_tf = cloud;
}
// Need setInputCloud () here because we have to extract x/y/z // Need setInputCloud () here because we have to extract x/y/z
IndicesPtr vindices; IndicesPtr vindices;
if (indices) if (indices) {
vindices.reset (new std::vector<int> (indices->indices)); vindices.reset(new std::vector<int>(indices->indices));
}
computePublish (cloud_tf, vindices); computePublish(cloud_tf, vindices);
} }

79
pcl_ros/src/pcl_ros/filters/passthrough.cpp
View File

@ -40,81 +40,88 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
pcl_ros::PassThrough::child_init (ros::NodeHandle &nh, bool &has_service) pcl_ros::PassThrough::child_init(ros::NodeHandle & nh, bool & has_service)
{ {
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
has_service = true; has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (nh); srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2); dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &PassThrough::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t level) pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock(mutex_);
double filter_min, filter_max; double filter_min, filter_max;
impl_.getFilterLimits (filter_min, filter_max); impl_.getFilterLimits(filter_min, filter_max);
// Check the current values for filter min-max // Check the current values for filter min-max
if (filter_min != config.filter_limit_min) if (filter_min != config.filter_limit_min) {
{
filter_min = config.filter_limit_min; filter_min = config.filter_limit_min;
NODELET_DEBUG ("[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_min); NODELET_DEBUG(
"[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
getName().c_str(), filter_min);
// Set the filter min-max if different // Set the filter min-max if different
impl_.setFilterLimits (filter_min, filter_max); impl_.setFilterLimits(filter_min, filter_max);
} }
// Check the current values for filter min-max // Check the current values for filter min-max
if (filter_max != config.filter_limit_max) if (filter_max != config.filter_limit_max) {
{
filter_max = config.filter_limit_max; filter_max = config.filter_limit_max;
NODELET_DEBUG ("[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_max); NODELET_DEBUG(
"[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
getName().c_str(), filter_max);
// Set the filter min-max if different // Set the filter min-max if different
impl_.setFilterLimits (filter_min, filter_max); impl_.setFilterLimits(filter_min, filter_max);
} }
// Check the current value for the filter field // Check the current value for the filter field
//std::string filter_field = impl_.getFilterFieldName (); //std::string filter_field = impl_.getFilterFieldName ();
if (impl_.getFilterFieldName () != config.filter_field_name) if (impl_.getFilterFieldName() != config.filter_field_name) {
{
// Set the filter field if different // Set the filter field if different
impl_.setFilterFieldName (config.filter_field_name); 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 ()); 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 // Check the current value for keep_organized
if (impl_.getKeepOrganized () != config.keep_organized) if (impl_.getKeepOrganized() != config.keep_organized) {
{ NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); "[%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 // Call the virtual method in the child
impl_.setKeepOrganized (config.keep_organized); impl_.setKeepOrganized(config.keep_organized);
} }
// Check the current value for the negative flag // Check the current value for the negative flag
if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
{ NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); "[%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 // Call the virtual method in the child
impl_.setFilterLimitsNegative (config.filter_limit_negative); impl_.setFilterLimitsNegative(config.filter_limit_negative);
} }
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame) if (tf_input_frame_ != config.input_frame) {
{
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); NODELET_DEBUG(
"[%s::config_callback] Setting the input TF frame to: %s.",
getName().c_str(), tf_input_frame_.c_str());
} }
if (tf_output_frame_ != config.output_frame) if (tf_output_frame_ != config.output_frame) {
{
tf_output_frame_ = config.output_frame; tf_output_frame_ = config.output_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); NODELET_DEBUG(
"[%s::config_callback] Setting the output TF frame to: %s.",
getName().c_str(), tf_output_frame_.c_str());
} }
} }
typedef pcl_ros::PassThrough PassThrough; typedef pcl_ros::PassThrough PassThrough;
PLUGINLIB_EXPORT_CLASS(PassThrough,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet);

130
pcl_ros/src/pcl_ros/filters/project_inliers.cpp
View File

@ -41,16 +41,17 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ProjectInliers::onInit () pcl_ros::ProjectInliers::onInit()
{ {
PCLNodelet::onInit (); PCLNodelet::onInit();
// ---[ Mandatory parameters // ---[ Mandatory parameters
// The type of model to use (user given parameter). // The type of model to use (user given parameter).
int model_type; int model_type;
if (!pnh_->getParam ("model_type", model_type)) if (!pnh_->getParam("model_type", model_type)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
@ -60,103 +61,114 @@ pcl_ros::ProjectInliers::onInit ()
// True if all fields will be returned, false if only XYZ. Default: true. // True if all fields will be returned, false if only XYZ. Default: true.
bool copy_all_fields = true; bool copy_all_fields = true;
pnh_->getParam ("copy_all_data", copy_all_data); pnh_->getParam("copy_all_data", copy_all_data);
pnh_->getParam ("copy_all_fields", copy_all_fields); pnh_->getParam("copy_all_fields", copy_all_fields);
pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - model_type : %d\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - copy_all_data : %s\n" " - model_type : %d\n"
" - copy_all_fields : %s", " - copy_all_data : %s\n"
getName ().c_str (), " - copy_all_fields : %s",
model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); getName().c_str(),
model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
// Set given parameters here // Set given parameters here
impl_.setModelType (model_type); impl_.setModelType(model_type);
impl_.setCopyAllFields (copy_all_fields); impl_.setCopyAllFields(copy_all_fields);
impl_.setCopyAllData (copy_all_data); impl_.setCopyAllData(copy_all_data);
onInitPostProcess (); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ProjectInliers::subscribe () pcl_ros::ProjectInliers::subscribe()
{ {
/* /*
TODO : implement use_indices_ TODO : implement use_indices_
if (use_indices_) if (use_indices_)
{*/ {*/
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
sub_model_.subscribe (*pnh_, "model", max_queue_size_); sub_model_.subscribe(*pnh_, "model", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
{ sync_input_indices_model_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2,
sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_); PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); 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)); sync_input_indices_model_a_->registerCallback(
} bind(
else &ProjectInliers::input_indices_model_callback,
{ this, _1, _2, _3));
sync_input_indices_model_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_); } else {
sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2,
sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
sync_input_indices_model_e_->registerCallback(
bind(
&ProjectInliers::input_indices_model_callback,
this, _1, _2, _3));
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ProjectInliers::unsubscribe () pcl_ros::ProjectInliers::unsubscribe()
{ {
/* /*
TODO : implement use_indices_ TODO : implement use_indices_
if (use_indices_) if (use_indices_)
{*/ {*/
sub_input_filter_.unsubscribe (); sub_input_filter_.unsubscribe();
sub_model_.unsubscribe (); sub_model_.unsubscribe();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud, pcl_ros::ProjectInliers::input_indices_model_callback(
const PointIndicesConstPtr &indices, const PointCloud2::ConstPtr & cloud,
const ModelCoefficientsConstPtr &model) const PointIndicesConstPtr & indices,
const ModelCoefficientsConstPtr & model)
{ {
if (pub_output_.getNumSubscribers () <= 0) 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; return;
} }
NODELET_DEBUG ("[%s::input_indices_model_callback]\n" if (!isValid(model) || !isValid(indices) || !isValid(cloud)) {
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" NODELET_ERROR("[%s::input_indices_model_callback] Invalid input!", getName().c_str());
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" return;
" - 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 (), NODELET_DEBUG(
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (), "[%s::input_indices_model_callback]\n"
model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ()); " - 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; tf_input_orig_frame_ = cloud->header.frame_id;
IndicesPtr vindices; IndicesPtr vindices;
if (indices) if (indices) {
vindices.reset (new std::vector<int> (indices->indices)); vindices.reset(new std::vector<int>(indices->indices));
}
model_ = model; model_ = model;
computePublish (cloud, vindices); computePublish(cloud, vindices);
} }
typedef pcl_ros::ProjectInliers ProjectInliers; typedef pcl_ros::ProjectInliers ProjectInliers;
PLUGINLIB_EXPORT_CLASS(ProjectInliers,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet);

38
pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp
View File

@ -40,38 +40,42 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
pcl_ros::RadiusOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) pcl_ros::RadiusOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service)
{ {
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
has_service = true; has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig> > (nh); srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>::CallbackType f = boost::bind (&RadiusOutlierRemoval::config_callback, this, _1, _2); dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &RadiusOutlierRemoval::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level) pcl_ros::RadiusOutlierRemoval::config_callback(
pcl_ros::RadiusOutlierRemovalConfig & config,
uint32_t level)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock(mutex_);
if (impl_.getMinNeighborsInRadius () != config.min_neighbors) if (impl_.getMinNeighborsInRadius() != config.min_neighbors) {
{ impl_.setMinNeighborsInRadius(config.min_neighbors);
impl_.setMinNeighborsInRadius (config.min_neighbors); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the number of neighbors in radius: %d.", getName ().c_str (), config.min_neighbors); "[%s::config_callback] Setting the number of neighbors in radius: %d.",
getName().c_str(), config.min_neighbors);
} }
if (impl_.getRadiusSearch () != config.radius_search) if (impl_.getRadiusSearch() != config.radius_search) {
{ impl_.setRadiusSearch(config.radius_search);
impl_.setRadiusSearch (config.radius_search); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the radius to search neighbors: %f.", getName ().c_str (), config.radius_search); "[%s::config_callback] Setting the radius to search neighbors: %f.",
getName().c_str(), config.radius_search);
} }
} }
typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval;
PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval, nodelet::Nodelet);

47
pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp
View File

@ -40,42 +40,47 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
pcl_ros::StatisticalOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) pcl_ros::StatisticalOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service)
{ {
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
has_service = true; has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > (nh); srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>>(
dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2); nh);
srv_->setCallback (f); dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f =
boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level) pcl_ros::StatisticalOutlierRemoval::config_callback(
pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock(mutex_);
if (impl_.getMeanK () != config.mean_k) if (impl_.getMeanK() != config.mean_k) {
{ impl_.setMeanK(config.mean_k);
impl_.setMeanK (config.mean_k); NODELET_DEBUG(
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); "[%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) if (impl_.getStddevMulThresh() != config.stddev) {
{ impl_.setStddevMulThresh(config.stddev);
impl_.setStddevMulThresh (config.stddev); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev); "[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.",
getName().c_str(), config.stddev);
} }
if (impl_.getNegative () != config.negative) if (impl_.getNegative() != config.negative) {
{ impl_.setNegative(config.negative);
impl_.setNegative (config.negative); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true"); "[%s::config_callback] Returning only inliers: %s.",
getName().c_str(), config.negative ? "false" : "true");
} }
} }
typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval; typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval;
PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval, nodelet::Nodelet);

94
pcl_ros/src/pcl_ros/filters/voxel_grid.cpp
View File

@ -40,88 +40,92 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
pcl_ros::VoxelGrid::child_init (ros::NodeHandle &nh, bool &has_service) pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service)
{ {
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
has_service = true; has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > (nh); srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2); dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &VoxelGrid::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, pcl_ros::VoxelGrid::filter(
const IndicesPtr &indices, const PointCloud2::ConstPtr & input,
PointCloud2 &output) const IndicesPtr & indices,
PointCloud2 & output)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock(mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL (*(input), *(pcl_input)); pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud (pcl_input); impl_.setInputCloud(pcl_input);
impl_.setIndices (indices); impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output; pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output); impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output); pcl_conversions::moveFromPCL(pcl_output, output);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level) pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock(mutex_);
Eigen::Vector3f leaf_size = impl_.getLeafSize (); Eigen::Vector3f leaf_size = impl_.getLeafSize();
if (leaf_size[0] != config.leaf_size) if (leaf_size[0] != config.leaf_size) {
{ leaf_size.setConstant(config.leaf_size);
leaf_size.setConstant (config.leaf_size); NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
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]);
impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]);
} }
double filter_min, filter_max; double filter_min, filter_max;
impl_.getFilterLimits (filter_min, filter_max); impl_.getFilterLimits(filter_min, filter_max);
if (filter_min != config.filter_limit_min) if (filter_min != config.filter_limit_min) {
{
filter_min = config.filter_limit_min; filter_min = config.filter_limit_min;
NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min); NODELET_DEBUG(
"[config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
filter_min);
} }
if (filter_max != config.filter_limit_max) if (filter_max != config.filter_limit_max) {
{
filter_max = config.filter_limit_max; filter_max = config.filter_limit_max;
NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max); NODELET_DEBUG(
"[config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
filter_max);
} }
impl_.setFilterLimits (filter_min, filter_max); impl_.setFilterLimits(filter_min, filter_max);
if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
{ impl_.setFilterLimitsNegative(config.filter_limit_negative);
impl_.setFilterLimitsNegative (config.filter_limit_negative); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); "[%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) if (impl_.getFilterFieldName() != config.filter_field_name) {
{ impl_.setFilterFieldName(config.filter_field_name);
impl_.setFilterFieldName (config.filter_field_name); NODELET_DEBUG(
NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ()); "[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 // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter
if (tf_input_frame_ != config.input_frame) if (tf_input_frame_ != config.input_frame) {
{
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ()); NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str());
} }
if (tf_output_frame_ != config.output_frame) if (tf_output_frame_ != config.output_frame) {
{
tf_output_frame_ = config.output_frame; tf_output_frame_ = config.output_frame;
NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ()); NODELET_DEBUG(
"[config_callback] Setting the output TF frame to: %s.",
tf_output_frame_.c_str());
} }
// ]--- // ]---
} }
typedef pcl_ros::VoxelGrid VoxelGrid; typedef pcl_ros::VoxelGrid VoxelGrid;
PLUGINLIB_EXPORT_CLASS(VoxelGrid,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet);

81
pcl_ros/src/pcl_ros/io/bag_io.cpp
View File

@ -40,74 +40,73 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name) pcl_ros::BAGReader::open(const std::string & file_name, const std::string & topic_name)
{ {
try try {
{ bag_.open(file_name, rosbag::bagmode::Read);
bag_.open (file_name, rosbag::bagmode::Read); view_.addQuery(bag_, rosbag::TopicQuery(topic_name));
view_.addQuery (bag_, rosbag::TopicQuery (topic_name));
if (view_.size () == 0) if (view_.size() == 0) {
return (false); return false;
}
it_ = view_.begin (); it_ = view_.begin();
} catch (rosbag::BagException & e) {
return false;
} }
catch (rosbag::BagException &e) return true;
{
return (false);
}
return (true);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::BAGReader::onInit () pcl_ros::BAGReader::onInit()
{ {
boost::shared_ptr<ros::NodeHandle> pnh_; boost::shared_ptr<ros::NodeHandle> pnh_;
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); pnh_.reset(new ros::NodeHandle(getMTPrivateNodeHandle()));
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam ("file_name", file_name_)) if (!pnh_->getParam("file_name", file_name_)) {
{ NODELET_ERROR("[onInit] Need a 'file_name' parameter to be set before continuing!");
NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
return; return;
} }
if (!pnh_->getParam ("topic_name", topic_name_)) if (!pnh_->getParam("topic_name", topic_name_)) {
{ NODELET_ERROR("[onInit] Need a 'topic_name' parameter to be set before continuing!");
NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!");
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
int max_queue_size = 1; int max_queue_size = 1;
pnh_->getParam ("publish_rate", publish_rate_); pnh_->getParam("publish_rate", publish_rate_);
pnh_->getParam ("max_queue_size", max_queue_size); pnh_->getParam("max_queue_size", max_queue_size);
ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size); ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2>("output", max_queue_size);
NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - file_name : %s\n" "[onInit] Nodelet successfully created with the following parameters:\n"
" - topic_name : %s", " - file_name : %s\n"
file_name_.c_str (), topic_name_.c_str ()); " - topic_name : %s",
file_name_.c_str(), topic_name_.c_str());
if (!open (file_name_, topic_name_)) if (!open(file_name_, topic_name_)) {
return; return;
}
PointCloud output; PointCloud output;
output_ = boost::make_shared<PointCloud> (output); output_ = boost::make_shared<PointCloud>(output);
output_->header.stamp = ros::Time::now (); output_->header.stamp = ros::Time::now();
// Continous publishing enabled? // Continous publishing enabled?
while (pnh_->ok ()) while (pnh_->ok()) {
{ PointCloudConstPtr cloud = getNextCloud();
PointCloudConstPtr cloud = getNextCloud (); NODELET_DEBUG(
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ()); "Publishing data (%d points) on topic %s in frame %s.",
output_->header.stamp = ros::Time::now (); output_->width * output_->height, pnh_->resolveName(
"output").c_str(), output_->header.frame_id.c_str());
output_->header.stamp = ros::Time::now();
pub_output.publish (output_); pub_output.publish(output_);
ros::Duration (publish_rate_).sleep (); ros::Duration(publish_rate_).sleep();
ros::spinOnce (); ros::spinOnce();
} }
} }
typedef pcl_ros::BAGReader BAGReader; typedef pcl_ros::BAGReader BAGReader;
PLUGINLIB_EXPORT_CLASS(BAGReader,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet);

320
pcl_ros/src/pcl_ros/io/concatenate_data.cpp
View File

@ -44,226 +44,260 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () pcl_ros::PointCloudConcatenateDataSynchronizer::onInit()
{ {
nodelet_topic_tools::NodeletLazy::onInit (); nodelet_topic_tools::NodeletLazy::onInit();
// ---[ Mandatory parameters // ---[ Mandatory parameters
pnh_->getParam ("output_frame", output_frame_); pnh_->getParam("output_frame", output_frame_);
pnh_->getParam ("approximate_sync", approximate_sync_); pnh_->getParam("approximate_sync", approximate_sync_);
if (output_frame_.empty ()) if (output_frame_.empty()) {
{ NODELET_ERROR("[onInit] Need an 'output_frame' parameter to be set before continuing!");
NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
return; return;
} }
if (!pnh_->getParam ("input_topics", input_topics_)) if (!pnh_->getParam("input_topics", input_topics_)) {
{ NODELET_ERROR("[onInit] Need a 'input_topics' parameter to be set before continuing!");
NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
return; return;
} }
if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray) if (input_topics_.getType() != XmlRpc::XmlRpcValue::TypeArray) {
{ NODELET_ERROR("[onInit] Invalid 'input_topics' parameter given!");
NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
return; return;
} }
if (input_topics_.size () == 1) if (input_topics_.size() == 1) {
{ NODELET_ERROR("[onInit] Only one topic given. Need at least two topics to continue.");
NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
return; return;
} }
if (input_topics_.size () > 8) if (input_topics_.size() > 8) {
{ NODELET_ERROR("[onInit] More than 8 topics passed!");
NODELET_ERROR ("[onInit] More than 8 topics passed!");
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("max_queue_size", maximum_queue_size_); pnh_->getParam("max_queue_size", maximum_queue_size_);
// Output // Output
pub_output_ = advertise<PointCloud2> (*pnh_, "output", maximum_queue_size_); pub_output_ = advertise<PointCloud2>(*pnh_, "output", maximum_queue_size_);
onInitPostProcess (); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe () pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe()
{ {
ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:"); ROS_INFO_STREAM("Subscribing to " << input_topics_.size() << " user given topics as inputs:");
for (int d = 0; d < input_topics_.size (); ++d) for (int d = 0; d < input_topics_.size(); ++d) {
ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d])); ROS_INFO_STREAM(" - " << (std::string)(input_topics_[d]));
}
// Subscribe to the filters // Subscribe to the filters
filters_.resize (input_topics_.size ()); filters_.resize(input_topics_.size());
// 8 topics // 8 topics
if (approximate_sync_) if (approximate_sync_) {
ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2, ts_a_.reset(
PointCloud2, PointCloud2, PointCloud2, new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2,
PointCloud2, PointCloud2> PointCloud2,
> (maximum_queue_size_)); PointCloud2, PointCloud2, PointCloud2,
else PointCloud2, PointCloud2>
ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2, >(maximum_queue_size_));
PointCloud2, PointCloud2, PointCloud2, } else {
PointCloud2, PointCloud2> ts_e_.reset(
> (maximum_queue_size_)); new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2,
PointCloud2,
PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2>
>(maximum_queue_size_));
}
// First input_topics_.size () filters are valid // First input_topics_.size () filters are valid
for (int d = 0; d < input_topics_.size (); ++d) for (int d = 0; d < input_topics_.size(); ++d) {
{ filters_[d].reset(new message_filters::Subscriber<PointCloud2>());
filters_[d].reset (new message_filters::Subscriber<PointCloud2> ()); filters_[d]->subscribe(*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
} }
// Bogus null filter // Bogus null filter
filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1)); filters_[0]->registerCallback(
bind(
&PointCloudConcatenateDataSynchronizer::input_callback, this,
_1));
switch (input_topics_.size ()) switch (input_topics_.size()) {
{
case 2: case 2:
{ {
if (approximate_sync_) if (approximate_sync_) {
ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); ts_a_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
else } else {
ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_); ts_e_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
break; }
} break;
}
case 3: case 3:
{ {
if (approximate_sync_) if (approximate_sync_) {
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); ts_a_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
else } else {
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_); ts_e_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
break; }
} break;
}
case 4: case 4:
{ {
if (approximate_sync_) if (approximate_sync_) {
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); ts_a_->connectInput(
else *filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_); nf_);
break; } else {
} ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
nf_);
}
break;
}
case 5: case 5:
{ {
if (approximate_sync_) if (approximate_sync_) {
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); ts_a_->connectInput(
else *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_); nf_, nf_, nf_);
break; } else {
} ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
nf_, nf_, nf_);
}
break;
}
case 6: case 6:
{ {
if (approximate_sync_) if (approximate_sync_) {
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); ts_a_->connectInput(
else *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_); *filters_[5], nf_, nf_);
break; } else {
} ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], nf_, nf_);
}
break;
}
case 7: case 7:
{ {
if (approximate_sync_) if (approximate_sync_) {
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); ts_a_->connectInput(
else *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_); *filters_[5], *filters_[6], nf_);
break; } else {
} ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], *filters_[6], nf_);
}
break;
}
case 8: case 8:
{ {
if (approximate_sync_) if (approximate_sync_) {
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); ts_a_->connectInput(
else *filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]); *filters_[5], *filters_[6], *filters_[7]);
break; } else {
} ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], *filters_[6], *filters_[7]);
}
break;
}
default: default:
{ {
NODELET_FATAL ("Invalid 'input_topics' parameter given!"); NODELET_FATAL("Invalid 'input_topics' parameter given!");
return; return;
} }
} }
if (approximate_sync_) if (approximate_sync_) {
ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8)); ts_a_->registerCallback(
else boost::bind(
ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8)); &PointCloudConcatenateDataSynchronizer::input, this, _1, _2,
_3, _4, _5, _6, _7, _8));
} else {
ts_e_->registerCallback(
boost::bind(
&PointCloudConcatenateDataSynchronizer::input, this, _1, _2,
_3, _4, _5, _6, _7, _8));
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe () pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe()
{ {
for (int d = 0; d < filters_.size (); ++d) for (int d = 0; d < filters_.size(); ++d) {
{ filters_[d]->unsubscribe();
filters_[d]->unsubscribe ();
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out) pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(
const PointCloud2 & in1,
const PointCloud2 & in2,
PointCloud2 & out)
{ {
//ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ()); //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
PointCloud2::Ptr in1_t (new PointCloud2 ()); PointCloud2::Ptr in1_t(new PointCloud2());
PointCloud2::Ptr in2_t (new PointCloud2 ()); PointCloud2::Ptr in2_t(new PointCloud2());
// Transform the point clouds into the specified output frame // Transform the point clouds into the specified output frame
if (output_frame_ != in1.header.frame_id) if (output_frame_ != in1.header.frame_id) {
pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_); pcl_ros::transformPointCloud(output_frame_, in1, *in1_t, tf_);
else } else {
in1_t = boost::make_shared<PointCloud2> (in1); in1_t = boost::make_shared<PointCloud2>(in1);
}
if (output_frame_ != in2.header.frame_id) if (output_frame_ != in2.header.frame_id) {
pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_); pcl_ros::transformPointCloud(output_frame_, in2, *in2_t, tf_);
else } else {
in2_t = boost::make_shared<PointCloud2> (in2); in2_t = boost::make_shared<PointCloud2>(in2);
}
// Concatenate the results // Concatenate the results
pcl::concatenatePointCloud (*in1_t, *in2_t, out); pcl::concatenatePointCloud(*in1_t, *in2_t, out);
// Copy header // Copy header
out.header.stamp = in1.header.stamp; out.header.stamp = in1.header.stamp;
} }
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateDataSynchronizer::input ( pcl_ros::PointCloudConcatenateDataSynchronizer::input(
const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2,
const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4,
const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6,
const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8) const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8)
{ {
PointCloud2::Ptr out1 (new PointCloud2 ()); PointCloud2::Ptr out1(new PointCloud2());
PointCloud2::Ptr out2 (new PointCloud2 ()); PointCloud2::Ptr out2(new PointCloud2());
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1); pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*in1, *in2, *out1);
if (in3 && in3->width * in3->height > 0) if (in3 && in3->width * in3->height > 0) {
{ pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in3, *out2);
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2);
out1 = out2; out1 = out2;
if (in4 && in4->width * in4->height > 0) if (in4 && in4->width * in4->height > 0) {
{ pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in4, *out1);
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1); if (in5 && in5->width * in5->height > 0) {
if (in5 && in5->width * in5->height > 0) pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in5, *out2);
{
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2);
out1 = out2; out1 = out2;
if (in6 && in6->width * in6->height > 0) if (in6 && in6->width * in6->height > 0) {
{ pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in6, *out1);
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1); if (in7 && in7->width * in7->height > 0) {
if (in7 && in7->width * in7->height > 0) pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in7, *out2);
{
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2);
out1 = out2; out1 = out2;
} }
} }
} }
} }
} }
pub_output_.publish (boost::make_shared<PointCloud2> (*out1)); pub_output_.publish(boost::make_shared<PointCloud2>(*out1));
} }
typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer; typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer;
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer, nodelet::Nodelet);

119
pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
View File

@ -45,126 +45,129 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
{ {
nodelet_topic_tools::NodeletLazy::onInit (); nodelet_topic_tools::NodeletLazy::onInit();
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam ("input_messages", input_messages_)) if (!pnh_->getParam("input_messages", input_messages_)) {
{ NODELET_ERROR("[onInit] Need a 'input_messages' parameter to be set before continuing!");
NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
return; return;
} }
if (input_messages_ < 2) if (input_messages_ < 2) {
{ NODELET_ERROR("[onInit] Invalid 'input_messages' parameter given!");
NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!");
return; return;
} }
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("max_queue_size", maximum_queue_size_); pnh_->getParam("max_queue_size", maximum_queue_size_);
pnh_->getParam ("maximum_seconds", maximum_seconds_); pnh_->getParam("maximum_seconds", maximum_seconds_);
pub_output_ = advertise<sensor_msgs::PointCloud2> (*pnh_, "output", maximum_queue_size_); pub_output_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", maximum_queue_size_);
onInitPostProcess (); onInitPostProcess();
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe () pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
{ {
sub_input_ = pnh_->subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this); sub_input_ = pnh_->subscribe(
"input", maximum_queue_size_,
&PointCloudConcatenateFieldsSynchronizer::input_callback, this);
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe () pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe()
{ {
sub_input_.shutdown (); sub_input_.shutdown();
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud) pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud)
{ {
NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", NODELET_DEBUG(
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); "[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(),
pnh_->resolveName("input").c_str());
// Erase old data in the queue // Erase old data in the queue
if (maximum_seconds_ > 0 && queue_.size () > 0) if (maximum_seconds_ > 0 && queue_.size() > 0) {
{ while (fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ) > maximum_seconds_ &&
while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0) queue_.size() > 0)
{ {
NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_, NODELET_WARN(
(*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () )); "[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
queue_.erase (queue_.begin ()); (*queue_.begin()).first.toSec(),
fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ));
queue_.erase(queue_.begin());
} }
} }
// Push back new data // Push back new data
queue_[cloud->header.stamp].push_back (cloud); queue_[cloud->header.stamp].push_back(cloud);
if ((int)queue_[cloud->header.stamp].size () >= input_messages_) if ((int)queue_[cloud->header.stamp].size() >= input_messages_) {
{
// Concatenate together and publish // Concatenate together and publish
std::vector<PointCloudConstPtr> &clouds = queue_[cloud->header.stamp]; std::vector<PointCloudConstPtr> & clouds = queue_[cloud->header.stamp];
PointCloud cloud_out = *clouds[0]; PointCloud cloud_out = *clouds[0];
// Resize the output dataset // Resize the output dataset
int data_size = cloud_out.data.size (); int data_size = cloud_out.data.size();
int nr_fields = cloud_out.fields.size (); int nr_fields = cloud_out.fields.size();
int nr_points = cloud_out.width * cloud_out.height; int nr_points = cloud_out.width * cloud_out.height;
for (size_t i = 1; i < clouds.size (); ++i) for (size_t i = 1; i < clouds.size(); ++i) {
{ assert(
assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step); clouds[i]->data.size() / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) {
{ NODELET_ERROR(
NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!", "[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height); i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
break; break;
} }
// Point step must increase with the length of each new field // Point step must increase with the length of each new field
cloud_out.point_step += clouds[i]->point_step; cloud_out.point_step += clouds[i]->point_step;
// Resize data to hold all clouds // Resize data to hold all clouds
data_size += clouds[i]->data.size (); data_size += clouds[i]->data.size();
// Concatenate fields // Concatenate fields
cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ()); cloud_out.fields.resize(nr_fields + clouds[i]->fields.size());
int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype); int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize(
for (size_t d = 0; d < clouds[i]->fields.size (); ++d) cloud_out.fields[nr_fields - 1].datatype);
{ for (size_t d = 0; d < clouds[i]->fields.size(); ++d) {
cloud_out.fields[nr_fields + d] = clouds[i]->fields[d]; cloud_out.fields[nr_fields + d] = clouds[i]->fields[d];
cloud_out.fields[nr_fields + d].offset += delta_offset; cloud_out.fields[nr_fields + d].offset += delta_offset;
} }
nr_fields += clouds[i]->fields.size (); nr_fields += clouds[i]->fields.size();
} }
// Recalculate row_step // Recalculate row_step
cloud_out.row_step = cloud_out.point_step * cloud_out.width; cloud_out.row_step = cloud_out.point_step * cloud_out.width;
cloud_out.data.resize (data_size); cloud_out.data.resize(data_size);
// Iterate over each point and perform the appropriate memcpys // Iterate over each point and perform the appropriate memcpys
int point_offset = 0; int point_offset = 0;
for (int cp = 0; cp < nr_points; ++cp) for (int cp = 0; cp < nr_points; ++cp) {
{ for (size_t i = 0; i < clouds.size(); ++i) {
for (size_t i = 0; i < clouds.size (); ++i)
{
// Copy each individual point // Copy each individual point
memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step); memcpy(
&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step],
clouds[i]->point_step);
point_offset += clouds[i]->point_step; point_offset += clouds[i]->point_step;
} }
} }
pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out)); pub_output_.publish(boost::make_shared<const PointCloud>(cloud_out));
queue_.erase (cloud->header.stamp); queue_.erase(cloud->header.stamp);
} }
// Clean the queue to avoid overflowing // Clean the queue to avoid overflowing
if (maximum_queue_size_ > 0) if (maximum_queue_size_ > 0) {
{ while ((int)queue_.size() > maximum_queue_size_) {
while ((int)queue_.size () > maximum_queue_size_) queue_.erase(queue_.begin());
queue_.erase (queue_.begin ()); }
} }
} }
typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet);

8
pcl_ros/src/pcl_ros/io/io.cpp
View File

@ -42,7 +42,8 @@
#include <nodelet_topic_tools/nodelet_mux.h> #include <nodelet_topic_tools/nodelet_mux.h>
#include <nodelet_topic_tools/nodelet_demux.h> #include <nodelet_topic_tools/nodelet_demux.h>
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2, message_filters::Subscriber<sensor_msgs::PointCloud2> > NodeletMUX; typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2,
message_filters::Subscriber<sensor_msgs::PointCloud2>> NodeletMUX;
//typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2, pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX; //typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2, pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX;
typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX; typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
@ -51,7 +52,6 @@ typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
//#include "concatenate_fields.cpp" //#include "concatenate_fields.cpp"
//#include "concatenate_data.cpp" //#include "concatenate_data.cpp"
PLUGINLIB_EXPORT_CLASS(NodeletMUX,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(NodeletDEMUX,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet);
//PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet); //PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);

159
pcl_ros/src/pcl_ros/io/pcd_io.cpp
View File

@ -40,143 +40,150 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PCDReader::onInit () pcl_ros::PCDReader::onInit()
{ {
PCLNodelet::onInit (); PCLNodelet::onInit();
// Provide a latched topic // Provide a latched topic
ros::Publisher pub_output = pnh_->advertise<PointCloud2> ("output", max_queue_size_, true); ros::Publisher pub_output = pnh_->advertise<PointCloud2>("output", max_queue_size_, true);
pnh_->getParam ("publish_rate", publish_rate_); pnh_->getParam("publish_rate", publish_rate_);
pnh_->getParam ("tf_frame", tf_frame_); pnh_->getParam("tf_frame", tf_frame_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - publish_rate : %f\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - tf_frame : %s", " - publish_rate : %f\n"
getName ().c_str (), " - tf_frame : %s",
publish_rate_, tf_frame_.c_str ()); getName().c_str(),
publish_rate_, tf_frame_.c_str());
PointCloud2Ptr output_new; PointCloud2Ptr output_new;
output_ = boost::make_shared<PointCloud2> (); output_ = boost::make_shared<PointCloud2>();
output_new = boost::make_shared<PointCloud2> (); output_new = boost::make_shared<PointCloud2>();
// Wait in a loop until someone connects // Wait in a loop until someone connects
do do {
{ ROS_DEBUG_ONCE("[%s::onInit] Waiting for a client to connect...", getName().c_str());
ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ()); ros::spinOnce();
ros::spinOnce (); ros::Duration(0.01).sleep();
ros::Duration (0.01).sleep (); } while (pnh_->ok() && pub_output.getNumSubscribers() == 0);
}
while (pnh_->ok () && pub_output.getNumSubscribers () == 0);
std::string file_name; std::string file_name;
while (pnh_->ok ()) while (pnh_->ok()) {
{
// Get the current filename parameter. If no filename set, loop // Get the current filename parameter. If no filename set, loop
if (!pnh_->getParam ("filename", file_name_) && file_name_.empty ()) if (!pnh_->getParam("filename", file_name_) && file_name_.empty()) {
{ ROS_ERROR_ONCE(
ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'filename' parameter to be set before continuing!",
ros::Duration (0.01).sleep (); getName().c_str());
ros::spinOnce (); ros::Duration(0.01).sleep();
ros::spinOnce();
continue; continue;
} }
// If the filename parameter holds a different value than the last one we read // If the filename parameter holds a different value than the last one we read
if (file_name_.compare (file_name) != 0 && !file_name_.empty ()) if (file_name_.compare(file_name) != 0 && !file_name_.empty()) {
{ NODELET_INFO("[%s::onInit] New file given: %s", getName().c_str(), file_name_.c_str());
NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
file_name = file_name_; file_name = file_name_;
pcl::PCLPointCloud2 cloud; pcl::PCLPointCloud2 cloud;
if (impl_.read (file_name_, cloud) < 0) if (impl_.read(file_name_, cloud) < 0) {
{ NODELET_ERROR("[%s::onInit] Error reading %s !", getName().c_str(), file_name_.c_str());
NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
return; return;
} }
pcl_conversions::moveFromPCL(cloud, *(output_)); pcl_conversions::moveFromPCL(cloud, *(output_));
output_->header.stamp = ros::Time::now (); output_->header.stamp = ros::Time::now();
output_->header.frame_id = tf_frame_; output_->header.frame_id = tf_frame_;
} }
// We do not publish empty data // We do not publish empty data
if (output_->data.size () == 0) if (output_->data.size() == 0) {
continue; continue;
}
if (publish_rate_ == 0) if (publish_rate_ == 0) {
{ if (output_ != output_new) {
if (output_ != output_new) NODELET_DEBUG(
{ "Publishing data once (%d points) on topic %s in frame %s.",
NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); output_->width * output_->height,
pub_output.publish (output_); getMTPrivateNodeHandle().resolveName("output").c_str(), output_->header.frame_id.c_str());
pub_output.publish(output_);
output_new = output_; output_new = output_;
} }
ros::Duration (0.01).sleep (); ros::Duration(0.01).sleep();
} } else {
else NODELET_DEBUG(
{ "Publishing data (%d points) on topic %s in frame %s.",
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); output_->width * output_->height, getMTPrivateNodeHandle().resolveName(
output_->header.stamp = ros::Time::now (); "output").c_str(), output_->header.frame_id.c_str());
pub_output.publish (output_); output_->header.stamp = ros::Time::now();
pub_output.publish(output_);
ros::Duration (publish_rate_).sleep (); ros::Duration(publish_rate_).sleep();
} }
ros::spinOnce (); ros::spinOnce();
// Update parameters from server // Update parameters from server
pnh_->getParam ("publish_rate", publish_rate_); pnh_->getParam("publish_rate", publish_rate_);
pnh_->getParam ("tf_frame", tf_frame_); pnh_->getParam("tf_frame", tf_frame_);
} }
onInitPostProcess (); onInitPostProcess();
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PCDWriter::onInit () pcl_ros::PCDWriter::onInit()
{ {
PCLNodelet::onInit (); PCLNodelet::onInit();
sub_input_ = pnh_->subscribe ("input", 1, &PCDWriter::input_callback, this); sub_input_ = pnh_->subscribe("input", 1, &PCDWriter::input_callback, this);
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("filename", file_name_); pnh_->getParam("filename", file_name_);
pnh_->getParam ("binary_mode", binary_mode_); pnh_->getParam("binary_mode", binary_mode_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - filename : %s\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - binary_mode : %s", " - filename : %s\n"
getName ().c_str (), " - binary_mode : %s",
file_name_.c_str (), (binary_mode_) ? "true" : "false"); getName().c_str(),
file_name_.c_str(), (binary_mode_) ? "true" : "false");
onInitPostProcess (); onInitPostProcess();
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud) pcl_ros::PCDWriter::input_callback(const PointCloud2ConstPtr & cloud)
{ {
if (!isValid (cloud)) if (!isValid(cloud)) {
return; return;
}
pnh_->getParam ("filename", file_name_); pnh_->getParam("filename", file_name_);
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height,
cloud->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("input").c_str());
std::string fname; std::string fname;
if (file_name_.empty ()) if (file_name_.empty()) {
fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd"; fname = boost::lexical_cast<std::string>(cloud->header.stamp.toSec()) + ".pcd";
else } else {
fname = file_name_; fname = file_name_;
}
pcl::PCLPointCloud2 pcl_cloud; pcl::PCLPointCloud2 pcl_cloud;
// It is safe to remove the const here because we are the only subscriber callback. // It is safe to remove the const here because we are the only subscriber callback.
pcl_conversions::moveToPCL(*(const_cast<PointCloud2*>(cloud.get())), pcl_cloud); pcl_conversions::moveToPCL(*(const_cast<PointCloud2 *>(cloud.get())), pcl_cloud);
impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); impl_.write(
fname, pcl_cloud, Eigen::Vector4f::Zero(),
Eigen::Quaternionf::Identity(), binary_mode_);
NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ()); NODELET_DEBUG("[%s::input_callback] Data saved to %s", getName().c_str(), fname.c_str());
} }
typedef pcl_ros::PCDReader PCDReader; typedef pcl_ros::PCDReader PCDReader;
typedef pcl_ros::PCDWriter PCDWriter; typedef pcl_ros::PCDWriter PCDWriter;
PLUGINLIB_EXPORT_CLASS(PCDReader,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(PCDReader, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(PCDWriter,nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(PCDWriter, nodelet::Nodelet);

239
pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
View File

@ -48,139 +48,155 @@ using pcl_conversions::toPCL;
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::EuclideanClusterExtraction::onInit () pcl_ros::EuclideanClusterExtraction::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
// ---[ Mandatory parameters // ---[ Mandatory parameters
double cluster_tolerance; double cluster_tolerance;
if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance)) if (!pnh_->getParam("cluster_tolerance", cluster_tolerance)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
int spatial_locator; int spatial_locator;
if (!pnh_->getParam ("spatial_locator", spatial_locator)) if (!pnh_->getParam("spatial_locator", spatial_locator)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
//private_nh.getParam ("use_indices", use_indices_); //private_nh.getParam ("use_indices", use_indices_);
pnh_->getParam ("publish_indices", publish_indices_); pnh_->getParam("publish_indices", publish_indices_);
if (publish_indices_) if (publish_indices_) {
pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointIndices>(*pnh_, "output", max_queue_size_);
else } else {
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
}
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>>(*pnh_);
dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2); dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &EuclideanClusterExtraction::config_callback, this, _1, _2);
srv_->setCallback(f);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - max_queue_size : %d\n" "[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - use_indices : %s\n" " - max_queue_size : %d\n"
" - cluster_tolerance : %f\n", " - use_indices : %s\n"
getName ().c_str (), " - cluster_tolerance : %f\n",
max_queue_size_, getName().c_str(),
(use_indices_) ? "true" : "false", cluster_tolerance); max_queue_size_,
(use_indices_) ? "true" : "false", cluster_tolerance);
// Set given parameters here // Set given parameters here
impl_.setClusterTolerance (cluster_tolerance); impl_.setClusterTolerance(cluster_tolerance);
onInitPostProcess (); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::EuclideanClusterExtraction::subscribe () pcl_ros::EuclideanClusterExtraction::subscribe()
{ {
// If we're supposed to look for PointIndices (indices) // If we're supposed to look for PointIndices (indices)
if (use_indices_) if (use_indices_) {
{
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_) if (approximate_sync_) {
{ sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud,
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_); PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2)); sync_input_indices_a_->registerCallback(
bind(
&EuclideanClusterExtraction::
input_indices_callback, this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(
bind(
&EuclideanClusterExtraction::
input_indices_callback, this, _1, _2));
} }
else } else {
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
}
}
else
// Subscribe in an old fashion to input only (no filters) // Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ())); sub_input_ =
pnh_->subscribe<PointCloud>(
"input", max_queue_size_,
bind(&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr()));
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::EuclideanClusterExtraction::unsubscribe () pcl_ros::EuclideanClusterExtraction::unsubscribe()
{ {
if (use_indices_) if (use_indices_) {
{ sub_input_filter_.unsubscribe();
sub_input_filter_.unsubscribe (); sub_indices_filter_.unsubscribe();
sub_indices_filter_.unsubscribe (); } else {
sub_input_.shutdown();
} }
else
sub_input_.shutdown ();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level) pcl_ros::EuclideanClusterExtraction::config_callback(
EuclideanClusterExtractionConfig & config,
uint32_t level)
{ {
if (impl_.getClusterTolerance () != config.cluster_tolerance) if (impl_.getClusterTolerance() != config.cluster_tolerance) {
{ impl_.setClusterTolerance(config.cluster_tolerance);
impl_.setClusterTolerance (config.cluster_tolerance); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance); "[%s::config_callback] Setting new clustering tolerance to: %f.",
getName().c_str(), config.cluster_tolerance);
} }
if (impl_.getMinClusterSize () != config.cluster_min_size) if (impl_.getMinClusterSize() != config.cluster_min_size) {
{ impl_.setMinClusterSize(config.cluster_min_size);
impl_.setMinClusterSize (config.cluster_min_size); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size); "[%s::config_callback] Setting the minimum cluster size to: %d.",
getName().c_str(), config.cluster_min_size);
} }
if (impl_.getMaxClusterSize () != config.cluster_max_size) if (impl_.getMaxClusterSize() != config.cluster_max_size) {
{ impl_.setMaxClusterSize(config.cluster_max_size);
impl_.setMaxClusterSize (config.cluster_max_size); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size); "[%s::config_callback] Setting the maximum cluster size to: %d.",
getName().c_str(), config.cluster_max_size);
} }
if (max_clusters_ != config.max_clusters) if (max_clusters_ != config.max_clusters) {
{
max_clusters_ = config.max_clusters; max_clusters_ = config.max_clusters;
NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters); NODELET_DEBUG(
"[%s::config_callback] Setting the maximum number of clusters to extract to: %d.",
getName().c_str(), config.max_clusters);
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::EuclideanClusterExtraction::input_indices_callback ( pcl_ros::EuclideanClusterExtraction::input_indices_callback(
const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices) const PointCloudConstPtr & cloud, const PointIndicesConstPtr & indices)
{ {
// No subscribers, no work // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0) {
return; return;
}
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid (cloud)) if (!isValid(cloud)) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid(indices)) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
return; return;
} }
@ -188,65 +204,72 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
if (indices) { if (indices) {
std_msgs::Header cloud_header = fromPCL(cloud->header); std_msgs::Header cloud_header = fromPCL(cloud->header);
std_msgs::Header indices_header = indices->header; std_msgs::Header indices_header = indices->header;
NODELET_DEBUG ("[%s::input_indices_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%s::input_indices_callback]\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - 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 (), getName().c_str(),
indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").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 { } else {
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
} }
/// ///
IndicesPtr indices_ptr; IndicesPtr indices_ptr;
if (indices) if (indices) {
indices_ptr.reset (new std::vector<int> (indices->indices)); indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices_ptr); impl_.setIndices(indices_ptr);
std::vector<pcl::PointIndices> clusters; std::vector<pcl::PointIndices> clusters;
impl_.extract (clusters); impl_.extract(clusters);
if (publish_indices_) if (publish_indices_) {
{ for (size_t i = 0; i < clusters.size(); ++i) {
for (size_t i = 0; i < clusters.size (); ++i) if ((int)i >= max_clusters_) {
{
if ((int)i >= max_clusters_)
break; break;
}
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
pcl_msgs::PointIndices ros_pi; pcl_msgs::PointIndices ros_pi;
moveFromPCL(clusters[i], ros_pi); moveFromPCL(clusters[i], ros_pi);
ros_pi.header.stamp += ros::Duration (i * 0.001); ros_pi.header.stamp += ros::Duration(i * 0.001);
pub_output_.publish (ros_pi); pub_output_.publish(ros_pi);
} }
NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ()); NODELET_DEBUG(
} "[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s",
else clusters.size(), pnh_->resolveName("output").c_str());
{ } else {
for (size_t i = 0; i < clusters.size (); ++i) for (size_t i = 0; i < clusters.size(); ++i) {
{ if ((int)i >= max_clusters_) {
if ((int)i >= max_clusters_)
break; break;
}
PointCloud output; PointCloud output;
copyPointCloud (*cloud, clusters[i].indices, output); copyPointCloud(*cloud, clusters[i].indices, output);
//PointCloud output_blob; // Convert from the templated output to the PointCloud blob //PointCloud output_blob; // Convert from the templated output to the PointCloud blob
//pcl::toROSMsg (output, output_blob); //pcl::toROSMsg (output, output_blob);
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
std_msgs::Header header = fromPCL(output.header); std_msgs::Header header = fromPCL(output.header);
header.stamp += ros::Duration (i * 0.001); header.stamp += ros::Duration(i * 0.001);
toPCL(header, output.header); toPCL(header, output.header);
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", NODELET_DEBUG(
i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); "[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
i, clusters[i].indices.size(), header.stamp.toSec(), pnh_->resolveName("output").c_str());
} }
} }
} }
typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction; typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction;
PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet)

222
pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
View File

@ -47,174 +47,206 @@ using pcl_conversions::moveToPCL;
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ExtractPolygonalPrismData::onInit () pcl_ros::ExtractPolygonalPrismData::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>>(*pnh_);
dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2); dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &ExtractPolygonalPrismData::config_callback, this, _1, _2);
srv_->setCallback(f);
// Advertise the output topics // Advertise the output topics
pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointIndices>(*pnh_, "output", max_queue_size_);
onInitPostProcess (); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ExtractPolygonalPrismData::subscribe () pcl_ros::ExtractPolygonalPrismData::subscribe()
{ {
sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_); sub_hull_filter_.subscribe(*pnh_, "planar_hull", max_queue_size_);
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
// Create the objects here // Create the objects here
if (approximate_sync_) if (approximate_sync_) {
sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_); sync_input_hull_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
else PointCloud, PointIndices>>>(max_queue_size_);
sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_); } else {
sync_input_hull_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
if (use_indices_) PointCloud, PointIndices>>>(max_queue_size_);
{
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
if (approximate_sync_)
sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
else
sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
} }
else
{
sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1));
if (approximate_sync_) if (use_indices_) {
sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
else if (approximate_sync_) {
sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_); sync_input_hull_indices_a_->connectInput(
sub_input_filter_, sub_hull_filter_,
sub_indices_filter_);
} else {
sync_input_hull_indices_e_->connectInput(
sub_input_filter_, sub_hull_filter_,
sub_indices_filter_);
}
} else {
sub_input_filter_.registerCallback(bind(&ExtractPolygonalPrismData::input_callback, this, _1));
if (approximate_sync_) {
sync_input_hull_indices_a_->connectInput(sub_input_filter_, sub_hull_filter_, nf_);
} else {
sync_input_hull_indices_e_->connectInput(sub_input_filter_, sub_hull_filter_, nf_);
}
} }
// Register callbacks // Register callbacks
if (approximate_sync_) if (approximate_sync_) {
sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); sync_input_hull_indices_a_->registerCallback(
else bind(
sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3)); &ExtractPolygonalPrismData::
input_hull_indices_callback, this, _1, _2, _3));
} else {
sync_input_hull_indices_e_->registerCallback(
bind(
&ExtractPolygonalPrismData::
input_hull_indices_callback, this, _1, _2, _3));
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ExtractPolygonalPrismData::unsubscribe () pcl_ros::ExtractPolygonalPrismData::unsubscribe()
{ {
sub_hull_filter_.unsubscribe (); sub_hull_filter_.unsubscribe();
sub_input_filter_.unsubscribe (); sub_input_filter_.unsubscribe();
if (use_indices_) if (use_indices_) {
sub_indices_filter_.unsubscribe (); sub_indices_filter_.unsubscribe();
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level) pcl_ros::ExtractPolygonalPrismData::config_callback(
ExtractPolygonalPrismDataConfig & config,
uint32_t level)
{ {
double height_min, height_max; double height_min, height_max;
impl_.getHeightLimits (height_min, height_max); impl_.getHeightLimits(height_min, height_max);
if (height_min != config.height_min) if (height_min != config.height_min) {
{
height_min = config.height_min; height_min = config.height_min;
NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min); NODELET_DEBUG(
impl_.setHeightLimits (height_min, height_max); "[%s::config_callback] Setting new minimum height to the planar model to: %f.",
getName().c_str(), height_min);
impl_.setHeightLimits(height_min, height_max);
} }
if (height_max != config.height_max) if (height_max != config.height_max) {
{
height_max = config.height_max; height_max = config.height_max;
NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max); NODELET_DEBUG(
impl_.setHeightLimits (height_min, height_max); "[%s::config_callback] Setting new maximum height to the planar model to: %f.",
getName().c_str(), height_max);
impl_.setHeightLimits(height_min, height_max);
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
const PointCloudConstPtr &cloud, const PointCloudConstPtr & cloud,
const PointCloudConstPtr &hull, const PointCloudConstPtr & hull,
const PointIndicesConstPtr &indices) const PointIndicesConstPtr & indices)
{ {
// No subscribers, no work // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0) {
return; return;
}
// Copy the header (stamp + frame_id) // Copy the header (stamp + frame_id)
pcl_msgs::PointIndices inliers; pcl_msgs::PointIndices inliers;
inliers.header = fromPCL(cloud->header); inliers.header = fromPCL(cloud->header);
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid (cloud) || !isValid (hull, "planar_hull")) if (!isValid(cloud) || !isValid(hull, "planar_hull")) {
{ NODELET_ERROR("[%s::input_hull_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ()); pub_output_.publish(inliers);
pub_output_.publish (inliers);
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid(indices)) {
{ NODELET_ERROR("[%s::input_hull_indices_callback] Invalid indices!", getName().c_str());
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ()); pub_output_.publish(inliers);
pub_output_.publish (inliers);
return; return;
} }
/// DEBUG /// DEBUG
if (indices) if (indices) {
NODELET_DEBUG ("[%s::input_indices_hull_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), getName().c_str(),
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ()); cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
else "input").c_str(),
NODELET_DEBUG ("[%s::input_indices_hull_callback]\n" hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "planar_hull").c_str(),
getName ().c_str (), indices->indices.size(), indices->header.stamp.toSec(),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ()); } else {
NODELET_DEBUG(
"[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL(
hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName(
"planar_hull").c_str());
}
/// ///
if (cloud->header.frame_id != hull->header.frame_id) if (cloud->header.frame_id != hull->header.frame_id) {
{ NODELET_DEBUG(
NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ()); "[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.",
getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str());
PointCloud planar_hull; PointCloud planar_hull;
if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_)) if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) {
{
// Publish empty before return // Publish empty before return
pub_output_.publish (inliers); pub_output_.publish(inliers);
return; return;
} }
impl_.setInputPlanarHull (pcl_ptr(planar_hull.makeShared ())); impl_.setInputPlanarHull(pcl_ptr(planar_hull.makeShared()));
} else {
impl_.setInputPlanarHull(pcl_ptr(hull));
} }
else
impl_.setInputPlanarHull (pcl_ptr(hull));
IndicesPtr indices_ptr; IndicesPtr indices_ptr;
if (indices && !indices->header.frame_id.empty ()) if (indices && !indices->header.frame_id.empty()) {
indices_ptr.reset (new std::vector<int> (indices->indices)); indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices_ptr); impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty ()) { if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers; pcl::PointIndices pcl_inliers;
moveToPCL(inliers, pcl_inliers); moveToPCL(inliers, pcl_inliers);
impl_.segment (pcl_inliers); impl_.segment(pcl_inliers);
moveFromPCL(pcl_inliers, inliers); moveFromPCL(pcl_inliers, inliers);
} }
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
inliers.header = fromPCL(cloud->header); inliers.header = fromPCL(cloud->header);
pub_output_.publish (inliers); pub_output_.publish(inliers);
NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); NODELET_DEBUG(
"[%s::input_hull_callback] Publishing %zu indices.",
getName().c_str(), inliers.indices.size());
} }
typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData; typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet)

779
pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp
View File

File diff suppressed because it is too large Load Diff

130
pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp
View File

@ -41,103 +41,119 @@
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::onInit () pcl_ros::SegmentDifferences::onInit()
{ {
// Call the super onInit () // Call the super onInit ()
PCLNodelet::onInit (); PCLNodelet::onInit();
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - max_queue_size : %d", "[%s::onInit] Nodelet successfully created with the following parameters:\n"
getName ().c_str (), " - max_queue_size : %d",
max_queue_size_); getName().c_str(),
max_queue_size_);
onInitPostProcess (); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::subscribe () pcl_ros::SegmentDifferences::subscribe()
{ {
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_); sub_target_filter_.subscribe(*pnh_, "target", max_queue_size_);
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<SegmentDifferencesConfig>>(*pnh_);
dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2); dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &SegmentDifferences::config_callback, this, _1, _2);
srv_->setCallback(f);
if (approximate_sync_) if (approximate_sync_) {
{ sync_input_target_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_); PointCloud>>>(max_queue_size_);
sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_); sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2)); sync_input_target_a_->registerCallback(
} bind(
else &SegmentDifferences::input_target_callback, this,
{ _1, _2));
sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > (max_queue_size_); } else {
sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_); sync_input_target_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2)); PointCloud>>>(max_queue_size_);
sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_e_->registerCallback(
bind(
&SegmentDifferences::input_target_callback, this,
_1, _2));
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::unsubscribe () pcl_ros::SegmentDifferences::unsubscribe()
{ {
sub_input_filter_.unsubscribe (); sub_input_filter_.unsubscribe();
sub_target_filter_.unsubscribe (); sub_target_filter_.unsubscribe();
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level) pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level)
{ {
if (impl_.getDistanceThreshold () != config.distance_threshold) if (impl_.getDistanceThreshold() != config.distance_threshold) {
{ impl_.setDistanceThreshold(config.distance_threshold);
impl_.setDistanceThreshold (config.distance_threshold); NODELET_DEBUG(
NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold); "[%s::config_callback] Setting new distance threshold to: %f.",
getName().c_str(), config.distance_threshold);
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud, pcl_ros::SegmentDifferences::input_target_callback(
const PointCloudConstPtr &cloud_target) const PointCloudConstPtr & cloud,
const PointCloudConstPtr & cloud_target)
{ {
if (pub_output_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0) {
return;
if (!isValid (cloud) || !isValid (cloud_target, "target"))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
PointCloud output;
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
return; return;
} }
NODELET_DEBUG ("[%s::input_indices_callback]\n" if (!isValid(cloud) || !isValid(cloud_target, "target")) {
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", PointCloud output;
getName ().c_str (), output.header = cloud->header;
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), pub_output_.publish(ros_ptr(output.makeShared()));
cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ()); return;
}
impl_.setInputCloud (pcl_ptr(cloud)); NODELET_DEBUG(
impl_.setTargetCloud (pcl_ptr(cloud_target)); "[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_target->width * cloud_target->height, pcl::getFieldsList(*cloud_target).c_str(),
fromPCL(cloud_target->header).stamp.toSec(),
cloud_target->header.frame_id.c_str(), pnh_->resolveName("target").c_str());
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setTargetCloud(pcl_ptr(cloud_target));
PointCloud output; PointCloud output;
impl_.segment (output); impl_.segment(output);
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (), NODELET_DEBUG(
output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ()); "[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s",
getName().c_str(),
output.points.size(), fromPCL(output.header).stamp.toSec(),
pnh_->resolveName("output").c_str());
} }
typedef pcl_ros::SegmentDifferences SegmentDifferences; typedef pcl_ros::SegmentDifferences SegmentDifferences;
PLUGINLIB_EXPORT_CLASS(SegmentDifferences, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(SegmentDifferences, nodelet::Nodelet)

2
pcl_ros/src/pcl_ros/segmentation/segmentation.cpp
View File

@ -40,5 +40,3 @@
//#include "extract_polygonal_prism_data.cpp" //#include "extract_polygonal_prism_data.cpp"
//#include "sac_segmentation.cpp" //#include "sac_segmentation.cpp"
//#include "segment_differences.cpp" //#include "segment_differences.cpp"

164
pcl_ros/src/pcl_ros/surface/convex_hull.cpp
View File

@ -42,20 +42,21 @@
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ConvexHull2D::onInit () pcl_ros::ConvexHull2D::onInit()
{ {
PCLNodelet::onInit (); PCLNodelet::onInit();
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
pub_plane_ = advertise<geometry_msgs::PolygonStamped> (*pnh_, "output_polygon", max_queue_size_); pub_plane_ = advertise<geometry_msgs::PolygonStamped>(*pnh_, "output_polygon", max_queue_size_);
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("use_indices", use_indices_); pnh_->getParam("use_indices", use_indices_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - use_indices : %s", "[%s::onInit] Nodelet successfully created with the following parameters:\n"
getName ().c_str (), " - use_indices : %s",
(use_indices_) ? "true" : "false"); getName().c_str(),
(use_indices_) ? "true" : "false");
onInitPostProcess(); onInitPostProcess();
} }
@ -65,139 +66,150 @@ void
pcl_ros::ConvexHull2D::subscribe() pcl_ros::ConvexHull2D::subscribe()
{ {
// If we're supposed to look for PointIndices (indices) // If we're supposed to look for PointIndices (indices)
if (use_indices_) if (use_indices_) {
{
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", 1); sub_input_filter_.subscribe(*pnh_, "input", 1);
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe (*pnh_, "indices", 1); sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_) if (approximate_sync_) {
{ sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_); PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2)); sync_input_indices_a_->registerCallback(
} bind(
else &ConvexHull2D::input_indices_callback, this, _1,
{ _2));
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_); } else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2)); sync_input_indices_e_->registerCallback(
bind(
&ConvexHull2D::input_indices_callback, this, _1,
_2));
} }
} } else {
else
// Subscribe in an old fashion to input only (no filters) // Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<PointCloud> ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); sub_input_ =
pnh_->subscribe<PointCloud>(
"input", 1,
bind(&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr()));
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ConvexHull2D::unsubscribe() pcl_ros::ConvexHull2D::unsubscribe()
{ {
if (use_indices_) if (use_indices_) {
{
sub_input_filter_.unsubscribe(); sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe(); sub_indices_filter_.unsubscribe();
} } else {
else
sub_input_.shutdown(); sub_input_.shutdown();
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud, pcl_ros::ConvexHull2D::input_indices_callback(
const PointIndicesConstPtr &indices) const PointCloudConstPtr & cloud,
const PointIndicesConstPtr & indices)
{ {
// No subscribers, no work // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0 && pub_plane_.getNumSubscribers() <= 0) {
return; return;
}
PointCloud output; PointCloud output;
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid (cloud)) if (!isValid(cloud)) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
// Publish an empty message // Publish an empty message
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices, "indices")) if (indices && !isValid(indices, "indices")) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
// Publish an empty message // Publish an empty message
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
return; return;
} }
/// DEBUG /// DEBUG
if (indices) if (indices) {
NODELET_DEBUG ("[%s::input_indices_model_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%s::input_indices_model_callback]\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), getName().c_str(),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
else cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); getMTPrivateNodeHandle().resolveName("input").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
getMTPrivateNodeHandle().resolveName("input").c_str());
}
// Reset the indices and surface pointers // Reset the indices and surface pointers
IndicesPtr indices_ptr; IndicesPtr indices_ptr;
if (indices) if (indices) {
indices_ptr.reset (new std::vector<int> (indices->indices)); indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices (indices_ptr); impl_.setIndices(indices_ptr);
// Estimate the feature // Estimate the feature
impl_.reconstruct (output); impl_.reconstruct(output);
// If more than 3 points are present, send a PolygonStamped hull too // If more than 3 points are present, send a PolygonStamped hull too
if (output.points.size () >= 3) if (output.points.size() >= 3) {
{
geometry_msgs::PolygonStamped poly; geometry_msgs::PolygonStamped poly;
poly.header = fromPCL(output.header); poly.header = fromPCL(output.header);
poly.polygon.points.resize (output.points.size ()); poly.polygon.points.resize(output.points.size());
// Get three consecutive points (without copying) // Get three consecutive points (without copying)
pcl::Vector4fMap O = output.points[1].getVector4fMap (); pcl::Vector4fMap O = output.points[1].getVector4fMap();
pcl::Vector4fMap B = output.points[0].getVector4fMap (); pcl::Vector4fMap B = output.points[0].getVector4fMap();
pcl::Vector4fMap A = output.points[2].getVector4fMap (); pcl::Vector4fMap A = output.points[2].getVector4fMap();
// Check the direction of points -- polygon must have CCW direction // Check the direction of points -- polygon must have CCW direction
Eigen::Vector4f OA = A - O; Eigen::Vector4f OA = A - O;
Eigen::Vector4f OB = B - O; Eigen::Vector4f OB = B - O;
Eigen::Vector4f N = OA.cross3 (OB); Eigen::Vector4f N = OA.cross3(OB);
double theta = N.dot (O); double theta = N.dot(O);
bool reversed = false; bool reversed = false;
if (theta < (M_PI / 2.0)) if (theta < (M_PI / 2.0)) {
reversed = true; reversed = true;
for (size_t i = 0; i < output.points.size (); ++i) }
{ for (size_t i = 0; i < output.points.size(); ++i) {
if (reversed) if (reversed) {
{ size_t j = output.points.size() - i - 1;
size_t j = output.points.size () - i - 1;
poly.polygon.points[i].x = output.points[j].x; poly.polygon.points[i].x = output.points[j].x;
poly.polygon.points[i].y = output.points[j].y; poly.polygon.points[i].y = output.points[j].y;
poly.polygon.points[i].z = output.points[j].z; poly.polygon.points[i].z = output.points[j].z;
} } else {
else
{
poly.polygon.points[i].x = output.points[i].x; poly.polygon.points[i].x = output.points[i].x;
poly.polygon.points[i].y = output.points[i].y; poly.polygon.points[i].y = output.points[i].y;
poly.polygon.points[i].z = output.points[i].z; poly.polygon.points[i].z = output.points[i].z;
} }
} }
pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly)); pub_plane_.publish(boost::make_shared<const geometry_msgs::PolygonStamped>(poly));
} }
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
} }
typedef pcl_ros::ConvexHull2D ConvexHull2D; typedef pcl_ros::ConvexHull2D ConvexHull2D;
PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet)

213
pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp
View File

@ -40,140 +40,160 @@
#include <pcl/io/io.h> #include <pcl/io/io.h>
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::MovingLeastSquares::onInit () pcl_ros::MovingLeastSquares::onInit()
{ {
PCLNodelet::onInit (); PCLNodelet::onInit();
//ros::NodeHandle private_nh = getMTPrivateNodeHandle (); //ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
pub_output_ = advertise<PointCloudIn> (*pnh_, "output", max_queue_size_); pub_output_ = advertise<PointCloudIn>(*pnh_, "output", max_queue_size_);
pub_normals_ = advertise<NormalCloudOut> (*pnh_, "normals", max_queue_size_); pub_normals_ = advertise<NormalCloudOut>(*pnh_, "normals", max_queue_size_);
//if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
if (!pnh_->getParam ("search_radius", search_radius_)) if (!pnh_->getParam("search_radius", search_radius_)) {
{
//NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); //NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ()); NODELET_ERROR(
"[%s::onInit] Need a 'search_radius' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_)) if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
{ NODELET_ERROR(
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ()); "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return; return;
} }
// Enable the dynamic reconfigure service // Enable the dynamic reconfigure service
srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_); srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig>>(*pnh_);
dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 ); dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind(
srv_->setCallback (f); &MovingLeastSquares::config_callback, this, _1, _2);
srv_->setCallback(f);
// ---[ Optional parameters // ---[ Optional parameters
pnh_->getParam ("use_indices", use_indices_); pnh_->getParam("use_indices", use_indices_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" NODELET_DEBUG(
" - use_indices : %s", "[%s::onInit] Nodelet successfully created with the following parameters:\n"
getName ().c_str (), " - use_indices : %s",
(use_indices_) ? "true" : "false"); getName().c_str(),
(use_indices_) ? "true" : "false");
onInitPostProcess (); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::MovingLeastSquares::subscribe () pcl_ros::MovingLeastSquares::subscribe()
{ {
// If we're supposed to look for PointIndices (indices) // If we're supposed to look for PointIndices (indices)
if (use_indices_) if (use_indices_) {
{
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", 1); sub_input_filter_.subscribe(*pnh_, "input", 1);
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe (*pnh_, "indices", 1); sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_) if (approximate_sync_) {
{ sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn,
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_); PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); sync_input_indices_a_->registerCallback(
} bind(
else &MovingLeastSquares::input_indices_callback,
{ this, _1, _2));
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_); } else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2)); sync_input_indices_e_->registerCallback(
bind(
&MovingLeastSquares::input_indices_callback,
this, _1, _2));
} }
} } else {
else
// Subscribe in an old fashion to input only (no filters) // Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ())); sub_input_ =
} pnh_->subscribe<PointCloudIn>(
"input", 1,
////////////////////////////////////////////////////////////////////////////////////////////// bind(&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr()));
void
pcl_ros::MovingLeastSquares::unsubscribe ()
{
if (use_indices_)
{
sub_input_filter_.unsubscribe ();
sub_indices_filter_.unsubscribe ();
} }
else
sub_input_.shutdown ();
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud, pcl_ros::MovingLeastSquares::unsubscribe()
const PointIndicesConstPtr &indices) {
if (use_indices_) {
sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe();
} else {
sub_input_.shutdown();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::MovingLeastSquares::input_indices_callback(
const PointCloudInConstPtr & cloud,
const PointIndicesConstPtr & indices)
{ {
// No subscribers, no work // No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0) if (pub_output_.getNumSubscribers() <= 0 && pub_normals_.getNumSubscribers() <= 0) {
return; return;
}
// Output points have the same type as the input, they are only smoothed // Output points have the same type as the input, they are only smoothed
PointCloudIn output; PointCloudIn output;
// Normals are also estimated and published on a separate topic // Normals are also estimated and published on a separate topic
NormalCloudOut::Ptr normals (new NormalCloudOut ()); NormalCloudOut::Ptr normals(new NormalCloudOut());
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid (cloud)) if (!isValid(cloud)) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices, "indices")) if (indices && !isValid(indices, "indices")) {
{ NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
return; return;
} }
/// DEBUG /// DEBUG
if (indices) if (indices) {
NODELET_DEBUG ("[%s::input_indices_model_callback]\n" NODELET_DEBUG(
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "[%s::input_indices_model_callback]\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
getName ().c_str (), " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), getName().c_str(),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
else cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); getMTPrivateNodeHandle().resolveName("input").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
getMTPrivateNodeHandle().resolveName("input").c_str());
}
/// ///
// Reset the indices and surface pointers // Reset the indices and surface pointers
impl_.setInputCloud (pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
IndicesPtr indices_ptr; IndicesPtr indices_ptr;
if (indices) if (indices) {
indices_ptr.reset (new std::vector<int> (indices->indices)); indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setIndices (indices_ptr); impl_.setIndices(indices_ptr);
// Initialize the spatial locator // Initialize the spatial locator
@ -183,14 +203,14 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr
// Publish a Boost shared ptr const data // Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
output.header = cloud->header; output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ())); pub_output_.publish(ros_ptr(output.makeShared()));
normals->header = cloud->header; normals->header = cloud->header;
pub_normals_.publish (ros_ptr(normals)); pub_normals_.publish(ros_ptr(normals));
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level) pcl_ros::MovingLeastSquares::config_callback(MLSConfig & config, uint32_t level)
{ {
// \Note Zoli, shouldn't this be implemented in MLS too? // \Note Zoli, shouldn't this be implemented in MLS too?
/*if (k_ != config.k_search) /*if (k_ != config.k_search)
@ -198,34 +218,33 @@ pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
k_ = config.k_search; k_ = config.k_search;
NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_); NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_);
}*/ }*/
if (search_radius_ != config.search_radius) if (search_radius_ != config.search_radius) {
{
search_radius_ = config.search_radius; search_radius_ = config.search_radius;
NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_); NODELET_DEBUG("[config_callback] Setting the search radius: %f.", search_radius_);
impl_.setSearchRadius (search_radius_); impl_.setSearchRadius(search_radius_);
} }
if (spatial_locator_type_ != config.spatial_locator) if (spatial_locator_type_ != config.spatial_locator) {
{
spatial_locator_type_ = config.spatial_locator; spatial_locator_type_ = config.spatial_locator;
NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_); NODELET_DEBUG(
"[config_callback] Setting the spatial locator to type: %d.",
spatial_locator_type_);
} }
if (use_polynomial_fit_ != config.use_polynomial_fit) if (use_polynomial_fit_ != config.use_polynomial_fit) {
{
use_polynomial_fit_ = config.use_polynomial_fit; use_polynomial_fit_ = config.use_polynomial_fit;
NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_); NODELET_DEBUG(
impl_.setPolynomialFit (use_polynomial_fit_); "[config_callback] Setting the use_polynomial_fit flag to: %d.",
use_polynomial_fit_);
impl_.setPolynomialFit(use_polynomial_fit_);
} }
if (polynomial_order_ != config.polynomial_order) if (polynomial_order_ != config.polynomial_order) {
{
polynomial_order_ = config.polynomial_order; polynomial_order_ = config.polynomial_order;
NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_); NODELET_DEBUG("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
impl_.setPolynomialOrder (polynomial_order_); impl_.setPolynomialOrder(polynomial_order_);
} }
if (gaussian_parameter_ != config.gaussian_parameter) if (gaussian_parameter_ != config.gaussian_parameter) {
{
gaussian_parameter_ = config.gaussian_parameter; gaussian_parameter_ = config.gaussian_parameter;
NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_); NODELET_DEBUG("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_); impl_.setSqrGaussParam(gaussian_parameter_ * gaussian_parameter_);
} }
} }

2
pcl_ros/src/pcl_ros/surface/surface.cpp
View File

@ -44,5 +44,3 @@
// Include the implementations here instead of compiling them separately to speed up compile time // Include the implementations here instead of compiling them separately to speed up compile time
//#include "convex_hull.cpp" //#include "convex_hull.cpp"
//#include "moving_least_squares.cpp" //#include "moving_least_squares.cpp"

72
pcl_ros/src/test/test_tf_message_filter_pcl.cpp
View File

@ -58,7 +58,7 @@ typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PCDType;
/// Sets pcl_stamp from stamp, BUT alters stamp /// Sets pcl_stamp from stamp, BUT alters stamp
/// a little to round it to millisecond. This is because converting back /// a little to round it to millisecond. This is because converting back
/// and forth from pcd to ros time induces some rounding errors. /// and forth from pcd to ros time induces some rounding errors.
void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp) void setStamp(ros::Time & stamp, std::uint64_t & pcl_stamp)
{ {
// round to millisecond // round to millisecond
static const uint32_t mult = 1e6; static const uint32_t mult = 1e6;
@ -71,7 +71,7 @@ void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp)
{ {
ros::Time t; ros::Time t;
pcl_conversions::fromPCL(pcl_stamp, t); pcl_conversions::fromPCL(pcl_stamp, t);
ROS_ASSERT_MSG(t==stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec); ROS_ASSERT_MSG(t == stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec);
} }
} }
@ -79,18 +79,18 @@ class Notification
{ {
public: public:
Notification(int expected_count) Notification(int expected_count)
: count_(0) : count_(0),
, expected_count_(expected_count) expected_count_(expected_count),
, failure_count_(0) failure_count_(0)
{ {
} }
void notify(const PCDType::ConstPtr& message) void notify(const PCDType::ConstPtr & message)
{ {
++count_; ++count_;
} }
void failure(const PCDType::ConstPtr& message, FilterFailureReason reason) void failure(const PCDType::ConstPtr & message, FilterFailureReason reason)
{ {
++failure_count_; ++failure_count_;
} }
@ -144,7 +144,11 @@ TEST(MessageFilter, preexistingTransforms)
ros::Time stamp = ros::Time::now(); ros::Time stamp = ros::Time::now();
setStamp(stamp, msg->header.stamp); setStamp(stamp, msg->header.stamp);
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf::StampedTransform transform(tf::Transform(
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
1, 2,
3)), stamp, "frame1",
"frame2");
tf_client.setTransform(transform); tf_client.setTransform(transform);
msg->header.frame_id = "frame2"; msg->header.frame_id = "frame2";
@ -169,7 +173,11 @@ TEST(MessageFilter, postTransforms)
EXPECT_EQ(0, n.count_); EXPECT_EQ(0, n.count_);
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf::StampedTransform transform(tf::Transform(
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
1, 2,
3)), stamp, "frame1",
"frame2");
tf_client.setTransform(transform); tf_client.setTransform(transform);
ros::WallDuration(0.1).sleep(); ros::WallDuration(0.1).sleep();
@ -190,8 +198,7 @@ TEST(MessageFilter, queueSize)
std::uint64_t pcl_stamp; std::uint64_t pcl_stamp;
setStamp(stamp, pcl_stamp); setStamp(stamp, pcl_stamp);
for (int i = 0; i < 20; ++i) for (int i = 0; i < 20; ++i) {
{
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);
msg->header.stamp = pcl_stamp; msg->header.stamp = pcl_stamp;
msg->header.frame_id = "frame2"; msg->header.frame_id = "frame2";
@ -202,7 +209,11 @@ TEST(MessageFilter, queueSize)
EXPECT_EQ(0, n.count_); EXPECT_EQ(0, n.count_);
EXPECT_EQ(10, n.failure_count_); EXPECT_EQ(10, n.failure_count_);
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf::StampedTransform transform(tf::Transform(
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
1, 2,
3)), stamp, "frame1",
"frame2");
tf_client.setTransform(transform); tf_client.setTransform(transform);
ros::WallDuration(0.1).sleep(); ros::WallDuration(0.1).sleep();
@ -224,7 +235,11 @@ TEST(MessageFilter, setTargetFrame)
setStamp(stamp, msg->header.stamp); setStamp(stamp, msg->header.stamp);
msg->header.frame_id = "frame2"; msg->header.frame_id = "frame2";
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2"); tf::StampedTransform transform(tf::Transform(
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
1, 2,
3)), stamp, "frame1000",
"frame2");
tf_client.setTransform(transform); tf_client.setTransform(transform);
filter.add(msg); filter.add(msg);
@ -249,7 +264,11 @@ TEST(MessageFilter, multipleTargetFrames)
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);
setStamp(stamp, msg->header.stamp); setStamp(stamp, msg->header.stamp);
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3"); tf::StampedTransform transform(tf::Transform(
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
1, 2,
3)), stamp, "frame1",
"frame3");
tf_client.setTransform(transform); tf_client.setTransform(transform);
msg->header.frame_id = "frame3"; msg->header.frame_id = "frame3";
@ -283,7 +302,11 @@ TEST(MessageFilter, tolerance)
ros::Time stamp = ros::Time::now(); ros::Time stamp = ros::Time::now();
std::uint64_t pcl_stamp; std::uint64_t pcl_stamp;
setStamp(stamp, pcl_stamp); setStamp(stamp, pcl_stamp);
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf::StampedTransform transform(tf::Transform(
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
1, 2,
3)), stamp, "frame1",
"frame2");
tf_client.setTransform(transform); tf_client.setTransform(transform);
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);
@ -295,7 +318,7 @@ TEST(MessageFilter, tolerance)
//ros::Time::setNow(ros::Time::now() + ros::Duration(0.1)); //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
transform.stamp_ += offset*1.1; transform.stamp_ += offset * 1.1;
tf_client.setTransform(transform); tf_client.setTransform(transform);
ros::WallDuration(0.1).sleep(); ros::WallDuration(0.1).sleep();
@ -323,7 +346,11 @@ TEST(MessageFilter, outTheBackFailure)
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);
setStamp(stamp, msg->header.stamp); setStamp(stamp, msg->header.stamp);
tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf::StampedTransform transform(tf::Transform(
tf::Quaternion(0, 0, 0, 1), tf::Vector3(
1, 2,
3)), stamp, "frame1",
"frame2");
tf_client.setTransform(transform); tf_client.setTransform(transform);
transform.stamp_ = stamp + ros::Duration(10000); transform.stamp_ = stamp + ros::Duration(10000);
@ -359,16 +386,17 @@ TEST(MessageFilter, removeCallback)
// TF filters; no data needs to be published // TF filters; no data needs to be published
boost::scoped_ptr<tf::TransformListener> tf_listener; boost::scoped_ptr<tf::TransformListener> tf_listener;
boost::scoped_ptr<tf::MessageFilter<PCDType> > tf_filter; boost::scoped_ptr<tf::MessageFilter<PCDType>> tf_filter;
spinner.start(); spinner.start();
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
tf_listener.reset(new tf::TransformListener()); tf_listener.reset(new tf::TransformListener());
// Have callback fire at high rate to increase chances of race condition // Have callback fire at high rate to increase chances of race condition
tf_filter.reset( tf_filter.reset(
new tf::MessageFilter<PCDType>(*tf_listener, new tf::MessageFilter<PCDType>(
"map", 5, threaded_nh, *tf_listener,
ros::Duration(0.000001))); "map", 5, threaded_nh,
ros::Duration(0.000001)));
// Sleep and reset; sleeping increases chances of race condition // Sleep and reset; sleeping increases chances of race condition
ros::Duration(0.001).sleep(); ros::Duration(0.001).sleep();
@ -378,7 +406,7 @@ TEST(MessageFilter, removeCallback)
spinner.stop(); spinner.stop();
} }
int main(int argc, char** argv) int main(int argc, char ** argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);

358
pcl_ros/src/transforms.cpp
View File

@ -45,159 +45,145 @@ namespace pcl_ros
{ {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool bool
transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, transformPointCloud(
sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener) const std::string & target_frame, const sensor_msgs::PointCloud2 & in,
sensor_msgs::PointCloud2 & out, const tf::TransformListener & tf_listener)
{ {
if (in.header.frame_id == target_frame) if (in.header.frame_id == target_frame) {
{
out = in; out = in;
return (true); return true;
} }
// Get the TF transform // Get the TF transform
tf::StampedTransform transform; tf::StampedTransform transform;
try try {
{ tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform);
tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform); } catch (tf::LookupException & e) {
} ROS_ERROR("%s", e.what());
catch (tf::LookupException &e) return false;
{ } catch (tf::ExtrapolationException & e) {
ROS_ERROR ("%s", e.what ()); ROS_ERROR("%s", e.what());
return (false); return false;
}
catch (tf::ExtrapolationException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
} }
// Convert the TF transform to Eigen format // Convert the TF transform to Eigen format
Eigen::Matrix4f eigen_transform; Eigen::Matrix4f eigen_transform;
transformAsMatrix (transform, eigen_transform); transformAsMatrix(transform, eigen_transform);
transformPointCloud (eigen_transform, in, out); transformPointCloud(eigen_transform, in, out);
out.header.frame_id = target_frame; out.header.frame_id = target_frame;
return (true); return true;
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, transformPointCloud(
const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) const std::string & target_frame, const tf::Transform & net_transform,
const sensor_msgs::PointCloud2 & in, sensor_msgs::PointCloud2 & out)
{ {
if (in.header.frame_id == target_frame) if (in.header.frame_id == target_frame) {
{
out = in; out = in;
return; return;
} }
// Get the transformation // Get the transformation
Eigen::Matrix4f transform; Eigen::Matrix4f transform;
transformAsMatrix (net_transform, transform); transformAsMatrix(net_transform, transform);
transformPointCloud (transform, in, out); transformPointCloud(transform, in, out);
out.header.frame_id = target_frame; out.header.frame_id = target_frame;
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, transformPointCloud(
sensor_msgs::PointCloud2 &out) const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in,
sensor_msgs::PointCloud2 & out)
{ {
// Get X-Y-Z indices // Get X-Y-Z indices
int x_idx = pcl::getFieldIndex (in, "x"); int x_idx = pcl::getFieldIndex(in, "x");
int y_idx = pcl::getFieldIndex (in, "y"); int y_idx = pcl::getFieldIndex(in, "y");
int z_idx = pcl::getFieldIndex (in, "z"); int z_idx = pcl::getFieldIndex(in, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1) if (x_idx == -1 || y_idx == -1 || z_idx == -1) {
{ ROS_ERROR("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
return; return;
} }
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
{ {
ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); ROS_ERROR("X-Y-Z coordinates not floats. Currently only floats are supported.");
return; return;
} }
// Check if distance is available // Check if distance is available
int dist_idx = pcl::getFieldIndex (in, "distance"); int dist_idx = pcl::getFieldIndex(in, "distance");
// Copy the other data // Copy the other data
if (&in != &out) if (&in != &out) {
{
out.header = in.header; out.header = in.header;
out.height = in.height; out.height = in.height;
out.width = in.width; out.width = in.width;
out.fields = in.fields; out.fields = in.fields;
out.is_bigendian = in.is_bigendian; out.is_bigendian = in.is_bigendian;
out.point_step = in.point_step; out.point_step = in.point_step;
out.row_step = in.row_step; out.row_step = in.row_step;
out.is_dense = in.is_dense; out.is_dense = in.is_dense;
out.data.resize (in.data.size ()); out.data.resize(in.data.size());
// Copy everything as it's faster than copying individual elements // Copy everything as it's faster than copying individual elements
memcpy (&out.data[0], &in.data[0], in.data.size ()); memcpy(&out.data[0], &in.data[0], in.data.size());
} }
Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0); Eigen::Array4i xyz_offset(in.fields[x_idx].offset, in.fields[y_idx].offset,
in.fields[z_idx].offset, 0);
for (size_t i = 0; i < in.width * in.height; ++i) for (size_t i = 0; i < in.width * in.height; ++i) {
{ Eigen::Vector4f pt(*(float *)&in.data[xyz_offset[0]], *(float *)&in.data[xyz_offset[1]],
Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); *(float *)&in.data[xyz_offset[2]], 1);
Eigen::Vector4f pt_out; Eigen::Vector4f pt_out;
bool max_range_point = false; bool max_range_point = false;
int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset;
float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); float * distance_ptr = (dist_idx < 0 ? NULL : (float *)(&in.data[distance_ptr_offset]));
if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
{ if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point
{
pt_out = pt; pt_out = pt;
} } else { // max range point
else // max range point
{
pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt[0] = *distance_ptr; // Replace x with the x value saved in distance
pt_out = transform * pt; pt_out = transform * pt;
max_range_point = true; max_range_point = true;
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n"; //std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
} }
} } else {
else
{
pt_out = transform * pt; pt_out = transform * pt;
} }
if (max_range_point) if (max_range_point) {
{
// Save x value in distance again // Save x value in distance again
*(float*)(&out.data[distance_ptr_offset]) = pt_out[0]; *(float *)(&out.data[distance_ptr_offset]) = pt_out[0];
pt_out[0] = std::numeric_limits<float>::quiet_NaN(); pt_out[0] = std::numeric_limits<float>::quiet_NaN();
} }
memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); memcpy(&out.data[xyz_offset[0]], &pt_out[0], sizeof(float));
memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); memcpy(&out.data[xyz_offset[1]], &pt_out[1], sizeof(float));
memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); memcpy(&out.data[xyz_offset[2]], &pt_out[2], sizeof(float));
xyz_offset += in.point_step; xyz_offset += in.point_step;
} }
// Check if the viewpoint information is present // Check if the viewpoint information is present
int vp_idx = pcl::getFieldIndex (in, "vp_x"); int vp_idx = pcl::getFieldIndex(in, "vp_x");
if (vp_idx != -1) if (vp_idx != -1) {
{
// Transform the viewpoint info too // Transform the viewpoint info too
for (size_t i = 0; i < out.width * out.height; ++i) for (size_t i = 0; i < out.width * out.height; ++i) {
{ float * pstep = (float *)&out.data[i * out.point_step + out.fields[vp_idx].offset];
float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
// Assume vp_x, vp_y, vp_z are consecutive // Assume vp_x, vp_y, vp_z are consecutive
Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1);
Eigen::Vector4f vp_out = transform * vp_in; Eigen::Vector4f vp_out = transform * vp_in;
pstep[0] = vp_out[0]; pstep[0] = vp_out[0];
@ -209,73 +195,193 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat)
{ {
double mv[12]; double mv[12];
bt.getBasis ().getOpenGLSubMatrix (mv); bt.getBasis().getOpenGLSubMatrix(mv);
tf::Vector3 origin = bt.getOrigin (); tf::Vector3 origin = bt.getOrigin();
out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; out_mat(0, 0) = mv[0]; out_mat(0, 1) = mv[4]; out_mat(0, 2) = mv[8];
out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; out_mat(1, 0) = mv[1]; out_mat(1, 1) = mv[5]; out_mat(1, 2) = mv[9];
out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; out_mat(2, 0) = mv[2]; out_mat(2, 1) = mv[6]; out_mat(2, 2) = mv[10];
out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; out_mat(3, 0) = out_mat(3, 1) = out_mat(3, 2) = 0; out_mat(3, 3) = 1;
out_mat (0, 3) = origin.x (); out_mat(0, 3) = origin.x();
out_mat (1, 3) = origin.y (); out_mat(1, 3) = origin.y();
out_mat (2, 3) = origin.z (); out_mat(2, 3) = origin.z();
} }
} // namespace pcl_ros } // namespace pcl_ros
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &); template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &); const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &); const tf::Transform &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::Transform &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::Transform &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &); template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &); const std::string &,
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &); const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
const std::string &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
const std::string &,
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::TransformListener &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &); template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &); const std::string &,
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &); const ros::Time &,
const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloud<pcl::PointXYZ> (const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::Transform &); template void pcl_ros::transformPointCloud<pcl::PointXYZ>(
template void pcl_ros::transformPointCloud<pcl::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &); const pcl::PointCloud<pcl::PointXYZ> &,
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::Transform &); pcl::PointCloud<pcl::PointXYZ> &,
template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::Transform &); const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::InterestPoint> (const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::Transform &); template void pcl_ros::transformPointCloud<pcl::PointXYZI>(
template void pcl_ros::transformPointCloud<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &); const pcl::PointCloud<pcl::PointXYZI> &,
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &); pcl::PointCloud<pcl::PointXYZI> &,
template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &); const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointWithRange> (const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::Transform &); template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::Transform &); const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::InterestPoint>(
const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointNormal>(
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointWithRange>(
const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
const tf::Transform &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &); const std::string &,
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &); const pcl::PointCloud<pcl::PointXYZ> &,
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZ> &,
template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &); const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &); const std::string &,
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &); const pcl::PointCloud<pcl::PointXYZI> &,
template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZI> &,
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &); const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
const std::string &,
const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
const std::string &,
const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
const std::string &,
const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
const std::string &,
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
const std::string &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
const std::string &,
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
const std::string &,
const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
const std::string &,
const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
const tf::TransformListener &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZ> &, const std::string &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZI> &, const std::string &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &); const std::string &, const ros::Time &,
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBA> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &); const pcl::PointCloud<pcl::PointXYZ> &,
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGB> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &); const std::string &,
template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::InterestPoint> &, const std::string &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZ> &,
template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &); const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &); template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &); const std::string &, const ros::Time &,
template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithRange> &, const std::string &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &); const pcl::PointCloud<pcl::PointXYZI> &,
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithViewpoint> &, const std::string &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &); const std::string &,
pcl::PointCloud<pcl::PointXYZI> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::PointXYZRGBA> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGBA> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
const std::string &, const ros::Time &,
const pcl::PointCloud<pcl::PointXYZRGB> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGB> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::InterestPoint> &, const std::string &,
pcl::PointCloud<pcl::InterestPoint> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
const std::string &, const ros::Time &,
const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::PointWithRange> &, const std::string &,
pcl::PointCloud<pcl::PointWithRange> &, const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
const std::string &,
const ros::Time &,
const pcl::PointCloud<pcl::PointWithViewpoint> &, const std::string &,
pcl::PointCloud<pcl::PointWithViewpoint> &, const tf::TransformListener &);

101
pcl_ros/tools/bag_to_pcd.cpp
View File

@ -61,14 +61,15 @@ typedef PointCloud::ConstPtr PointCloudConstPtr;
/* ---[ */ /* ---[ */
int int
main (int argc, char** argv) main(int argc, char ** argv)
{ {
ros::init (argc, argv, "bag_to_pcd"); ros::init(argc, argv, "bag_to_pcd");
if (argc < 4) if (argc < 4) {
{ std::cerr << "Syntax is: " << argv[0] <<
std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl; " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl; std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" <<
return (-1); std::endl;
return -1;
} }
// TF // TF
@ -79,87 +80,77 @@ int
rosbag::View view; rosbag::View view;
rosbag::View::iterator view_it; rosbag::View::iterator view_it;
try try {
{ bag.open(argv[1], rosbag::bagmode::Read);
bag.open (argv[1], rosbag::bagmode::Read); } catch (rosbag::BagException) {
}
catch (rosbag::BagException)
{
std::cerr << "Error opening file " << argv[1] << std::endl; std::cerr << "Error opening file " << argv[1] << std::endl;
return (-1); return -1;
} }
view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2")); view.addQuery(bag, rosbag::TypeQuery("sensor_msgs/PointCloud2"));
view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage")); view.addQuery(bag, rosbag::TypeQuery("tf/tfMessage"));
view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage")); view.addQuery(bag, rosbag::TypeQuery("tf2_msgs/TFMessage"));
view_it = view.begin (); view_it = view.begin();
std::string output_dir = std::string (argv[3]); std::string output_dir = std::string(argv[3]);
boost::filesystem::path outpath (output_dir); boost::filesystem::path outpath(output_dir);
if (!boost::filesystem::exists (outpath)) if (!boost::filesystem::exists(outpath)) {
{ if (!boost::filesystem::create_directories(outpath)) {
if (!boost::filesystem::create_directories (outpath))
{
std::cerr << "Error creating directory " << output_dir << std::endl; std::cerr << "Error creating directory " << output_dir << std::endl;
return (-1); return -1;
} }
std::cerr << "Creating directory " << output_dir << std::endl; std::cerr << "Creating directory " << output_dir << std::endl;
} }
// Add the PointCloud2 handler // Add the PointCloud2 handler
std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl; std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " <<
output_dir << std::endl;
PointCloud cloud_t; PointCloud cloud_t;
ros::Duration r (0.001); ros::Duration r(0.001);
// Loop over the whole bag file // Loop over the whole bag file
while (view_it != view.end ()) while (view_it != view.end()) {
{
// Handle TF messages first // Handle TF messages first
tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> (); tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage>();
if (tf != NULL) if (tf != NULL) {
{ tf_broadcaster.sendTransform(tf->transforms);
tf_broadcaster.sendTransform (tf->transforms); ros::spinOnce();
ros::spinOnce (); r.sleep();
r.sleep (); } else {
}
else
{
// Get the PointCloud2 message // Get the PointCloud2 message
PointCloudConstPtr cloud = view_it->instantiate<PointCloud> (); PointCloudConstPtr cloud = view_it->instantiate<PointCloud>();
if (cloud == NULL) if (cloud == NULL) {
{
++view_it; ++view_it;
continue; continue;
} }
// If a target_frame was specified // If a target_frame was specified
if(argc > 4) if (argc > 4) {
{
// Transform it // Transform it
if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener)) if (!pcl_ros::transformPointCloud(argv[4], *cloud, cloud_t, tf_listener)) {
{ ++view_it;
++view_it; continue;
continue;
} }
} } else {
else
{
// Else, don't transform it // Else, don't transform it
cloud_t = *cloud; cloud_t = *cloud;
} }
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::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; std::stringstream ss;
ss << output_dir << "/" << cloud_t.header.stamp << ".pcd"; ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
std::cerr << "Data saved to " << ss.str () << std::endl; std::cerr << "Data saved to " << ss.str() << std::endl;
pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (), pcl::io::savePCDFile(
Eigen::Quaternionf::Identity (), true); ss.str(), cloud_t, Eigen::Vector4f::Zero(),
Eigen::Quaternionf::Identity(), true);
} }
// Increment the iterator // Increment the iterator
++view_it; ++view_it;
} }
return (0); return 0;
} }
/* ]--- */ /* ]--- */

38
pcl_ros/tools/convert_pcd_to_image.cpp
View File

@ -56,41 +56,37 @@
/* ---[ */ /* ---[ */
int int
main (int argc, char **argv) main(int argc, char ** argv)
{ {
ros::init (argc, argv, "image_publisher"); ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh; ros::NodeHandle nh;
ros::Publisher image_pub = nh.advertise <sensor_msgs::Image> ("output", 1); ros::Publisher image_pub = nh.advertise<sensor_msgs::Image>("output", 1);
if (argc != 2) if (argc != 2) {
{
std::cout << "usage:\n" << argv[0] << " cloud.pcd" << std::endl; std::cout << "usage:\n" << argv[0] << " cloud.pcd" << std::endl;
return 1; return 1;
} }
sensor_msgs::Image image; sensor_msgs::Image image;
sensor_msgs::PointCloud2 cloud; sensor_msgs::PointCloud2 cloud;
pcl::io::loadPCDFile (std::string (argv[1]), cloud); pcl::io::loadPCDFile(std::string(argv[1]), cloud);
try try {
{ pcl::toROSMsg(cloud, image); //convert the cloud
pcl::toROSMsg (cloud, image); //convert the cloud } catch (std::runtime_error & e) {
} ROS_ERROR_STREAM(
catch (std::runtime_error &e) "Error in converting cloud to image message: " <<
{ e.what());
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
return 1; //fail! return 1; //fail!
} }
ros::Rate loop_rate (5); ros::Rate loop_rate(5);
while (nh.ok ()) while (nh.ok()) {
{ image_pub.publish(image);
image_pub.publish (image); ros::spinOnce();
ros::spinOnce (); loop_rate.sleep();
loop_rate.sleep ();
} }
return (0); return 0;
} }
/* ]--- */ /* ]--- */

46
pcl_ros/tools/convert_pointcloud_to_image.cpp
View File

@ -54,37 +54,37 @@ class PointCloudToImage
{ {
public: public:
void void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) cloud_cb(const sensor_msgs::PointCloud2ConstPtr & cloud)
{ {
if (cloud->height <= 1) if (cloud->height <= 1) {
{
ROS_ERROR("Input point cloud is not organized, ignoring!"); ROS_ERROR("Input point cloud is not organized, ignoring!");
return; return;
} }
try try {
{ pcl::toROSMsg(*cloud, image_); //convert the cloud
pcl::toROSMsg (*cloud, image_); //convert the cloud
image_.header = cloud->header; image_.header = cloud->header;
image_pub_.publish (image_); //publish our cloud image image_pub_.publish(image_); //publish our cloud image
} } catch (std::runtime_error & e) {
catch (std::runtime_error &e) ROS_ERROR_STREAM(
{ "Error in converting cloud to image message: " <<
ROS_ERROR_STREAM("Error in converting cloud to image message: " e.what());
<< e.what());
} }
} }
PointCloudToImage () : cloud_topic_("input"),image_topic_("output") PointCloudToImage()
: cloud_topic_("input"), image_topic_("output")
{ {
sub_ = nh_.subscribe (cloud_topic_, 30, sub_ = nh_.subscribe(
&PointCloudToImage::cloud_cb, this); cloud_topic_, 30,
image_pub_ = nh_.advertise<sensor_msgs::Image> (image_topic_, 30); &PointCloudToImage::cloud_cb, this);
image_pub_ = nh_.advertise<sensor_msgs::Image>(image_topic_, 30);
//print some info about the node //print some info about the node
std::string r_ct = nh_.resolveName (cloud_topic_); std::string r_ct = nh_.resolveName(cloud_topic_);
std::string r_it = nh_.resolveName (image_topic_); std::string r_it = nh_.resolveName(image_topic_);
ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct ); ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct);
ROS_INFO_STREAM("Publishing image on topic " << r_it ); ROS_INFO_STREAM("Publishing image on topic " << r_it);
} }
private: private:
ros::NodeHandle nh_; ros::NodeHandle nh_;
sensor_msgs::Image image_; //cache the image message sensor_msgs::Image image_; //cache the image message
@ -95,10 +95,10 @@ private:
}; };
int int
main (int argc, char **argv) main(int argc, char ** argv)
{ {
ros::init (argc, argv, "convert_pointcloud_to_image"); ros::init(argc, argv, "convert_pointcloud_to_image");
PointCloudToImage pci; //this loads up the node PointCloudToImage pci; //this loads up the node
ros::spin (); //where she stops nobody knows ros::spin(); //where she stops nobody knows
return 0; return 0;
} }

167
pcl_ros/tools/pcd_to_pointcloud.cpp
View File

@ -60,113 +60,112 @@ using namespace std;
class PCDGenerator class PCDGenerator
{ {
protected: protected:
string tf_frame_; string tf_frame_;
ros::NodeHandle nh_; ros::NodeHandle nh_;
ros::NodeHandle private_nh_; ros::NodeHandle private_nh_;
public:
// ROS messages public:
sensor_msgs::PointCloud2 cloud_; // ROS messages
sensor_msgs::PointCloud2 cloud_;
string file_name_, cloud_topic_; string file_name_, cloud_topic_;
double wait_; double wait_;
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_; pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~") PCDGenerator()
{ : tf_frame_("/base_link"), private_nh_("~")
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1 {
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
cloud_topic_ = "cloud_pcd"; cloud_topic_ = "cloud_pcd";
pub_.advertise (nh_, cloud_topic_.c_str (), 1); pub_.advertise(nh_, cloud_topic_.c_str(), 1);
private_nh_.param("frame_id", tf_frame_, std::string("/base_link")); 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()); 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;
}
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Start // Spin (!)
int bool spin()
start () {
{ int nr_points = cloud_.width * cloud_.height;
if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1) string fields_list = pcl::getFieldsList(cloud_);
return (-1); double interval = wait_ * 1e+6;
cloud_.header.frame_id = tf_frame_; while (nh_.ok()) {
return (0); 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) {
// Spin (!) ROS_DEBUG("Publishing data to %d subscribers.", pub_.getNumSubscribers());
bool spin () pub_.publish(cloud_);
{ } else {
int nr_points = cloud_.width * cloud_.height; // check once a second if there is any subscriber
string fields_list = pcl::getFieldsList (cloud_); ros::Duration(1).sleep();
double interval = wait_ * 1e+6; continue;
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 ()); std::this_thread::sleep_for(std::chrono::microseconds(static_cast<uint32_t>(interval)));
cloud_.header.stamp = ros::Time::now ();
if (interval == 0) { // We only publish once if a 0 seconds interval is given
if (pub_.getNumSubscribers () > 0) // Give subscribers 3 seconds until point cloud decays... a little ugly!
{ ros::Duration(3.0).sleep();
ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ()); break;
pub_.publish (cloud_);
}
else
{
// check once a second if there is any subscriber
ros::Duration (1).sleep ();
continue;
}
std::this_thread::sleep_for(std::chrono::microseconds(static_cast<uint32_t>(interval)));
if (interval == 0) // We only publish once if a 0 seconds interval is given
{
// Give subscribers 3 seconds until point cloud decays... a little ugly!
ros::Duration (3.0).sleep ();
break;
}
} }
return (true);
} }
return true;
}
}; };
/* ---[ */ /* ---[ */
int int
main (int argc, char** argv) main(int argc, char ** argv)
{ {
if (argc < 2) if (argc < 2) {
{ std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" <<
std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl; std::endl;
return (-1); return -1;
} }
ros::init (argc, argv, "pcd_to_pointcloud"); ros::init(argc, argv, "pcd_to_pointcloud");
PCDGenerator c; PCDGenerator c;
c.file_name_ = string (argv[1]); c.file_name_ = string(argv[1]);
// check if publishing interval is given // check if publishing interval is given
if (argc == 2) if (argc == 2) {
{ c.wait_ = 0;
c.wait_ = 0; } else {
} c.wait_ = atof(argv[2]);
else
{
c.wait_ = 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); 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;
} }
/* ]--- */ /* ]--- */

197
pcl_ros/tools/pointcloud_to_pcd.cpp
View File

@ -64,124 +64,121 @@ Cloud Data) file format.
**/ **/
class PointCloudToPCD class PointCloudToPCD
{ {
protected: protected:
ros::NodeHandle nh_; ros::NodeHandle nh_;
private: private:
std::string prefix_; std::string prefix_;
bool binary_; bool binary_;
bool compressed_; bool compressed_;
std::string fixed_frame_; std::string fixed_frame_;
tf2_ros::Buffer tf_buffer_; tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_; tf2_ros::TransformListener tf_listener_;
public: public:
string cloud_topic_; string cloud_topic_;
ros::Subscriber sub_; ros::Subscriber sub_;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Callback // Callback
void void
cloud_cb (const boost::shared_ptr<const pcl::PCLPointCloud2>& cloud) cloud_cb(const boost::shared_ptr<const pcl::PCLPointCloud2> & cloud)
{ {
if ((cloud->width * cloud->height) == 0) 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());
Eigen::Vector4f v = Eigen::Vector4f::Zero();
Eigen::Quaternionf q = Eigen::Quaternionf::Identity();
if (!fixed_frame_.empty()) {
if (!tf_buffer_.canTransform(
fixed_frame_, cloud->header.frame_id,
pcl_conversions::fromPCL(cloud->header.stamp), ros::Duration(3.0)))
{
ROS_WARN("Could not get transform!");
return; 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 ());
Eigen::Vector4f v = Eigen::Vector4f::Zero ();
Eigen::Quaternionf q = Eigen::Quaternionf::Identity ();
if (!fixed_frame_.empty ()) {
if (!tf_buffer_.canTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp), ros::Duration (3.0))) {
ROS_WARN("Could not get transform!");
return;
}
Eigen::Affine3d transform;
transform = tf2::transformToEigen (tf_buffer_.lookupTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp)));
v = Eigen::Vector4f::Zero ();
v.head<3> () = transform.translation ().cast<float> ();
q = transform.rotation ().cast<float> ();
} }
std::stringstream ss; Eigen::Affine3d transform;
ss << prefix_ << cloud->header.stamp << ".pcd"; transform =
ROS_INFO ("Data saved to %s", ss.str ().c_str ()); tf2::transformToEigen(
tf_buffer_.lookupTransform(
pcl::PCDWriter writer; fixed_frame_, cloud->header.frame_id,
if(binary_) pcl_conversions::fromPCL(cloud->header.stamp)));
{ v = Eigen::Vector4f::Zero();
if(compressed_) v.head<3>() = transform.translation().cast<float>();
{ q = transform.rotation().cast<float>();
writer.writeBinaryCompressed (ss.str (), *cloud, v, q);
}
else
{
writer.writeBinary (ss.str (), *cloud, v, q);
}
}
else
{
writer.writeASCII (ss.str (), *cloud, v, q, 8);
}
} }
//////////////////////////////////////////////////////////////////////////////// std::stringstream ss;
PointCloudToPCD () : binary_(false), compressed_(false), tf_listener_(tf_buffer_) ss << prefix_ << cloud->header.stamp << ".pcd";
{ ROS_INFO("Data saved to %s", ss.str().c_str());
// 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_);
}
priv_nh.getParam ("fixed_frame", fixed_frame_); pcl::PCDWriter writer;
priv_nh.getParam ("binary", binary_); if (binary_) {
priv_nh.getParam ("compressed", compressed_); if (compressed_) {
if(binary_) writer.writeBinaryCompressed(ss.str(), *cloud, v, q);
{ } else {
if(compressed_) writer.writeBinary(ss.str(), *cloud, v, q);
{ }
ROS_INFO_STREAM ("Saving as binary compressed PCD"); } else {
} writer.writeASCII(ss.str(), *cloud, v, q, 8);
else
{
ROS_INFO_STREAM ("Saving as binary PCD");
}
}
else
{
ROS_INFO_STREAM ("Saving as binary PCD");
}
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 ());
} }
}
////////////////////////////////////////////////////////////////////////////////
PointCloudToPCD()
: binary_(false), compressed_(false), tf_listener_(tf_buffer_)
{
// 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_);
}
priv_nh.getParam("fixed_frame", fixed_frame_);
priv_nh.getParam("binary", binary_);
priv_nh.getParam("compressed", compressed_);
if (binary_) {
if (compressed_) {
ROS_INFO_STREAM("Saving as binary compressed PCD");
} else {
ROS_INFO_STREAM("Saving as binary PCD");
}
} else {
ROS_INFO_STREAM("Saving as binary PCD");
}
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 int
main (int argc, char** argv) main(int argc, char ** argv)
{ {
ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName); ros::init(argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
PointCloudToPCD b; PointCloudToPCD b;
ros::spin (); ros::spin();
return (0); return 0;
} }
/* ]--- */ /* ]--- */