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
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \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.
*
* @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.
* \author Radu Bogdan Rusu
*/
class BoundaryEstimation: public FeatureFromNormals
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \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.
*
* @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.
* \author Radu Bogdan Rusu
*/
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:
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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 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
* are applicable to all features are defined here as static methods.
* \author Radu Bogdan Rusu
///////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Feature represents the base feature class. Some generic 3D operations that
* are applicable to all features are defined here as static methods.
* \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:
typedef pcl::KdTree<pcl::PointXYZ> KdTree;
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
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));
}
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
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_;
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
/** \brief Nodelet initialization routine. */
virtual void onInit();
/** \brief Empty constructor. */
Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0),
use_surface_(false), spatial_locator_type_(-1)
{};
/** \brief NodeletLazy connection routine. */
virtual void subscribe();
virtual void unsubscribe();
protected:
/** \brief The input point cloud dataset. */
//PointCloudInConstPtr input_;
/** \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);
/** \brief A pointer to the vector of point indices to use. */
//IndicesConstPtr indices_;
public:
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. */
KdTreePtr tree_;
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
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. */
int k_;
FeatureFromNormals()
: normals_() {}
/** \brief The nearest neighbors search radius for each point. */
double search_radius_;
protected:
/** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
PointCloudNConstPtr normals_;
// 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 Child initialization routine. Internal method. */
virtual bool childInit(ros::NodeHandle & nh) = 0;
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */
bool use_surface_;
/** \brief Publish an empty point cloud of the feature output type. */
virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
/** \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 Compute the feature and publish it. */
virtual void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices) = 0;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<FeatureConfig> > srv_;
private:
/** \brief The normals PointCloud subscriber filter. */
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
/** \brief Child initialization routine. Internal method. */
virtual bool childInit (ros::NodeHandle &nh) = 0;
/** \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 Publish an empty point cloud of the feature output type. */
virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
/** \brief Internal method. */
void computePublish(
const PointCloudInConstPtr &,
const PointCloudInConstPtr &,
const IndicesPtr &) {} // This should never be called
/** \brief Compute the feature and publish it. Internal method. */
virtual void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices) = 0;
/** \brief Nodelet initialization routine. */
virtual void onInit();
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (FeatureConfig &config, uint32_t level);
/** \brief NodeletLazy connection routine. */
virtual void subscribe();
virtual void unsubscribe();
/** \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. 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);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FEATURE_H_

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

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

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

@ -43,54 +43,54 @@
namespace pcl_ros
{
/** \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.
*
* @note If you use this code in any academic work, please cite:
*
* <ul>
* <li> R.B. Rusu, N. Blodow, M. Beetz.
* Fast Point Feature Histograms (FPFH) for 3D Registration.
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
* Kobe, Japan, May 12-17 2009.
* </li>
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
* Fast Geometric Point Labeling using Conditional Random Fields.
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
* St. Louis, MO, USA, October 11-15 2009.
* </li>
* </ul>
* \author Radu Bogdan Rusu
*/
class FPFHEstimationOMP : public FeatureFromNormals
/** \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.
*
* @note If you use this code in any academic work, please cite:
*
* <ul>
* <li> R.B. Rusu, N. Blodow, M. Beetz.
* Fast Point Feature Histograms (FPFH) for 3D Registration.
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
* Kobe, Japan, May 12-17 2009.
* </li>
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
* Fast Geometric Point Labeling using Conditional Random Fields.
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
* St. Louis, MO, USA, October 11-15 2009.
* </li>
* </ul>
* \author Radu Bogdan Rusu
*/
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:
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \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
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu
*/
class MomentInvariantsEstimation: public Feature
/** \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
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu
*/
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:
pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
* curvatures.
*
* @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.
* \author Radu Bogdan Rusu
*/
class NormalEstimation: public Feature
/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
* curvatures.
*
* @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.
* \author Radu Bogdan Rusu
*/
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:
/** \brief PCL implementation object. */
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
// Create the output publisher
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.
* \param cloud the input point cloud to copy the header from.
*/
void emptyPublish(const PointCloudInConstPtr & cloud);
/** \brief Child initialization routine. Internal method. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \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 Compute the feature and publish it. */
void computePublish (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
* curvatures, in parallel, using the OpenMP standard.
* \author Radu Bogdan Rusu
*/
class NormalEstimationOMP: public Feature
/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
* curvatures, in parallel, using the OpenMP standard.
* \author Radu Bogdan Rusu
*/
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:
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \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.
* \author Radu Bogdan Rusu
*/
class NormalEstimationTBB: public Feature
/** \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.
* \author Radu Bogdan Rusu
*/
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:
pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloud> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
//#endif // HAVE_TBB
#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
{
/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
* containing points and normals.
*
* @note If you use this code in any academic work, please cite:
*
* <ul>
* <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
* Aligning Point Cloud Views using Persistent Feature Histograms.
* In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
* Nice, France, September 22-26 2008.
* </li>
* <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
* 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),
* Hanoi, Vietnam, December 17-20 2008.
* </li>
* </ul>
*
* @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).
* \author Radu Bogdan Rusu
*/
class PFHEstimation : public FeatureFromNormals
/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
* containing points and normals.
*
* @note If you use this code in any academic work, please cite:
*
* <ul>
* <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
* Aligning Point Cloud Views using Persistent Feature Histograms.
* In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
* Nice, France, September 22-26 2008.
* </li>
* <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
* 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),
* Hanoi, Vietnam, December 17-20 2008.
* </li>
* </ul>
*
* @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).
* \author Radu Bogdan Rusu
*/
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:
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
* 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
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu, Jared Glover
*/
class PrincipalCurvaturesEstimation : public FeatureFromNormals
/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
* 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
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
* \author Radu Bogdan Rusu, Jared Glover
*/
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:
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b SHOTEstimation estimates SHOT descriptor.
*
*/
class SHOTEstimation : public FeatureFromNormals
/** \brief @b SHOTEstimation estimates SHOT descriptor.
*
*/
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:
pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_SHOT_H_

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

@ -42,37 +42,37 @@
namespace pcl_ros
{
/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP.
*/
class SHOTEstimationOMP : public FeatureFromNormals
/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP.
*/
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:
pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor 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
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
* \author Radu Bogdan Rusu
*/
class VFHEstimation : public FeatureFromNormals
/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor 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
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
* \author Radu Bogdan Rusu
*/
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:
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
// Create the output publisher
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. */
inline bool
childInit (ros::NodeHandle &nh)
{
// Create the output publisher
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
return (true);
}
/** \brief Compute the feature and publish it. */
void computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices);
/** \brief Publish an empty point cloud of the feature output type. */
void emptyPublish (const PointCloudInConstPtr &cloud);
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FEATURES_VFH_H_

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

@ -1,5 +1,5 @@
/*
*
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
@ -32,7 +32,7 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: cropbox.cpp
* $Id: cropbox.cpp
*
*/
@ -48,57 +48,59 @@
namespace pcl_ros
{
/** \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 Justin Rosen
* \author Marti Morta Garriga
/** \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 Justin Rosen
* \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:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::CropBoxConfig> > srv_; // TODO
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 Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
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.
* \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 Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback.
* \param config the dynamic reconfigure CropBoxConfig object
* \param level the dynamic reconfigure level
*/
void
config_callback (pcl_ros::CropBoxConfig &config, uint32_t level);
/** \brief Dynamic reconfigure service callback.
* \param config the dynamic reconfigure CropBoxConfig object
* \param level the dynamic reconfigure level
*/
void
config_callback(pcl_ros::CropBoxConfig & config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::CropBox<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
private:
/** \brief The PCL filter implementation used. */
pcl::CropBox<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ExtractIndices : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>> srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
class ExtractIndices : public Filter
inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > srv_;
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 Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
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.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init(ros::NodeHandle & nh, bool & has_service);
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback. */
void
config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level);
/** \brief Dynamic reconfigure service callback. */
void
config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
private:
/** \brief The PCL filter implementation used. */
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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 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
* are defined here as static methods.
* \author Radu Bogdan Rusu
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters
* are defined here as static methods.
* \author Radu Bogdan Rusu
*/
class Filter : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef 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:
typedef sensor_msgs::PointCloud2 PointCloud2;
has_service = false;
return true;
}
typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
/** \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;
Filter () {}
/** \brief Lazy transport subscribe routine. */
virtual void
subscribe();
protected:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Lazy transport unsubscribe routine. */
virtual void
unsubscribe();
message_filters::Subscriber<PointCloud2> sub_input_filter_;
/** \brief Nodelet initialization routine. */
virtual void
onInit();
/** \brief The desired user filter field name. */
std::string filter_field_name_;
/** \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);
/** \brief The minimum allowed filter value a point will be considered from. */
double filter_limit_min_;
private:
/** \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. */
double filter_limit_max_;
/** \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 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 Dynamic reconfigure service callback. */
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. */
std::string tf_input_frame_;
/** \brief PointCloud2 + Indices data callback. */
void
input_indices_callback(
const PointCloud2::ConstPtr & cloud,
const PointIndicesConstPtr & indices);
/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
std::string tf_output_frame_;
/** \brief Internal mutex. */
boost::mutex mutex_;
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init (ros::NodeHandle &nh, bool &has_service)
{
has_service = false;
return (true);
}
/** \brief Virtual abstract filter method. To be implemented by every child.
* \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use.
* \param output the resultant filtered PointCloud2
*/
virtual void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output) = 0;
/** \brief 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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_FILTER_H_

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

@ -44,55 +44,57 @@
namespace pcl_ros
{
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
* constraints.
* \author Radu Bogdan Rusu
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
* constraints.
* \author Radu Bogdan Rusu
*/
class PassThrough : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
class PassThrough : public Filter
inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_;
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 Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
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.
* \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 Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure service callback.
* \param config the dynamic reconfigure FilterConfig object
* \param level the dynamic reconfigure level
*/
void
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
/** \brief Dynamic reconfigure service callback.
* \param config the dynamic reconfigure FilterConfig object
* \param level the dynamic reconfigure level
*/
void
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::PassThrough<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
private:
/** \brief The PCL filter implementation used. */
pcl::PassThrough<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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 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
* separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
* separate PointCloud.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class ProjectInliers : public Filter
{
public:
ProjectInliers()
: model_() {}
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
class ProjectInliers : public Filter
inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
public:
ProjectInliers () : model_ () {}
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);
}
protected:
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
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 A pointer to the vector of model coefficients. */
ModelCoefficientsConstPtr model_;
private:
/** \brief A pointer to the vector of model coefficients. */
ModelCoefficientsConstPtr model_;
/** \brief The message filter subscriber for model coefficients. */
message_filters::Subscriber<ModelCoefficients> sub_model_;
/** \brief The message filter subscriber for model coefficients. */
message_filters::Subscriber<ModelCoefficients> sub_model_;
/** \brief Synchronized input, indices, and model coefficients.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
PointIndices, ModelCoefficients>>> sync_input_indices_model_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
PointIndices, ModelCoefficients>>> sync_input_indices_model_a_;
/** \brief The PCL filter implementation used. */
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
/** \brief Synchronized input, indices, and model coefficients.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_;
/** \brief The PCL filter implementation used. */
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
/** \brief Nodelet initialization routine. */
virtual void
onInit();
/** \brief Nodelet initialization routine. */
virtual void
onInit ();
/** \brief NodeletLazy connection routine. */
void subscribe();
void unsubscribe();
/** \brief NodeletLazy connection routine. */
void subscribe ();
void unsubscribe ();
/** \brief PointCloud2 + Indices + Model data callback. */
void
input_indices_model_callback(
const PointCloud2::ConstPtr & cloud,
const PointIndicesConstPtr & indices,
const ModelCoefficientsConstPtr & model);
/** \brief PointCloud2 + Indices + Model data callback. */
void
input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
const PointIndicesConstPtr &indices,
const ModelCoefficientsConstPtr &model);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
* search radius is smaller than a given K.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
* search radius is smaller than a given K.
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class RadiusOutlierRemoval : public Filter
{
protected:
/** \brief 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:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig> > srv_;
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 Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
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.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual inline bool child_init(ros::NodeHandle & nh, bool & has_service);
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual inline 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::RadiusOutlierRemovalConfig & config, uint32_t level);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
private:
/** \brief The PCL filter implementation used. */
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
* information check:
* <ul>
* <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
* Towards 3D Point Cloud Based Object Maps for Household Environments
* Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
* </ul>
*
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
* information check:
* <ul>
* <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
* Towards 3D Point Cloud Based Object Maps for Household Environments
* Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
* </ul>
*
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
* \author Radu Bogdan Rusu
*/
class StatisticalOutlierRemoval : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>> srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
class StatisticalOutlierRemoval : public Filter
inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > srv_;
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 Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
inline void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
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.
* \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 Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level);
private:
/** \brief The PCL filter implementation used. */
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
private:
/** \brief The PCL filter implementation used. */
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
* \author Radu Bogdan Rusu
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
* \author Radu Bogdan Rusu
*/
class VoxelGrid : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>> srv_;
/** \brief The PCL filter implementation used. */
pcl::VoxelGrid<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
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > srv_;
virtual void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output);
/** \brief The PCL filter implementation used. */
pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init(ros::NodeHandle & nh, bool & has_service);
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
* \param output the resultant filtered dataset
*/
virtual void
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void
config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level);
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
bool
child_init (ros::NodeHandle &nh, bool &has_service);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void
config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
template<typename PointT>
void
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
// 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.
// 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.
tf::Quaternion q = transform.getRotation ();
Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin ();
Eigen::Vector3f origin (v.x (), v.y (), v.z ());
tf::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v);
// Assemble an Eigen Transform
//Eigen::Transform3f t;
//t = translation * rotation;
transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation);
transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
template<typename PointT>
void
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
// 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.
// 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.
tf::Quaternion q = transform.getRotation ();
Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin ();
Eigen::Vector3f origin (v.x (), v.y (), v.z ());
tf::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v);
// Assemble an Eigen Transform
//Eigen::Transform3f t;
//t = translation * rotation;
transformPointCloud (cloud_in, cloud_out, origin, rotation);
transformPointCloud(cloud_in, cloud_out, origin, rotation);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
transformPointCloudWithNormals (const std::string &target_frame,
const pcl::PointCloud <PointT> &cloud_in,
pcl::PointCloud <PointT> &cloud_out,
const tf::TransformListener &tf_listener)
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
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;
return (true);
return true;
}
tf::StampedTransform transform;
try
{
tf_listener.lookupTransform (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);
try {
tf_listener.lookupTransform(
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;
}
transformPointCloudWithNormals (cloud_in, cloud_out, transform);
transformPointCloudWithNormals(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame;
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
transformPointCloud (const std::string &target_frame,
const pcl::PointCloud <PointT> &cloud_in,
pcl::PointCloud <PointT> &cloud_out,
const tf::TransformListener &tf_listener)
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame,
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;
return (true);
return true;
}
tf::StampedTransform transform;
try
{
tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
try {
tf_listener.lookupTransform(
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)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
catch (tf::ExtrapolationException &e)
{
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;
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
transformPointCloud (const std::string &target_frame,
const ros::Time & target_time,
const pcl::PointCloud <PointT> &cloud_in,
const std::string &fixed_frame,
pcl::PointCloud <PointT> &cloud_out,
const tf::TransformListener &tf_listener)
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame,
const ros::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener)
{
tf::StampedTransform transform;
try
{
tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
}
catch (tf::LookupException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
catch (tf::ExtrapolationException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
try {
tf_listener.lookupTransform(
target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, fixed_frame, transform);
} catch (tf::LookupException & e) {
ROS_ERROR("%s", e.what());
return false;
} catch (tf::ExtrapolationException & e) {
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;
std_msgs::Header header;
header.stamp = target_time;
cloud_out.header = toPCL(header);
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
transformPointCloudWithNormals (const std::string &target_frame,
const ros::Time & target_time,
const pcl::PointCloud <PointT> &cloud_in,
const std::string &fixed_frame,
pcl::PointCloud <PointT> &cloud_out,
const tf::TransformListener &tf_listener)
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
const ros::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener)
{
tf::StampedTransform transform;
try
{
tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
}
catch (tf::LookupException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
catch (tf::ExtrapolationException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
try {
tf_listener.lookupTransform(
target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, fixed_frame, transform);
} catch (tf::LookupException & e) {
ROS_ERROR("%s", e.what());
return false;
} catch (tf::ExtrapolationException & e) {
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;
std_msgs::Header header;
header.stamp = target_time;
cloud_out.header = toPCL(header);
return (true);
return true;
}
} // namespace pcl_ros

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

@ -45,85 +45,86 @@
namespace pcl_ros
{
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief BAG PointCloud file format reader.
* \author Radu Bogdan Rusu
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief BAG PointCloud file format reader.
* \author Radu Bogdan Rusu
*/
class BAGReader : public nodelet::Nodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
/** \brief Empty constructor. */
BAGReader()
: publish_rate_(0), output_() /*, cloud_received_ (false)*/ {}
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
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:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
if (it_ != view_.end()) {
output_ = it_->instantiate<sensor_msgs::PointCloud2>();
++it_;
}
return output_;
}
/** \brief Empty constructor. */
BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {};
/** \brief Open a BAG file for reading and select a specified topic
* \param file_name the BAG file to open
* \param topic_name the topic that we want to read data from
*/
bool open(const std::string & file_name, const std::string & topic_name);
/** \brief 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 Close an open BAG file. */
inline void
close()
{
bag_.close();
}
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate () { return (publish_rate_); }
/** \brief Nodelet initialization routine. */
virtual void onInit();
/** \brief Get the next point cloud dataset in the BAG file.
* \return the next point cloud dataset as read from the file
*/
inline PointCloudConstPtr
getNextCloud ()
{
if (it_ != view_.end ())
{
output_ = it_->instantiate<sensor_msgs::PointCloud2> ();
++it_;
}
return (output_);
}
private:
/** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
double publish_rate_;
/** \brief Open a BAG file for reading and select a specified topic
* \param file_name the BAG file to open
* \param topic_name the topic that we want to read data from
*/
bool open (const std::string &file_name, const std::string &topic_name);
/** \brief The BAG object. */
rosbag::Bag bag_;
/** \brief Close an open BAG file. */
inline void
close ()
{
bag_.close ();
}
/** \brief The BAG view object. */
rosbag::View view_;
/** \brief Nodelet initialization routine. */
virtual void onInit ();
/** \brief The BAG view iterator object. */
rosbag::View::iterator it_;
private:
/** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
double publish_rate_;
/** \brief The name of the topic that contains the PointCloud data. */
std::string topic_name_;
/** \brief The BAG object. */
rosbag::Bag bag_;
/** \brief The name of the BAG file that contains the PointCloud data. */
std::string file_name_;
/** \brief The BAG view object. */
rosbag::View view_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloudPtr output_;
/** \brief The BAG view iterator object. */
rosbag::View::iterator it_;
/** \brief Signals that a new PointCloud2 message has been read from the file. */
//bool cloud_received_;
/** \brief The name of the topic that contains the PointCloud data. */
std::string topic_name_;
/** \brief The name of the BAG file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloudPtr output_;
/** \brief Signals that a new PointCloud2 message has been read from the file. */
//bool cloud_received_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
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 sync_policies = message_filters::sync_policies;
namespace sync_policies = message_filters::sync_policies;
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
* checks their timestamps, and concatenates their fields together into a single
* PointCloud output message.
* \author Radu Bogdan Rusu
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
* checks their timestamps, and concatenates their fields together into a single
* PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateDataSynchronizer : public nodelet_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:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
PointCloud2 cloud;
cloud.header.stamp = input->header.stamp;
nf_.add(boost::make_shared<PointCloud2>(cloud));
}
/** \brief Empty constructor. */
PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {};
/** \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);
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {};
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateDataSynchronizer () {};
void onInit ();
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);
};
void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out);
};
}
#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
{
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
* input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
* a single PointCloud output message.
* \author Radu Bogdan Rusu
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
* input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
* a single PointCloud output message.
* \author Radu Bogdan Rusu
*/
class PointCloudConcatenateFieldsSynchronizer : public nodelet_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
{
public:
typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
PointCloudConcatenateFieldsSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), maximum_seconds_(0) {}
/** \brief Empty constructor. */
PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {};
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateFieldsSynchronizer() {}
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {};
void onInit();
void subscribe();
void unsubscribe();
void input_callback(const PointCloudConstPtr & cloud);
/** \brief Empty destructor. */
virtual ~PointCloudConcatenateFieldsSynchronizer () {};
private:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
void onInit ();
void subscribe ();
void unsubscribe ();
void input_callback (const PointCloudConstPtr &cloud);
/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
private:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief The number of input messages that we expect on the input topic. */
int input_messages_;
/** \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 The number of input messages that we expect on the input topic. */
int input_messages_;
/** \brief The maximum number of seconds to wait until we drop the synchronization. */
double maximum_seconds_;
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief The maximum number of seconds to wait until we drop the synchronization. */
double maximum_seconds_;
/** \brief A queue for messages. */
std::map<ros::Time, std::vector<PointCloudConstPtr> > queue_;
};
/** \brief A queue for messages. */
std::map<ros::Time, std::vector<PointCloudConstPtr>> queue_;
};
}
#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
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format reader.
* \author Radu Bogdan Rusu
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format reader.
* \author Radu Bogdan Rusu
*/
class PCDReader : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Empty constructor. */
PCDReader()
: publish_rate_(0), tf_frame_("/base_link") {}
virtual void onInit();
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
class PCDReader : public PCLNodelet
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
/** \brief Empty constructor. */
PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {};
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate() {return publish_rate_;}
virtual void onInit ();
/** \brief Set the publishing rate in seconds.
* \param publish_rate the publishing rate in seconds
*/
inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; }
/** \brief Get the publishing rate in seconds. */
inline double getPublishRate () { return (publish_rate_); }
/** \brief Set the TF frame the PointCloud will be published in.
* \param tf_frame the TF frame the PointCloud will be published in
*/
inline void setTFframe (std::string tf_frame) { tf_frame_ = tf_frame; }
/** \brief Get the TF frame the PointCloud is published in. */
inline std::string getTFframe () { return (tf_frame_); }
protected:
/** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
double publish_rate_;
/** \brief The TF frame the data should be published in ("/base_link" by default). */
std::string tf_frame_;
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloud2Ptr output_;
private:
/** \brief The PCL implementation used. */
pcl::PCDReader impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format writer.
* \author Radu Bogdan Rusu
/** \brief Set the TF frame the PointCloud will be published in.
* \param tf_frame the TF frame the PointCloud will be published in
*/
class PCDWriter : public PCLNodelet
{
public:
PCDWriter () : file_name_ (""), binary_mode_ (true) {}
inline void setTFframe(std::string tf_frame) {tf_frame_ = tf_frame;}
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
/** \brief Get the TF frame the PointCloud is published in. */
inline std::string getTFframe() {return tf_frame_;}
virtual void onInit ();
void input_callback (const PointCloud2ConstPtr &cloud);
protected:
/** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
double publish_rate_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief The TF frame the data should be published in ("/base_link" by default). */
std::string tf_frame_;
protected:
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \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_;
/** \brief The output point cloud dataset containing the points loaded from the file. */
PointCloud2Ptr output_;
private:
/** \brief The PCL implementation used. */
pcl::PCDWriter impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
private:
/** \brief The PCL implementation used. */
pcl::PCDReader impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Point Cloud Data (PCD) file format writer.
* \author Radu Bogdan Rusu
*/
class PCDWriter : public PCLNodelet
{
public:
PCDWriter()
: file_name_(""), binary_mode_(true) {}
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef PointCloud2::Ptr PointCloud2Ptr;
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
virtual void onInit();
void input_callback(const PointCloud2ConstPtr & cloud);
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
protected:
/** \brief The name of the file that contains the PointCloud data. */
std::string file_name_;
/** \brief Set to true if the output files should be saved in binary mode (true). */
bool binary_mode_;
private:
/** \brief The PCL implementation used. */
pcl::PCDWriter impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_IO_PCD_IO_H_

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

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

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

@ -11,286 +11,295 @@
#include <boost/mpl/size.hpp>
#include <boost/ref.hpp>
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>
struct FieldStreamer
{
FieldStreamer(Stream& stream) : stream_(stream) {}
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);
}
template<typename U> void operator() ()
{
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;
stream_.next(offset);
std::uint32_t offset = traits::offset<PointT, U>::value;
stream_.next(offset);
std::uint8_t datatype = traits::datatype<PointT, U>::value;
stream_.next(datatype);
std::uint8_t datatype = traits::datatype<PointT, U>::value;
stream_.next(datatype);
std::uint32_t count = traits::datatype<PointT, U>::size;
stream_.next(count);
}
std::uint32_t count = traits::datatype<PointT, U>::size;
stream_.next(count);
}
Stream & stream_;
};
Stream& stream_;
};
template<typename PointT>
struct FieldsLength
{
FieldsLength()
: length(0) {}
template<typename PointT>
struct FieldsLength
{
FieldsLength() : length(0) {}
template<typename U>
void operator()()
{
std::uint32_t name_length = strlen(traits::name<PointT, U>::value);
length += name_length + 13;
}
template<typename U> void operator() ()
{
std::uint32_t name_length = strlen(traits::name<PointT, U>::value);
length += name_length + 13;
}
std::uint32_t length;
};
} // namespace pcl::detail
std::uint32_t length;
};
} // namespace pcl::detail
} // namespace pcl
namespace ros
namespace ros
{
// 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
// data and object fields only once and reuse it.
// 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
// data and object fields only once and reuse it.
#if ROS_VERSION_MINIMUM(1, 3, 1)
template<typename T>
struct DefaultMessageCreator<pcl::PointCloud<T> >
{
boost::shared_ptr<pcl::MsgFieldMap> mapping_;
template<typename T>
struct DefaultMessageCreator<pcl::PointCloud<T>>
{
boost::shared_ptr<pcl::MsgFieldMap> mapping_;
DefaultMessageCreator()
: mapping_( boost::make_shared<pcl::MsgFieldMap>() )
{
}
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;
}
};
DefaultMessageCreator()
: mapping_(boost::make_shared<pcl::MsgFieldMap>() )
{
}
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
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> >
{
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) {
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
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)
{
template<typename T>
struct Serializer<pcl::PointCloud<T> >
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>
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>
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;
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);
}
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))
{
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);
} 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);
}
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)
{
uint32_t length = 0;
uint8_t is_dense;
stream.next(is_dense);
m.is_dense = is_dense;
}
length += serializationLength(m.header);
length += 8; // height/width
inline static uint32_t serializedLength(const pcl::PointCloud<T> & m)
{
uint32_t length = 0;
pcl::detail::FieldsLength<T> fl;
typedef typename pcl::traits::fieldList<T>::type FieldList;
pcl::for_each_type<FieldList>(boost::ref(fl));
length += 4; // size of 'fields'
length += fl.length;
length += serializationLength(m.header);
length += 8; // height/width
length += 1; // is_bigendian
length += 4; // point_step
length += 4; // row_step
length += 4; // size of 'data'
length += m.points.size() * sizeof(T); // data
length += 1; // is_dense
pcl::detail::FieldsLength<T> fl;
typedef typename pcl::traits::fieldList<T>::type FieldList;
pcl::for_each_type<FieldList>(boost::ref(fl));
length += 4; // size of 'fields'
length += fl.length;
return length;
}
};
} // namespace ros::serialization
length += 1; // is_bigendian
length += 4; // point_step
length += 4; // row_step
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
@ -315,103 +324,101 @@ namespace ros
namespace pcl
{
namespace detail
{
namespace detail
{
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
template <class T>
constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>,
pcl::shared_ptr<T>>::value;
template<class T>
constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>,
pcl::shared_ptr<T>>::value;
#else
template <class T>
constexpr static bool pcl_uses_boost = true;
template<class T>
constexpr static bool pcl_uses_boost = true;
#endif
template<class SharedPointer> struct Holder
{
SharedPointer p;
template<class SharedPointer>
struct Holder
{
SharedPointer p;
Holder(const SharedPointer &p) : p(p) {}
Holder(const Holder &other) : p(other.p) {}
Holder(Holder &&other) : p(std::move(other.p)) {}
Holder(const SharedPointer & p)
: p(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>
inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> &p)
{
typedef Holder<std::shared_ptr<T>> H;
if(H *h = boost::get_deleter<H>(p))
{
return h->p;
}
else
{
return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
}
}
template<class T>
inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> & p)
{
typedef Holder<std::shared_ptr<T>> H;
if (H * h = boost::get_deleter<H>(p)) {
return h->p;
} else {
return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
}
}
template<class T>
inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> &p)
{
typedef Holder<boost::shared_ptr<T>> H;
if(H * h = std::get_deleter<H>(p))
{
return h->p;
}
else
{
return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
}
}
template<class T>
inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
{
typedef Holder<boost::shared_ptr<T>> H;
if (H * h = std::get_deleter<H>(p)) {
return h->p;
} else {
return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
}
}
#endif
} // namespace pcl::detail
} // namespace pcl::detail
// add functions to convert to smart pointer used by ROS
template <class T>
inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> &p)
{
return p;
}
template<class T>
inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> & p)
{
return p;
}
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
template <class T>
inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> &p)
{
return detail::to_boost_ptr(p);
}
template<class T>
inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> & p)
{
return detail::to_boost_ptr(p);
}
// 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>
inline std::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> &p)
{
return p;
}
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)
{
return p;
}
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)
{
return detail::to_std_ptr(p);
}
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)
{
return detail::to_std_ptr(p);
}
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)
{
return detail::to_boost_ptr(p);
}
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)
{
return detail::to_boost_ptr(p);
}
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)
{
return p;
}
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)
{
return p;
}
#else
template <class T>
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> &p)
{
return p;
}
template<class T>
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
{
return p;
}
#endif
} // namespace pcl

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

@ -38,7 +38,7 @@
\author Patrick Mihelich
@b Publisher represents a ROS publisher for the templated PointCloud implementation.
@b Publisher represents a ROS publisher for the templated PointCloud implementation.
**/
@ -51,99 +51,99 @@
#include <pcl_conversions/pcl_conversions.h>
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:
void
advertise (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
{
pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
}
pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
}
std::string
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
std::string
getTopic()
{
public:
Publisher () {}
return pub_.getTopic();
}
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
uint32_t
getNumSubscribers() const
{
public:
Publisher () {}
return pub_.getNumSubscribers();
}
Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
{
advertise (nh, topic, queue_size);
}
void
shutdown()
{
pub_.shutdown();
}
~Publisher () {}
operator void *() const
{
return (pub_) ? (void *)1 : (void *)0;
}
void
publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const
{
pub_.publish (point_cloud);
//pub_.publish (*point_cloud);
}
protected:
ros::Publisher pub_;
};
void
publish (const sensor_msgs::PointCloud2& point_cloud) const
{
pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (point_cloud));
//pub_.publish (point_cloud);
}
};
template<typename PointT>
class Publisher : public BasePublisher
{
public:
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_

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

@ -47,63 +47,68 @@
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.
* \author Radu Bogdan Rusu
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
* \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
{
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_;
void config_callback(EuclideanClusterExtractionConfig & config, uint32_t level);
/** \brief Maximum number of clusters to publish. */
int max_clusters_;
/** \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);
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > srv_;
private:
/** \brief The PCL implementation used. */
pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
/** \brief Nodelet initialization routine. */
void onInit ();
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief LazyNodelet connection routine. */
void subscribe ();
void unsubscribe ();
/** \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_;
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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 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
* a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
* 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).
* \author Radu Bogdan Rusu
/** \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
* 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).
* \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;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
PointIndices cloud;
cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
nf_.add(boost::make_shared<PointIndices>(cloud));
}
protected:
/** \brief The output PointIndices publisher. */
ros::Publisher pub_output_;
/** \brief Nodelet initialization routine. */
void onInit();
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_hull_filter_;
/** \brief LazyNodelet connection routine. */
void subscribe();
void unsubscribe();
/** \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 Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(ExtractPolygonalPrismDataConfig & config, uint32_t level);
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > srv_;
/** \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);
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointIndices> nf_;
private:
/** \brief The PCL implementation used. */
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
/** \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 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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_

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

@ -51,235 +51,243 @@
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
* the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
* \author Radu Bogdan Rusu
////////////////////////////////////////////////////////////////////////////////////////////
/** \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.
* \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
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
public:
/** \brief Constructor. */
SACSegmentation () : min_inliers_(0) {}
/** \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 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 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.
/** \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
*/
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;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
indices_ = *indices;
}
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
/** \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);
}
public:
/** \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 Get the TF frame the input PointCloud should be transformed into before processing. */
inline std::string getInputTFframe () { return (tf_input_frame_); }
private:
/** \brief Internal mutex. */
boost::mutex mutex_;
/** \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_); }
/** \brief The PCL implementation used. */
pcl::SACSegmentation<pcl::PointXYZ> impl_;
protected:
// ROS nodelet attributes
/** \brief The normals PointCloud subscriber filter. */
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
/** \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_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_axis_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > srv_;
/** \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 PointCloudConstPtr &cloud)
{
PointIndices indices;
indices.header.stamp = fromPCL(cloud->header).stamp;
nf_.add (boost::make_shared<PointIndices> (indices));
}
////////////////////////////////////////////////////////////////////////////////////////////
/** \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
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointIndices> nf_;
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
/** \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_;
public:
/** \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 Nodelet initialization routine. */
virtual void onInit ();
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
inline std::string getInputTFframe() {return tf_input_frame_;}
/** \brief LazyNodelet connection routine. */
virtual void subscribe ();
virtual void unsubscribe ();
/** \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 Model callback
* \param model the sample consensus model found
*/
void axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model);
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
inline std::string getOutputTFframe() {return tf_output_frame_;}
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level);
protected:
// ROS nodelet attributes
/** \brief The normals PointCloud subscriber filter. */
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
/** \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 input PointCloud subscriber. */
ros::Subscriber sub_axis_;
/** \brief The PCL implementation used. */
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>> srv_;
/** \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_;
/** \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 PointCloudConstPtr & cloud)
{
PointIndices indices;
indices.header.stamp = fromPCL(cloud->header).stamp;
nf_.add(boost::make_shared<PointIndices>(indices));
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */
message_filters::PassThrough<PointIndices> nf_;
/** \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 Nodelet initialization routine. */
virtual void onInit();
/** \brief LazyNodelet connection routine. */
virtual void subscribe();
virtual void unsubscribe();
/** \brief Model callback
* \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_

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

@ -48,63 +48,66 @@
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
* difference between them for a maximum given distance threshold.
* \author Radu Bogdan Rusu
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
* difference between them for a maximum given distance threshold.
* \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
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
void config_callback(SegmentDifferencesConfig & config, uint32_t level);
public:
/** \brief Empty constructor. */
SegmentDifferences () {};
protected:
/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_target_filter_;
/** \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);
/** \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_;
private:
/** \brief The PCL implementation used. */
pcl::SegmentDifferences<pcl::PointXYZ> impl_;
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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 sync_policies = message_filters::sync_policies;
namespace sync_policies = message_filters::sync_policies;
/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
* \author Radu Bogdan Rusu
/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
* \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
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
void input_indices_callback(
const PointCloudConstPtr & cloud,
const PointIndicesConstPtr & indices);
private:
/** \brief Nodelet initialization routine. */
virtual void onInit ();
private:
/** \brief The PCL implementation used. */
pcl::ConvexHull<pcl::PointXYZ> impl_;
/** \brief LazyNodelet connection routine. */
virtual void subscribe ();
virtual void unsubscribe ();
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \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);
/** \brief Publisher for PolygonStamped. */
ros::Publisher pub_plane_;
private:
/** \brief The PCL implementation used. */
pcl::ConvexHull<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_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_

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

@ -46,105 +46,107 @@
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/MLSConfig.h"
#include "pcl_ros/MLSConfig.h"
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.
* 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.
* \author Radu Bogdan Rusu, Zoltan-Csaba Marton
/** \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.
* Normals are estimated at each point as well and published on a separate topic.
* \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
{
typedef pcl::PointXYZ PointIn;
typedef pcl::PointNormal NormalOut;
int spatial_locator_type_;
typedef pcl::PointCloud<PointIn> PointCloudIn;
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
typedef pcl::PointCloud<NormalOut> NormalCloudOut;
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<MLSConfig>> srv_;
typedef pcl::KdTree<PointIn> KdTree;
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(MLSConfig & config, uint32_t level);
protected:
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
PointCloudInConstPtr surface_;
/** \brief Nodelet initialization routine. */
virtual void onInit();
/** \brief A pointer to the spatial search object. */
KdTreePtr tree_;
/** \brief LazyNodelet connection routine. */
virtual void subscribe();
virtual void unsubscribe();
/** \brief The nearest neighbors search radius for each point. */
double search_radius_;
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);
/** \brief The number of K nearest neighbors to use for each point. */
//int k_;
private:
/** \brief The PCL implementation used. */
pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
/** \brief Whether to use a polynomial fit. */
bool use_polynomial_fit_;
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief The order of the polynomial to be fit. */
int polynomial_order_;
/** \brief The output PointCloud (containing the normals) publisher. */
ros::Publisher pub_normals_;
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */
double gaussian_parameter_;
/** \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_;
// 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
*/
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
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#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
{
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template <typename PointT> void
transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
pcl::PointCloud <PointT> &cloud_out,
const tf::Transform &transform);
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template<typename PointT>
void
transformPointCloudWithNormals(
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
* \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template <typename PointT> bool
transformPointCloudWithNormals (const std::string &target_frame,
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
* \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
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
* \param target_frame the target TF frame the point cloud should be transformed to
* \param target_time the target timestamp
* \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template <typename PointT> bool
transformPointCloudWithNormals (const std::string &target_frame,
const ros::Time & target_time,
const pcl::PointCloud <PointT> &cloud_in,
const std::string &fixed_frame,
pcl::PointCloud <PointT> &cloud_out,
const tf::TransformListener &tf_listener);
/** \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_time the target timestamp
* \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template<typename PointT>
bool
transformPointCloudWithNormals(
const std::string & target_frame,
const ros::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
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
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template <typename PointT> void
transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
pcl::PointCloud <PointT> &cloud_out,
const tf::Transform &transform);
/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param transform a rigid transformation from tf
* \note calls the Eigen version
*/
template<typename PointT>
void
transformPointCloud(
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
* \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template <typename PointT> bool
transformPointCloud (const std::string &target_frame,
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
* \param target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame,
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
* \param target_frame the target TF frame the point cloud should be transformed to
* \param target_time the target timestamp
* \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template <typename PointT> bool
transformPointCloud (const std::string &target_frame, const ros::Time & target_time,
const pcl::PointCloud <PointT> &cloud_in,
const std::string &fixed_frame,
pcl::PointCloud <PointT> &cloud_out,
const tf::TransformListener &tf_listener);
/** \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_time the target timestamp
* \param cloud_in the input point cloud
* \param fixed_frame fixed TF frame
* \param cloud_out the input point cloud
* \param tf_listener a TF listener object
*/
template<typename PointT>
bool
transformPointCloud(
const std::string & target_frame, const ros::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in,
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.
* \param target_frame the target TF frame
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
* \param tf_listener a TF listener object
*/
bool
transformPointCloud (const std::string &target_frame,
const sensor_msgs::PointCloud2 &in,
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.
* \param target_frame the target TF frame
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
* \param tf_listener a TF listener object
*/
bool
transformPointCloud(
const std::string & target_frame,
const sensor_msgs::PointCloud2 & in,
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.
* \param target_frame the target TF frame
* \param net_transform the TF transformer object
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
*/
void
transformPointCloud (const std::string &target_frame,
const tf::Transform &net_transform,
const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out);
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame
* \param net_transform the TF transformer object
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
*/
void
transformPointCloud(
const std::string & target_frame,
const tf::Transform & net_transform,
const sensor_msgs::PointCloud2 & in,
sensor_msgs::PointCloud2 & out);
/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
* \param transform the transformation to use on the points
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
*/
void
transformPointCloud (const Eigen::Matrix4f &transform,
const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out);
/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
* \param transform the transformation to use on the points
* \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset
*/
void
transformPointCloud(
const Eigen::Matrix4f & transform,
const sensor_msgs::PointCloud2 & in,
sensor_msgs::PointCloud2 & out);
/** \brief Obtain the transformation matrix from TF into an Eigen form
* \param bt the TF transformation
* \param out_mat the Eigen transformation
*/
void
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat);
/** \brief Obtain the transformation matrix from TF into an Eigen form
* \param bt the TF transformation
* \param out_mat the Eigen transformation
*/
void
transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat);
}
#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"
void
pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
}
void
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices)
pcl_ros::BoundaryEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch (k_);
impl_.setRadiusSearch (search_radius_);
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Set the inputs
impl_.setInputCloud (pcl_ptr(cloud));
impl_.setIndices (indices);
impl_.setSearchSurface (pcl_ptr(surface));
impl_.setInputNormals (pcl_ptr(normals));
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices);
impl_.setSearchSurface(pcl_ptr(surface));
impl_.setInputNormals(pcl_ptr(normals));
// Estimate the feature
PointCloudOut output;
impl_.compute (output);
impl_.compute(output);
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::BoundaryEstimation BoundaryEstimation;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -1,5 +1,5 @@
/*
*
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
@ -32,7 +32,7 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: cropbox.cpp
* $Id: cropbox.cpp
*
*/
@ -41,24 +41,25 @@
//////////////////////////////////////////////////////////////////////////////////////////////
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
has_service = true;
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);
srv_->setCallback (f);
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);
srv_->setCallback(f);
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
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();
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;
// Check the current values for minimum point
if (min_point != new_min_point)
{
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));
if (min_point != new_min_point) {
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));
// Set the filter min point if different
impl_.setMin(new_min_point);
}
// Check the current values for the maximum point
if (max_point != new_max_point)
{
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));
// Check the current values for the maximum point
if (max_point != new_max_point) {
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));
// Set the filter max point if different
impl_.setMax(new_max_point);
}
// Check the current value for keep_organized
if (impl_.getKeepOrganized () != config.keep_organized)
{
NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false");
if (impl_.getKeepOrganized() != config.keep_organized) {
NODELET_DEBUG(
"[%s::config_callback] Setting the filter keep_organized value to: %s.",
getName().c_str(), config.keep_organized ? "true" : "false");
// Call the virtual method in the child
impl_.setKeepOrganized (config.keep_organized);
impl_.setKeepOrganized(config.keep_organized);
}
// Check the current value for the negative flag
if (impl_.getNegative() != config.negative)
{
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.negative ? "true" : "false");
if (impl_.getNegative() != config.negative) {
NODELET_DEBUG(
"[%s::config_callback] Setting the filter negative flag to: %s.",
getName().c_str(), config.negative ? "true" : "false");
// Call the virtual method in the child
impl_.setNegative(config.negative);
}
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame)
{
if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
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;
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;
PLUGINLIB_EXPORT_CLASS(CropBox,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet);

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

@ -39,32 +39,33 @@
#include "pcl_ros/filters/extract_indices.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl_ros::ExtractIndices::child_init (ros::NodeHandle &nh, bool &has_service)
bool
pcl_ros::ExtractIndices::child_init(ros::NodeHandle & nh, bool & has_service)
{
has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > (nh);
dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind (&ExtractIndices::config_callback, this, _1, _2);
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind(
&ExtractIndices::config_callback, this, _1, _2);
srv_->setCallback(f);
use_indices_ = true;
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
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)
{
impl_.setNegative (config.negative);
NODELET_DEBUG ("[%s::config_callback] Setting the extraction to: %s.", getName ().c_str (), (config.negative ? "indices" : "everything but the indices"));
if (impl_.getNegative() != config.negative) {
impl_.setNegative(config.negative);
NODELET_DEBUG(
"[%s::config_callback] Setting the extraction to: %s.", getName().c_str(),
(config.negative ? "indices" : "everything but the indices"));
}
}
typedef pcl_ros::ExtractIndices ExtractIndices;
PLUGINLIB_EXPORT_CLASS(ExtractIndices,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -38,42 +38,42 @@
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/features/vfh.hpp"
void
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
void
pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
pub_output_.publish(output.makeShared());
}
void
pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices)
void
pcl_ros::VFHEstimation::computePublish(
const PointCloudInConstPtr & cloud,
const PointCloudNConstPtr & normals,
const PointCloudInConstPtr & surface,
const IndicesPtr & indices)
{
// Set the parameters
impl_.setKSearch (k_);
impl_.setRadiusSearch (search_radius_);
impl_.setKSearch(k_);
impl_.setRadiusSearch(search_radius_);
// Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_);
initTree(spatial_locator_type_, tree_, k_);
impl_.setSearchMethod(tree_);
// Set the inputs
impl_.setInputCloud (cloud);
impl_.setIndices (indices);
impl_.setSearchSurface (surface);
impl_.setInputNormals (normals);
impl_.setInputCloud(cloud);
impl_.setIndices(indices);
impl_.setSearchSurface(surface);
impl_.setInputNormals(normals);
// Estimate the feature
PointCloudOut output;
impl_.compute (output);
impl_.compute(output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
pub_output_.publish(output.makeShared());
}
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
pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices)
{
PointCloud2 output;
// 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
if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_)
{
NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) {
NODELET_DEBUG(
"[%s::computePublish] Transforming output dataset from %s to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str());
// Convert the cloud into the different frame
PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_))
{
NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_listener_)) {
NODELET_ERROR(
"[%s::computePublish] Error converting output dataset from %s to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str());
return;
}
cloud_tf.reset (new PointCloud2 (cloud_transformed));
cloud_tf.reset(new PointCloud2(cloud_transformed));
}
if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_)
// no tf_output_frame given, transform the dataset to its original frame
{
NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) {
// no tf_output_frame given, transform the dataset to its original frame
NODELET_DEBUG(
"[%s::computePublish] Transforming output dataset from %s back to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
// Convert the cloud into the different frame
PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_))
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;
}
cloud_tf.reset (new PointCloud2 (cloud_transformed));
cloud_tf.reset(new PointCloud2(cloud_transformed));
}
// Copy timestamp to keep it
cloud_tf->header.stamp = input->header.stamp;
// Publish a boost shared ptr
pub_output_.publish (cloud_tf);
pub_output_.publish(cloud_tf);
}
//////////////////////////////////////////////////////////////////////////////////////////////
@ -107,141 +114,152 @@ void
pcl_ros::Filter::subscribe()
{
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
if (use_indices_) {
// Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
pcl_msgs::PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
pcl_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
{
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 {
// 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
pcl_ros::Filter::unsubscribe()
{
if (use_indices_)
{
if (use_indices_) {
sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe();
}
else
} else {
sub_input_.shutdown();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Filter::onInit ()
pcl_ros::Filter::onInit()
{
// Call the super onInit ()
PCLNodelet::onInit ();
PCLNodelet::onInit();
// Call the child's local init
bool has_service = false;
if (!child_init (*pnh_, has_service))
{
NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
if (!child_init(*pnh_, has_service)) {
NODELET_ERROR("[%s::onInit] Initialization failed.", getName().c_str());
return;
}
pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
// Enable the dynamic reconfigure service
if (!has_service)
{
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2);
srv_->setCallback (f);
if (!has_service) {
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(*pnh_);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
&Filter::config_callback, this, _1, _2);
srv_->setCallback(f);
}
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str());
}
//////////////////////////////////////////////////////////////////////////////////////////////
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
if (tf_input_frame_ != config.input_frame)
{
if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
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;
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
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 (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
if (!isValid(cloud)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
if (indices && !isValid(indices)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height,
cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str());
}
///
// Check whether the user has given a different input TF frame
tf_input_orig_frame_ = cloud->header.frame_id;
PointCloud2::ConstPtr cloud_tf;
if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
{
NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) {
NODELET_DEBUG(
"[%s::input_indices_callback] Transforming input dataset from %s to %s.",
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
// Save the original frame ID
// Convert the cloud into the different frame
PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
{
NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) {
NODELET_ERROR(
"[%s::input_indices_callback] Error converting input dataset from %s to %s.",
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
return;
}
cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
}
else
cloud_tf = boost::make_shared<PointCloud2>(cloud_transformed);
} else {
cloud_tf = cloud;
}
// Need setInputCloud () here because we have to extract x/y/z
IndicesPtr vindices;
if (indices)
vindices.reset (new std::vector<int> (indices->indices));
if (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
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
has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (nh);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2);
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
&PassThrough::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
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;
impl_.getFilterLimits (filter_min, filter_max);
impl_.getFilterLimits(filter_min, filter_max);
// Check the current values for filter min-max
if (filter_min != config.filter_limit_min)
{
if (filter_min != config.filter_limit_min) {
filter_min = config.filter_limit_min;
NODELET_DEBUG ("[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_min);
NODELET_DEBUG(
"[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
getName().c_str(), filter_min);
// Set the filter min-max if different
impl_.setFilterLimits (filter_min, filter_max);
impl_.setFilterLimits(filter_min, filter_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;
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
impl_.setFilterLimits (filter_min, filter_max);
impl_.setFilterLimits(filter_min, filter_max);
}
// Check the current value for the filter field
//std::string filter_field = impl_.getFilterFieldName ();
if (impl_.getFilterFieldName () != config.filter_field_name)
{
if (impl_.getFilterFieldName() != config.filter_field_name) {
// Set the filter field if different
impl_.setFilterFieldName (config.filter_field_name);
NODELET_DEBUG ("[%s::config_callback] Setting the filter field name to: %s.", getName ().c_str (), config.filter_field_name.c_str ());
impl_.setFilterFieldName(config.filter_field_name);
NODELET_DEBUG(
"[%s::config_callback] Setting the filter field name to: %s.",
getName().c_str(), config.filter_field_name.c_str());
}
// Check the current value for keep_organized
if (impl_.getKeepOrganized () != config.keep_organized)
{
NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false");
if (impl_.getKeepOrganized() != config.keep_organized) {
NODELET_DEBUG(
"[%s::config_callback] Setting the filter keep_organized value to: %s.",
getName().c_str(), config.keep_organized ? "true" : "false");
// Call the virtual method in the child
impl_.setKeepOrganized (config.keep_organized);
impl_.setKeepOrganized(config.keep_organized);
}
// Check the current value for the negative flag
if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
{
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
NODELET_DEBUG(
"[%s::config_callback] Setting the filter negative flag to: %s.",
getName().c_str(), config.filter_limit_negative ? "true" : "false");
// Call the virtual method in the child
impl_.setFilterLimitsNegative (config.filter_limit_negative);
impl_.setFilterLimitsNegative(config.filter_limit_negative);
}
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame)
{
if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
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;
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;
PLUGINLIB_EXPORT_CLASS(PassThrough,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet);

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

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

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

@ -40,38 +40,42 @@
//////////////////////////////////////////////////////////////////////////////////////////////
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
has_service = true;
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);
srv_->setCallback (f);
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);
srv_->setCallback(f);
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
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)
{
impl_.setMinNeighborsInRadius (config.min_neighbors);
NODELET_DEBUG ("[%s::config_callback] Setting the number of neighbors in radius: %d.", getName ().c_str (), config.min_neighbors);
if (impl_.getMinNeighborsInRadius() != config.min_neighbors) {
impl_.setMinNeighborsInRadius(config.min_neighbors);
NODELET_DEBUG(
"[%s::config_callback] Setting the number of neighbors in radius: %d.",
getName().c_str(), config.min_neighbors);
}
if (impl_.getRadiusSearch () != config.radius_search)
{
impl_.setRadiusSearch (config.radius_search);
NODELET_DEBUG ("[%s::config_callback] Setting the radius to search neighbors: %f.", getName ().c_str (), config.radius_search);
if (impl_.getRadiusSearch() != config.radius_search) {
impl_.setRadiusSearch(config.radius_search);
NODELET_DEBUG(
"[%s::config_callback] Setting the radius to search neighbors: %f.",
getName().c_str(), config.radius_search);
}
}
typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval;
PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval, nodelet::Nodelet);

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

@ -40,42 +40,47 @@
//////////////////////////////////////////////////////////////////////////////////////////////
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
has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > (nh);
dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2);
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>>(
nh);
dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f =
boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
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)
{
impl_.setMeanK (config.mean_k);
NODELET_DEBUG ("[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", getName ().c_str (), config.mean_k);
}
if (impl_.getStddevMulThresh () != config.stddev)
{
impl_.setStddevMulThresh (config.stddev);
NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev);
if (impl_.getMeanK() != config.mean_k) {
impl_.setMeanK(config.mean_k);
NODELET_DEBUG(
"[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.",
getName().c_str(), config.mean_k);
}
if (impl_.getNegative () != config.negative)
{
impl_.setNegative (config.negative);
NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true");
if (impl_.getStddevMulThresh() != config.stddev) {
impl_.setStddevMulThresh(config.stddev);
NODELET_DEBUG(
"[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.",
getName().c_str(), config.stddev);
}
if (impl_.getNegative() != config.negative) {
impl_.setNegative(config.negative);
NODELET_DEBUG(
"[%s::config_callback] Returning only inliers: %s.",
getName().c_str(), config.negative ? "false" : "true");
}
}
typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval;
PLUGINLIB_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
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
has_service = true;
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > (nh);
dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2);
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind(
&VoxelGrid::config_callback, this, _1, _2);
srv_->setCallback(f);
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input,
const IndicesPtr &indices,
PointCloud2 &output)
pcl_ros::VoxelGrid::filter(
const PointCloud2::ConstPtr & input,
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_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
//////////////////////////////////////////////////////////////////////////////////////////////
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)
{
leaf_size.setConstant (config.leaf_size);
NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]);
if (leaf_size[0] != config.leaf_size) {
leaf_size.setConstant(config.leaf_size);
NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]);
}
double filter_min, filter_max;
impl_.getFilterLimits (filter_min, filter_max);
if (filter_min != config.filter_limit_min)
{
impl_.getFilterLimits(filter_min, filter_max);
if (filter_min != config.filter_limit_min) {
filter_min = config.filter_limit_min;
NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min);
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;
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)
{
impl_.setFilterLimitsNegative (config.filter_limit_negative);
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
impl_.setFilterLimitsNegative(config.filter_limit_negative);
NODELET_DEBUG(
"[%s::config_callback] Setting the filter negative flag to: %s.",
getName().c_str(), config.filter_limit_negative ? "true" : "false");
}
if (impl_.getFilterFieldName () != config.filter_field_name)
{
impl_.setFilterFieldName (config.filter_field_name);
NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ());
if (impl_.getFilterFieldName() != config.filter_field_name) {
impl_.setFilterFieldName(config.filter_field_name);
NODELET_DEBUG(
"[config_callback] Setting the filter field name to: %s.",
config.filter_field_name.c_str());
}
// ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter
if (tf_input_frame_ != config.input_frame)
{
if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
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;
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;
PLUGINLIB_EXPORT_CLASS(VoxelGrid,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet);

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

@ -39,75 +39,74 @@
#include "pcl_ros/io/bag_io.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name)
bool
pcl_ros::BAGReader::open(const std::string & file_name, const std::string & topic_name)
{
try
{
bag_.open (file_name, rosbag::bagmode::Read);
view_.addQuery (bag_, rosbag::TopicQuery (topic_name));
try {
bag_.open(file_name, rosbag::bagmode::Read);
view_.addQuery(bag_, rosbag::TopicQuery(topic_name));
if (view_.size () == 0)
return (false);
if (view_.size() == 0) {
return false;
}
it_ = view_.begin ();
it_ = view_.begin();
} catch (rosbag::BagException & e) {
return false;
}
catch (rosbag::BagException &e)
{
return (false);
}
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::BAGReader::onInit ()
pcl_ros::BAGReader::onInit()
{
boost::shared_ptr<ros::NodeHandle> pnh_;
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
pnh_.reset(new ros::NodeHandle(getMTPrivateNodeHandle()));
// ---[ Mandatory parameters
if (!pnh_->getParam ("file_name", file_name_))
{
NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
if (!pnh_->getParam("file_name", file_name_)) {
NODELET_ERROR("[onInit] Need a 'file_name' parameter to be set before continuing!");
return;
}
if (!pnh_->getParam ("topic_name", topic_name_))
{
NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!");
if (!pnh_->getParam("topic_name", topic_name_)) {
NODELET_ERROR("[onInit] Need a 'topic_name' parameter to be set before continuing!");
return;
}
// ---[ Optional parameters
int max_queue_size = 1;
pnh_->getParam ("publish_rate", publish_rate_);
pnh_->getParam ("max_queue_size", max_queue_size);
pnh_->getParam("publish_rate", publish_rate_);
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"
" - file_name : %s\n"
" - topic_name : %s",
file_name_.c_str (), topic_name_.c_str ());
NODELET_DEBUG(
"[onInit] Nodelet successfully created with the following parameters:\n"
" - file_name : %s\n"
" - topic_name : %s",
file_name_.c_str(), topic_name_.c_str());
if (!open (file_name_, topic_name_))
if (!open(file_name_, topic_name_)) {
return;
}
PointCloud output;
output_ = boost::make_shared<PointCloud> (output);
output_->header.stamp = ros::Time::now ();
output_ = boost::make_shared<PointCloud>(output);
output_->header.stamp = ros::Time::now();
// Continous publishing enabled?
while (pnh_->ok ())
{
PointCloudConstPtr cloud = getNextCloud ();
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 ());
output_->header.stamp = ros::Time::now ();
while (pnh_->ok()) {
PointCloudConstPtr cloud = getNextCloud();
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());
output_->header.stamp = ros::Time::now();
pub_output.publish (output_);
pub_output.publish(output_);
ros::Duration (publish_rate_).sleep ();
ros::spinOnce ();
ros::Duration(publish_rate_).sleep();
ros::spinOnce();
}
}
typedef pcl_ros::BAGReader BAGReader;
PLUGINLIB_EXPORT_CLASS(BAGReader,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet);

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

@ -44,226 +44,260 @@
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit()
{
nodelet_topic_tools::NodeletLazy::onInit ();
nodelet_topic_tools::NodeletLazy::onInit();
// ---[ Mandatory parameters
pnh_->getParam ("output_frame", output_frame_);
pnh_->getParam ("approximate_sync", approximate_sync_);
pnh_->getParam("output_frame", output_frame_);
pnh_->getParam("approximate_sync", approximate_sync_);
if (output_frame_.empty ())
{
NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
if (output_frame_.empty()) {
NODELET_ERROR("[onInit] Need an 'output_frame' parameter to be set before continuing!");
return;
}
if (!pnh_->getParam ("input_topics", input_topics_))
{
NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
if (!pnh_->getParam("input_topics", input_topics_)) {
NODELET_ERROR("[onInit] Need a 'input_topics' parameter to be set before continuing!");
return;
}
if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray)
{
NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
if (input_topics_.getType() != XmlRpc::XmlRpcValue::TypeArray) {
NODELET_ERROR("[onInit] Invalid 'input_topics' parameter given!");
return;
}
if (input_topics_.size () == 1)
{
NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
if (input_topics_.size() == 1) {
NODELET_ERROR("[onInit] Only one topic given. Need at least two topics to continue.");
return;
}
if (input_topics_.size () > 8)
{
NODELET_ERROR ("[onInit] More than 8 topics passed!");
if (input_topics_.size() > 8) {
NODELET_ERROR("[onInit] More than 8 topics passed!");
return;
}
// ---[ Optional parameters
pnh_->getParam ("max_queue_size", maximum_queue_size_);
pnh_->getParam("max_queue_size", maximum_queue_size_);
// Output
pub_output_ = advertise<PointCloud2> (*pnh_, "output", maximum_queue_size_);
pub_output_ = advertise<PointCloud2>(*pnh_, "output", maximum_queue_size_);
onInitPostProcess ();
onInitPostProcess();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe ()
pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe()
{
ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:");
for (int d = 0; d < input_topics_.size (); ++d)
ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d]));
ROS_INFO_STREAM("Subscribing to " << input_topics_.size() << " user given topics as inputs:");
for (int d = 0; d < input_topics_.size(); ++d) {
ROS_INFO_STREAM(" - " << (std::string)(input_topics_[d]));
}
// Subscribe to the filters
filters_.resize (input_topics_.size ());
filters_.resize(input_topics_.size());
// 8 topics
if (approximate_sync_)
ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2>
> (maximum_queue_size_));
else
ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2>
> (maximum_queue_size_));
if (approximate_sync_) {
ts_a_.reset(
new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2,
PointCloud2,
PointCloud2, PointCloud2, PointCloud2,
PointCloud2, PointCloud2>
>(maximum_queue_size_));
} else {
ts_e_.reset(
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
for (int d = 0; d < input_topics_.size (); ++d)
{
filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
for (int d = 0; d < input_topics_.size(); ++d) {
filters_[d].reset(new message_filters::Subscriber<PointCloud2>());
filters_[d]->subscribe(*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
}
// 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:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
break;
}
{
if (approximate_sync_) {
ts_a_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
} else {
ts_e_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
}
break;
}
case 3:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
break;
}
{
if (approximate_sync_) {
ts_a_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
} else {
ts_e_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
}
break;
}
case 4:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
break;
}
{
if (approximate_sync_) {
ts_a_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
nf_);
} else {
ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_,
nf_);
}
break;
}
case 5:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
break;
}
{
if (approximate_sync_) {
ts_a_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
nf_, nf_, nf_);
} else {
ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
nf_, nf_, nf_);
}
break;
}
case 6:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
break;
}
{
if (approximate_sync_) {
ts_a_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], nf_, nf_);
} else {
ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], nf_, nf_);
}
break;
}
case 7:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
break;
}
{
if (approximate_sync_) {
ts_a_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], *filters_[6], nf_);
} else {
ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], *filters_[6], nf_);
}
break;
}
case 8:
{
if (approximate_sync_)
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
else
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
break;
}
{
if (approximate_sync_) {
ts_a_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], *filters_[6], *filters_[7]);
} else {
ts_e_->connectInput(
*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
*filters_[5], *filters_[6], *filters_[7]);
}
break;
}
default:
{
NODELET_FATAL ("Invalid 'input_topics' parameter given!");
return;
}
{
NODELET_FATAL("Invalid 'input_topics' parameter given!");
return;
}
}
if (approximate_sync_)
ts_a_->registerCallback (boost::bind (&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));
if (approximate_sync_) {
ts_a_->registerCallback(
boost::bind(
&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
pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe ()
pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe()
{
for (int d = 0; d < filters_.size (); ++d)
{
filters_[d]->unsubscribe ();
for (int d = 0; d < filters_.size(); ++d) {
filters_[d]->unsubscribe();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
void
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 ());
PointCloud2::Ptr in1_t (new PointCloud2 ());
PointCloud2::Ptr in2_t (new PointCloud2 ());
PointCloud2::Ptr in1_t(new PointCloud2());
PointCloud2::Ptr in2_t(new PointCloud2());
// Transform the point clouds into the specified output frame
if (output_frame_ != in1.header.frame_id)
pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_);
else
in1_t = boost::make_shared<PointCloud2> (in1);
if (output_frame_ != in1.header.frame_id) {
pcl_ros::transformPointCloud(output_frame_, in1, *in1_t, tf_);
} else {
in1_t = boost::make_shared<PointCloud2>(in1);
}
if (output_frame_ != in2.header.frame_id)
pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_);
else
in2_t = boost::make_shared<PointCloud2> (in2);
if (output_frame_ != in2.header.frame_id) {
pcl_ros::transformPointCloud(output_frame_, in2, *in2_t, tf_);
} else {
in2_t = boost::make_shared<PointCloud2>(in2);
}
// Concatenate the results
pcl::concatenatePointCloud (*in1_t, *in2_t, out);
pcl::concatenatePointCloud(*in1_t, *in2_t, out);
// Copy header
out.header.stamp = in1.header.stamp;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateDataSynchronizer::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
pcl_ros::PointCloudConcatenateDataSynchronizer::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)
{
PointCloud2::Ptr out1 (new PointCloud2 ());
PointCloud2::Ptr out2 (new PointCloud2 ());
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1);
if (in3 && in3->width * in3->height > 0)
{
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2);
PointCloud2::Ptr out1(new PointCloud2());
PointCloud2::Ptr out2(new PointCloud2());
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*in1, *in2, *out1);
if (in3 && in3->width * in3->height > 0) {
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in3, *out2);
out1 = out2;
if (in4 && in4->width * in4->height > 0)
{
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1);
if (in5 && in5->width * in5->height > 0)
{
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2);
if (in4 && in4->width * in4->height > 0) {
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in4, *out1);
if (in5 && in5->width * in5->height > 0) {
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in5, *out2);
out1 = out2;
if (in6 && in6->width * in6->height > 0)
{
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1);
if (in7 && in7->width * in7->height > 0)
{
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2);
if (in6 && in6->width * in6->height > 0) {
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out2, *in6, *out1);
if (in7 && in7->width * in7->height > 0) {
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(*out1, *in7, *out2);
out1 = out2;
}
}
}
}
}
}
pub_output_.publish (boost::make_shared<PointCloud2> (*out1));
pub_output_.publish(boost::make_shared<PointCloud2>(*out1));
}
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
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
{
nodelet_topic_tools::NodeletLazy::onInit ();
nodelet_topic_tools::NodeletLazy::onInit();
// ---[ Mandatory parameters
if (!pnh_->getParam ("input_messages", input_messages_))
{
NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
if (!pnh_->getParam("input_messages", input_messages_)) {
NODELET_ERROR("[onInit] Need a 'input_messages' parameter to be set before continuing!");
return;
}
if (input_messages_ < 2)
{
NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!");
if (input_messages_ < 2) {
NODELET_ERROR("[onInit] Invalid 'input_messages' parameter given!");
return;
}
// ---[ Optional parameters
pnh_->getParam ("max_queue_size", maximum_queue_size_);
pnh_->getParam ("maximum_seconds", maximum_seconds_);
pub_output_ = advertise<sensor_msgs::PointCloud2> (*pnh_, "output", maximum_queue_size_);
pnh_->getParam("max_queue_size", maximum_queue_size_);
pnh_->getParam("maximum_seconds", maximum_seconds_);
pub_output_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", maximum_queue_size_);
onInitPostProcess ();
onInitPostProcess();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe ()
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe()
{
sub_input_.shutdown ();
sub_input_.shutdown();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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.",
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(
"[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
if (maximum_seconds_ > 0 && queue_.size () > 0)
{
while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
if (maximum_seconds_ > 0 && queue_.size() > 0) {
while (fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ) > maximum_seconds_ &&
queue_.size() > 0)
{
NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
(*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ));
queue_.erase (queue_.begin ());
NODELET_WARN(
"[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
(*queue_.begin()).first.toSec(),
fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ));
queue_.erase(queue_.begin());
}
}
// Push back new data
queue_[cloud->header.stamp].push_back (cloud);
if ((int)queue_[cloud->header.stamp].size () >= input_messages_)
{
queue_[cloud->header.stamp].push_back(cloud);
if ((int)queue_[cloud->header.stamp].size() >= input_messages_) {
// 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];
// Resize the output dataset
int data_size = cloud_out.data.size ();
int nr_fields = cloud_out.fields.size ();
int data_size = cloud_out.data.size();
int nr_fields = cloud_out.fields.size();
int nr_points = cloud_out.width * cloud_out.height;
for (size_t i = 1; i < clouds.size (); ++i)
{
assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
for (size_t i = 1; i < clouds.size(); ++i) {
assert(
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)
{
NODELET_ERROR ("[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);
if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) {
NODELET_ERROR(
"[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);
break;
}
// Point step must increase with the length of each new field
cloud_out.point_step += clouds[i]->point_step;
// Resize data to hold all clouds
data_size += clouds[i]->data.size ();
data_size += clouds[i]->data.size();
// Concatenate fields
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);
for (size_t d = 0; d < clouds[i]->fields.size (); ++d)
{
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);
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].offset += delta_offset;
}
nr_fields += clouds[i]->fields.size ();
nr_fields += clouds[i]->fields.size();
}
// Recalculate row_step
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
int point_offset = 0;
for (int cp = 0; cp < nr_points; ++cp)
{
for (size_t i = 0; i < clouds.size (); ++i)
{
for (int cp = 0; cp < nr_points; ++cp) {
for (size_t i = 0; i < clouds.size(); ++i) {
// 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;
}
}
pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out));
queue_.erase (cloud->header.stamp);
pub_output_.publish(boost::make_shared<const PointCloud>(cloud_out));
queue_.erase(cloud->header.stamp);
}
// Clean the queue to avoid overflowing
if (maximum_queue_size_ > 0)
{
while ((int)queue_.size () > maximum_queue_size_)
queue_.erase (queue_.begin ());
if (maximum_queue_size_ > 0) {
while ((int)queue_.size() > maximum_queue_size_) {
queue_.erase(queue_.begin());
}
}
}
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_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> NodeletDEMUX;
@ -51,7 +52,6 @@ typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
//#include "concatenate_fields.cpp"
//#include "concatenate_data.cpp"
PLUGINLIB_EXPORT_CLASS(NodeletMUX,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(NodeletDEMUX,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet);
//PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);

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

@ -40,143 +40,150 @@
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PCDReader::onInit ()
pcl_ros::PCDReader::onInit()
{
PCLNodelet::onInit ();
PCLNodelet::onInit();
// 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 ("tf_frame", tf_frame_);
pnh_->getParam("publish_rate", publish_rate_);
pnh_->getParam("tf_frame", tf_frame_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - publish_rate : %f\n"
" - tf_frame : %s",
getName ().c_str (),
publish_rate_, tf_frame_.c_str ());
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - publish_rate : %f\n"
" - tf_frame : %s",
getName().c_str(),
publish_rate_, tf_frame_.c_str());
PointCloud2Ptr output_new;
output_ = boost::make_shared<PointCloud2> ();
output_new = boost::make_shared<PointCloud2> ();
output_ = boost::make_shared<PointCloud2>();
output_new = boost::make_shared<PointCloud2>();
// Wait in a loop until someone connects
do
{
ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
ros::spinOnce ();
ros::Duration (0.01).sleep ();
}
while (pnh_->ok () && pub_output.getNumSubscribers () == 0);
do {
ROS_DEBUG_ONCE("[%s::onInit] Waiting for a client to connect...", getName().c_str());
ros::spinOnce();
ros::Duration(0.01).sleep();
} while (pnh_->ok() && pub_output.getNumSubscribers() == 0);
std::string file_name;
while (pnh_->ok ())
{
while (pnh_->ok()) {
// Get the current filename parameter. If no filename set, loop
if (!pnh_->getParam ("filename", file_name_) && file_name_.empty ())
{
ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
ros::Duration (0.01).sleep ();
ros::spinOnce ();
if (!pnh_->getParam("filename", file_name_) && file_name_.empty()) {
ROS_ERROR_ONCE(
"[%s::onInit] Need a 'filename' parameter to be set before continuing!",
getName().c_str());
ros::Duration(0.01).sleep();
ros::spinOnce();
continue;
}
// If the filename parameter holds a different value than the last one we read
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 ());
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());
file_name = file_name_;
pcl::PCLPointCloud2 cloud;
if (impl_.read (file_name_, cloud) < 0)
{
NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
if (impl_.read(file_name_, cloud) < 0) {
NODELET_ERROR("[%s::onInit] Error reading %s !", getName().c_str(), file_name_.c_str());
return;
}
pcl_conversions::moveFromPCL(cloud, *(output_));
output_->header.stamp = ros::Time::now ();
output_->header.stamp = ros::Time::now();
output_->header.frame_id = tf_frame_;
}
// We do not publish empty data
if (output_->data.size () == 0)
if (output_->data.size() == 0) {
continue;
}
if (publish_rate_ == 0)
{
if (output_ != output_new)
{
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 ());
pub_output.publish (output_);
if (publish_rate_ == 0) {
if (output_ != output_new) {
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());
pub_output.publish(output_);
output_new = output_;
}
ros::Duration (0.01).sleep ();
}
else
{
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_->header.stamp = ros::Time::now ();
pub_output.publish (output_);
ros::Duration(0.01).sleep();
} else {
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_->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
pnh_->getParam ("publish_rate", publish_rate_);
pnh_->getParam ("tf_frame", tf_frame_);
pnh_->getParam("publish_rate", publish_rate_);
pnh_->getParam("tf_frame", tf_frame_);
}
onInitPostProcess ();
onInitPostProcess();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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
pnh_->getParam ("filename", file_name_);
pnh_->getParam ("binary_mode", binary_mode_);
pnh_->getParam("filename", file_name_);
pnh_->getParam("binary_mode", binary_mode_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - filename : %s\n"
" - binary_mode : %s",
getName ().c_str (),
file_name_.c_str (), (binary_mode_) ? "true" : "false");
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - filename : %s\n"
" - binary_mode : %s",
getName().c_str(),
file_name_.c_str(), (binary_mode_) ? "true" : "false");
onInitPostProcess ();
onInitPostProcess();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
pcl_ros::PCDWriter::input_callback(const PointCloud2ConstPtr & cloud)
{
if (!isValid (cloud))
if (!isValid(cloud)) {
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;
if (file_name_.empty ())
fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
else
if (file_name_.empty()) {
fname = boost::lexical_cast<std::string>(cloud->header.stamp.toSec()) + ".pcd";
} else {
fname = file_name_;
}
pcl::PCLPointCloud2 pcl_cloud;
// 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);
impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
pcl_conversions::moveToPCL(*(const_cast<PointCloud2 *>(cloud.get())), pcl_cloud);
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::PCDWriter PCDWriter;
PLUGINLIB_EXPORT_CLASS(PCDReader,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(PCDWriter,nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(PCDReader, 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
pcl_ros::EuclideanClusterExtraction::onInit ()
pcl_ros::EuclideanClusterExtraction::onInit()
{
// Call the super onInit ()
PCLNodelet::onInit ();
PCLNodelet::onInit();
// ---[ Mandatory parameters
double cluster_tolerance;
if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
{
NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ());
if (!pnh_->getParam("cluster_tolerance", cluster_tolerance)) {
NODELET_ERROR(
"[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!",
getName().c_str());
return;
}
int spatial_locator;
if (!pnh_->getParam ("spatial_locator", spatial_locator))
{
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
if (!pnh_->getParam("spatial_locator", spatial_locator)) {
NODELET_ERROR(
"[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return;
}
//private_nh.getParam ("use_indices", use_indices_);
pnh_->getParam ("publish_indices", publish_indices_);
pnh_->getParam("publish_indices", publish_indices_);
if (publish_indices_)
pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
else
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
if (publish_indices_) {
pub_output_ = advertise<PointIndices>(*pnh_, "output", max_queue_size_);
} else {
pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
}
// Enable the dynamic reconfigure service
srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>>(*pnh_);
dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind(
&EuclideanClusterExtraction::config_callback, this, _1, _2);
srv_->setCallback(f);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - max_queue_size : %d\n"
" - use_indices : %s\n"
" - cluster_tolerance : %f\n",
getName ().c_str (),
max_queue_size_,
(use_indices_) ? "true" : "false", cluster_tolerance);
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - max_queue_size : %d\n"
" - use_indices : %s\n"
" - cluster_tolerance : %f\n",
getName().c_str(),
max_queue_size_,
(use_indices_) ? "true" : "false", cluster_tolerance);
// Set given parameters here
impl_.setClusterTolerance (cluster_tolerance);
impl_.setClusterTolerance(cluster_tolerance);
onInitPostProcess ();
onInitPostProcess();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::EuclideanClusterExtraction::subscribe ()
pcl_ros::EuclideanClusterExtraction::subscribe()
{
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
if (use_indices_) {
// Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
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
{
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 {
// 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
pcl_ros::EuclideanClusterExtraction::unsubscribe ()
pcl_ros::EuclideanClusterExtraction::unsubscribe()
{
if (use_indices_)
{
sub_input_filter_.unsubscribe ();
sub_indices_filter_.unsubscribe ();
if (use_indices_) {
sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe();
} else {
sub_input_.shutdown();
}
else
sub_input_.shutdown ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
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)
{
impl_.setClusterTolerance (config.cluster_tolerance);
NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
if (impl_.getClusterTolerance() != config.cluster_tolerance) {
impl_.setClusterTolerance(config.cluster_tolerance);
NODELET_DEBUG(
"[%s::config_callback] Setting new clustering tolerance to: %f.",
getName().c_str(), config.cluster_tolerance);
}
if (impl_.getMinClusterSize () != config.cluster_min_size)
{
impl_.setMinClusterSize (config.cluster_min_size);
NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
if (impl_.getMinClusterSize() != config.cluster_min_size) {
impl_.setMinClusterSize(config.cluster_min_size);
NODELET_DEBUG(
"[%s::config_callback] Setting the minimum cluster size to: %d.",
getName().c_str(), config.cluster_min_size);
}
if (impl_.getMaxClusterSize () != config.cluster_max_size)
{
impl_.setMaxClusterSize (config.cluster_max_size);
NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
if (impl_.getMaxClusterSize() != config.cluster_max_size) {
impl_.setMaxClusterSize(config.cluster_max_size);
NODELET_DEBUG(
"[%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;
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
pcl_ros::EuclideanClusterExtraction::input_indices_callback (
const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
pcl_ros::EuclideanClusterExtraction::input_indices_callback(
const PointCloudConstPtr & cloud, const PointIndicesConstPtr & indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
if (pub_output_.getNumSubscribers() <= 0) {
return;
}
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
if (!isValid(cloud)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
if (indices && !isValid(indices)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
return;
}
@ -188,65 +204,72 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
if (indices) {
std_msgs::Header cloud_header = fromPCL(cloud->header);
std_msgs::Header indices_header = indices->header;
NODELET_DEBUG ("[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
NODELET_DEBUG(
"[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud_header.stamp.toSec(), cloud_header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
indices->indices.size(), indices_header.stamp.toSec(),
indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG ("[%s::input_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;
if (indices)
indices_ptr.reset (new std::vector<int> (indices->indices));
if (indices) {
indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setInputCloud (pcl_ptr(cloud));
impl_.setIndices (indices_ptr);
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices_ptr);
std::vector<pcl::PointIndices> clusters;
impl_.extract (clusters);
impl_.extract(clusters);
if (publish_indices_)
{
for (size_t i = 0; i < clusters.size (); ++i)
{
if ((int)i >= max_clusters_)
if (publish_indices_) {
for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) {
break;
}
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
pcl_msgs::PointIndices ros_pi;
moveFromPCL(clusters[i], ros_pi);
ros_pi.header.stamp += ros::Duration (i * 0.001);
pub_output_.publish (ros_pi);
ros_pi.header.stamp += ros::Duration(i * 0.001);
pub_output_.publish(ros_pi);
}
NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
}
else
{
for (size_t i = 0; i < clusters.size (); ++i)
{
if ((int)i >= max_clusters_)
NODELET_DEBUG(
"[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s",
clusters.size(), pnh_->resolveName("output").c_str());
} else {
for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) {
break;
}
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
//pcl::toROSMsg (output, output_blob);
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
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);
// Publish a Boost shared ptr const data
pub_output_.publish (ros_ptr(output.makeShared ()));
NODELET_DEBUG ("[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 ());
pub_output_.publish(ros_ptr(output.makeShared()));
NODELET_DEBUG(
"[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;
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
pcl_ros::ExtractPolygonalPrismData::onInit ()
pcl_ros::ExtractPolygonalPrismData::onInit()
{
// Call the super onInit ()
PCLNodelet::onInit ();
PCLNodelet::onInit();
// Enable the dynamic reconfigure service
srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>>(*pnh_);
dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind(
&ExtractPolygonalPrismData::config_callback, this, _1, _2);
srv_->setCallback(f);
// 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
pcl_ros::ExtractPolygonalPrismData::subscribe ()
pcl_ros::ExtractPolygonalPrismData::subscribe()
{
sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_hull_filter_.subscribe(*pnh_, "planar_hull", max_queue_size_);
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
// Create the objects here
if (approximate_sync_)
sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
else
sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
if (use_indices_)
{
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_);
if (approximate_sync_) {
sync_input_hull_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud, PointIndices>>>(max_queue_size_);
} else {
sync_input_hull_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloud, PointIndices>>>(max_queue_size_);
}
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_);
if (use_indices_) {
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_) {
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
if (approximate_sync_)
sync_input_hull_indices_a_->registerCallback (bind (&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));
if (approximate_sync_) {
sync_input_hull_indices_a_->registerCallback(
bind(
&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
pcl_ros::ExtractPolygonalPrismData::unsubscribe ()
pcl_ros::ExtractPolygonalPrismData::unsubscribe()
{
sub_hull_filter_.unsubscribe ();
sub_input_filter_.unsubscribe ();
sub_hull_filter_.unsubscribe();
sub_input_filter_.unsubscribe();
if (use_indices_)
sub_indices_filter_.unsubscribe ();
if (use_indices_) {
sub_indices_filter_.unsubscribe();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
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;
impl_.getHeightLimits (height_min, height_max);
if (height_min != config.height_min)
{
impl_.getHeightLimits(height_min, height_max);
if (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);
impl_.setHeightLimits (height_min, height_max);
NODELET_DEBUG(
"[%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;
NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
impl_.setHeightLimits (height_min, height_max);
NODELET_DEBUG(
"[%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
pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
const PointCloudConstPtr &cloud,
const PointCloudConstPtr &hull,
const PointIndicesConstPtr &indices)
pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
const PointCloudConstPtr & cloud,
const PointCloudConstPtr & hull,
const PointIndicesConstPtr & indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
if (pub_output_.getNumSubscribers() <= 0) {
return;
}
// Copy the header (stamp + frame_id)
pcl_msgs::PointIndices inliers;
inliers.header = fromPCL(cloud->header);
// If cloud is given, check if it's valid
if (!isValid (cloud) || !isValid (hull, "planar_hull"))
{
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
pub_output_.publish (inliers);
if (!isValid(cloud) || !isValid(hull, "planar_hull")) {
NODELET_ERROR("[%s::input_hull_indices_callback] Invalid input!", getName().c_str());
pub_output_.publish(inliers);
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
pub_output_.publish (inliers);
if (indices && !isValid(indices)) {
NODELET_ERROR("[%s::input_hull_indices_callback] Invalid indices!", getName().c_str());
pub_output_.publish(inliers);
return;
}
/// DEBUG
if (indices)
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.\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 (),
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 (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").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 (indices) {
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.\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(),
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(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").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)
{
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 ());
if (cloud->header.frame_id != hull->header.frame_id) {
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());
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
pub_output_.publish (inliers);
pub_output_.publish(inliers);
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;
if (indices && !indices->header.frame_id.empty ())
indices_ptr.reset (new std::vector<int> (indices->indices));
if (indices && !indices->header.frame_id.empty()) {
indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setInputCloud (pcl_ptr(cloud));
impl_.setIndices (indices_ptr);
impl_.setInputCloud(pcl_ptr(cloud));
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)
if (!cloud->points.empty ()) {
if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers;
moveToPCL(inliers, pcl_inliers);
impl_.segment (pcl_inliers);
impl_.segment(pcl_inliers);
moveFromPCL(pcl_inliers, inliers);
}
// Enforce that the TF frame and the timestamp are copied
inliers.header = fromPCL(cloud->header);
pub_output_.publish (inliers);
NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
pub_output_.publish(inliers);
NODELET_DEBUG(
"[%s::input_hull_callback] Publishing %zu indices.",
getName().c_str(), inliers.indices.size());
}
typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet)

795
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
pcl_ros::SegmentDifferences::onInit ()
pcl_ros::SegmentDifferences::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"
" - max_queue_size : %d",
getName ().c_str (),
max_queue_size_);
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - max_queue_size : %d",
getName().c_str(),
max_queue_size_);
onInitPostProcess ();
onInitPostProcess();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::subscribe ()
pcl_ros::SegmentDifferences::subscribe()
{
// Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_);
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_target_filter_.subscribe(*pnh_, "target", max_queue_size_);
// Enable the dynamic reconfigure service
srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<SegmentDifferencesConfig>>(*pnh_);
dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind(
&SegmentDifferences::config_callback, this, _1, _2);
srv_->setCallback(f);
if (approximate_sync_)
{
sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_);
sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
}
else
{
sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, 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));
if (approximate_sync_) {
sync_input_target_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud>>>(max_queue_size_);
sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_a_->registerCallback(
bind(
&SegmentDifferences::input_target_callback, this,
_1, _2));
} else {
sync_input_target_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
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
pcl_ros::SegmentDifferences::unsubscribe ()
pcl_ros::SegmentDifferences::unsubscribe()
{
sub_input_filter_.unsubscribe ();
sub_target_filter_.unsubscribe ();
sub_input_filter_.unsubscribe();
sub_target_filter_.unsubscribe();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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)
{
impl_.setDistanceThreshold (config.distance_threshold);
NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
if (impl_.getDistanceThreshold() != config.distance_threshold) {
impl_.setDistanceThreshold(config.distance_threshold);
NODELET_DEBUG(
"[%s::config_callback] Setting new distance threshold to: %f.",
getName().c_str(), config.distance_threshold);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud,
const PointCloudConstPtr &cloud_target)
pcl_ros::SegmentDifferences::input_target_callback(
const PointCloudConstPtr & cloud,
const PointCloudConstPtr & cloud_target)
{
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 ()));
if (pub_output_.getNumSubscribers() <= 0) {
return;
}
NODELET_DEBUG ("[%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 ());
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;
}
impl_.setInputCloud (pcl_ptr(cloud));
impl_.setTargetCloud (pcl_ptr(cloud_target));
NODELET_DEBUG(
"[%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;
impl_.segment (output);
impl_.segment(output);
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 (),
output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ());
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(),
output.points.size(), fromPCL(output.header).stamp.toSec(),
pnh_->resolveName("output").c_str());
}
typedef pcl_ros::SegmentDifferences SegmentDifferences;
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 "sac_segmentation.cpp"
//#include "segment_differences.cpp"

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

@ -42,20 +42,21 @@
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::ConvexHull2D::onInit ()
pcl_ros::ConvexHull2D::onInit()
{
PCLNodelet::onInit ();
PCLNodelet::onInit();
pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
pub_plane_ = advertise<geometry_msgs::PolygonStamped> (*pnh_, "output_polygon", max_queue_size_);
pub_output_ = advertise<PointCloud>(*pnh_, "output", max_queue_size_);
pub_plane_ = advertise<geometry_msgs::PolygonStamped>(*pnh_, "output_polygon", max_queue_size_);
// ---[ 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"
" - use_indices : %s",
getName ().c_str (),
(use_indices_) ? "true" : "false");
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - use_indices : %s",
getName().c_str(),
(use_indices_) ? "true" : "false");
onInitPostProcess();
}
@ -65,139 +66,150 @@ void
pcl_ros::ConvexHull2D::subscribe()
{
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
if (use_indices_) {
// 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
sub_indices_filter_.subscribe (*pnh_, "indices", 1);
sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>>(max_queue_size_);
// 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_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
bind(
&ConvexHull2D::input_indices_callback, this, _1,
_2));
} 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
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_->connectInput(sub_input_filter_, sub_indices_filter_);
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)
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
pcl_ros::ConvexHull2D::unsubscribe()
{
if (use_indices_)
{
if (use_indices_) {
sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe();
}
else
} else {
sub_input_.shutdown();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud,
const PointIndicesConstPtr &indices)
pcl_ros::ConvexHull2D::input_indices_callback(
const PointCloudConstPtr & cloud,
const PointIndicesConstPtr & indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
if (pub_output_.getNumSubscribers() <= 0 && pub_plane_.getNumSubscribers() <= 0) {
return;
}
PointCloud output;
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
if (!isValid(cloud)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
// Publish an empty message
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices, "indices"))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
if (indices && !isValid(indices, "indices")) {
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
// Publish an empty message
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
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 (),
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 ());
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName().c_str(),
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(),
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
IndicesPtr indices_ptr;
if (indices)
indices_ptr.reset (new std::vector<int> (indices->indices));
if (indices) {
indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setInputCloud (pcl_ptr(cloud));
impl_.setIndices (indices_ptr);
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices_ptr);
// Estimate the feature
impl_.reconstruct (output);
impl_.reconstruct(output);
// 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;
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)
pcl::Vector4fMap O = output.points[1].getVector4fMap ();
pcl::Vector4fMap B = output.points[0].getVector4fMap ();
pcl::Vector4fMap A = output.points[2].getVector4fMap ();
pcl::Vector4fMap O = output.points[1].getVector4fMap();
pcl::Vector4fMap B = output.points[0].getVector4fMap();
pcl::Vector4fMap A = output.points[2].getVector4fMap();
// Check the direction of points -- polygon must have CCW direction
Eigen::Vector4f OA = A - O;
Eigen::Vector4f OB = B - O;
Eigen::Vector4f N = OA.cross3 (OB);
double theta = N.dot (O);
Eigen::Vector4f N = OA.cross3(OB);
double theta = N.dot(O);
bool reversed = false;
if (theta < (M_PI / 2.0))
if (theta < (M_PI / 2.0)) {
reversed = true;
for (size_t i = 0; i < output.points.size (); ++i)
{
if (reversed)
{
size_t j = output.points.size () - i - 1;
}
for (size_t i = 0; i < output.points.size(); ++i) {
if (reversed) {
size_t j = output.points.size() - i - 1;
poly.polygon.points[i].x = output.points[j].x;
poly.polygon.points[i].y = output.points[j].y;
poly.polygon.points[i].z = output.points[j].z;
}
else
{
} else {
poly.polygon.points[i].x = output.points[i].x;
poly.polygon.points[i].y = output.points[i].y;
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
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
}
typedef pcl_ros::ConvexHull2D ConvexHull2D;
PLUGINLIB_EXPORT_CLASS(ConvexHull2D, nodelet::Nodelet)

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

@ -40,192 +40,211 @@
#include <pcl/io/io.h>
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::MovingLeastSquares::onInit ()
pcl_ros::MovingLeastSquares::onInit()
{
PCLNodelet::onInit ();
PCLNodelet::onInit();
//ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
pub_output_ = advertise<PointCloudIn> (*pnh_, "output", 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 ("search_radius", search_radius_))
{
pub_output_ = advertise<PointCloudIn>(*pnh_, "output", 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("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] 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;
}
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
{
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
NODELET_ERROR(
"[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName().c_str());
return;
}
// Enable the dynamic reconfigure service
srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_);
dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 );
srv_->setCallback (f);
srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig>>(*pnh_);
dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind(
&MovingLeastSquares::config_callback, this, _1, _2);
srv_->setCallback(f);
// ---[ 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"
" - use_indices : %s",
getName ().c_str (),
(use_indices_) ? "true" : "false");
NODELET_DEBUG(
"[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - use_indices : %s",
getName().c_str(),
(use_indices_) ? "true" : "false");
onInitPostProcess ();
onInitPostProcess();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::MovingLeastSquares::subscribe ()
pcl_ros::MovingLeastSquares::subscribe()
{
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
if (use_indices_) {
// 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
sub_indices_filter_.subscribe (*pnh_, "indices", 1);
sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
// 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_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
bind(
&MovingLeastSquares::input_indices_callback,
this, _1, _2));
} 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
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_->connectInput(sub_input_filter_, sub_indices_filter_);
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)
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 ();
sub_input_ =
pnh_->subscribe<PointCloudIn>(
"input", 1,
bind(&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr()));
}
else
sub_input_.shutdown ();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud,
const PointIndicesConstPtr &indices)
pcl_ros::MovingLeastSquares::unsubscribe()
{
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
if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
if (pub_output_.getNumSubscribers() <= 0 && pub_normals_.getNumSubscribers() <= 0) {
return;
}
// Output points have the same type as the input, they are only smoothed
PointCloudIn output;
// 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 (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
if (!isValid(cloud)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices, "indices"))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
if (indices && !isValid(indices, "indices")) {
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
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 (),
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 ());
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName().c_str(),
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(),
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
impl_.setInputCloud (pcl_ptr(cloud));
impl_.setInputCloud(pcl_ptr(cloud));
IndicesPtr indices_ptr;
if (indices)
indices_ptr.reset (new std::vector<int> (indices->indices));
if (indices) {
indices_ptr.reset(new std::vector<int>(indices->indices));
}
impl_.setIndices (indices_ptr);
impl_.setIndices(indices_ptr);
// Initialize the spatial locator
// Do the reconstructon
//impl_.process (output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish (ros_ptr(output.makeShared ()));
pub_output_.publish(ros_ptr(output.makeShared()));
normals->header = cloud->header;
pub_normals_.publish (ros_ptr(normals));
pub_normals_.publish(ros_ptr(normals));
}
//////////////////////////////////////////////////////////////////////////////////////////////
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?
/*if (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;
NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
impl_.setSearchRadius (search_radius_);
NODELET_DEBUG("[config_callback] Setting the search radius: %f.", 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;
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;
NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
impl_.setPolynomialFit (use_polynomial_fit_);
NODELET_DEBUG(
"[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;
NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
impl_.setPolynomialOrder (polynomial_order_);
NODELET_DEBUG("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
impl_.setPolynomialOrder(polynomial_order_);
}
if (gaussian_parameter_ != config.gaussian_parameter)
{
if (gaussian_parameter_ != config.gaussian_parameter) {
gaussian_parameter_ = config.gaussian_parameter;
NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
NODELET_DEBUG("[config_callback] Setting the gaussian parameter to: %f.", 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 "convex_hull.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
/// a little to round it to millisecond. This is because converting back
/// 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
static const uint32_t mult = 1e6;
@ -71,7 +71,7 @@ void setStamp(ros::Time &stamp, std::uint64_t &pcl_stamp)
{
ros::Time 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:
Notification(int expected_count)
: count_(0)
, expected_count_(expected_count)
, failure_count_(0)
: count_(0),
expected_count_(expected_count),
failure_count_(0)
{
}
void notify(const PCDType::ConstPtr& message)
void notify(const PCDType::ConstPtr & message)
{
++count_;
}
void failure(const PCDType::ConstPtr& message, FilterFailureReason reason)
void failure(const PCDType::ConstPtr & message, FilterFailureReason reason)
{
++failure_count_;
}
@ -144,7 +144,11 @@ TEST(MessageFilter, preexistingTransforms)
ros::Time stamp = ros::Time::now();
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);
msg->header.frame_id = "frame2";
@ -169,7 +173,11 @@ TEST(MessageFilter, postTransforms)
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);
ros::WallDuration(0.1).sleep();
@ -190,8 +198,7 @@ TEST(MessageFilter, queueSize)
std::uint64_t 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);
msg->header.stamp = pcl_stamp;
msg->header.frame_id = "frame2";
@ -202,7 +209,11 @@ TEST(MessageFilter, queueSize)
EXPECT_EQ(0, n.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);
ros::WallDuration(0.1).sleep();
@ -224,7 +235,11 @@ TEST(MessageFilter, setTargetFrame)
setStamp(stamp, msg->header.stamp);
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);
filter.add(msg);
@ -249,7 +264,11 @@ TEST(MessageFilter, multipleTargetFrames)
PCDType::Ptr msg(new PCDType);
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);
msg->header.frame_id = "frame3";
@ -283,7 +302,11 @@ TEST(MessageFilter, tolerance)
ros::Time stamp = ros::Time::now();
std::uint64_t 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);
PCDType::Ptr msg(new PCDType);
@ -295,7 +318,7 @@ TEST(MessageFilter, tolerance)
//ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
transform.stamp_ += offset*1.1;
transform.stamp_ += offset * 1.1;
tf_client.setTransform(transform);
ros::WallDuration(0.1).sleep();
@ -323,7 +346,11 @@ TEST(MessageFilter, outTheBackFailure)
PCDType::Ptr msg(new PCDType);
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);
transform.stamp_ = stamp + ros::Duration(10000);
@ -359,16 +386,17 @@ TEST(MessageFilter, removeCallback)
// TF filters; no data needs to be published
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();
for (int i = 0; i < 3; ++i) {
tf_listener.reset(new tf::TransformListener());
// Have callback fire at high rate to increase chances of race condition
tf_filter.reset(
new tf::MessageFilter<PCDType>(*tf_listener,
"map", 5, threaded_nh,
ros::Duration(0.000001)));
new tf::MessageFilter<PCDType>(
*tf_listener,
"map", 5, threaded_nh,
ros::Duration(0.000001)));
// Sleep and reset; sleeping increases chances of race condition
ros::Duration(0.001).sleep();
@ -378,7 +406,7 @@ TEST(MessageFilter, removeCallback)
spinner.stop();
}
int main(int argc, char** argv)
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);

372
pcl_ros/src/transforms.cpp
View File

@ -45,159 +45,145 @@ namespace pcl_ros
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)
transformPointCloud(
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;
return (true);
return true;
}
// Get the TF transform
tf::StampedTransform transform;
try
{
tf_listener.lookupTransform (target_frame, in.header.frame_id, 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);
try {
tf_listener.lookupTransform(target_frame, in.header.frame_id, 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;
}
// Convert the TF transform to Eigen format
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;
return (true);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform,
const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
void
transformPointCloud(
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;
return;
}
// Get the transformation
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;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out)
void
transformPointCloud(
const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in,
sensor_msgs::PointCloud2 & out)
{
// Get X-Y-Z indices
int x_idx = pcl::getFieldIndex (in, "x");
int y_idx = pcl::getFieldIndex (in, "y");
int z_idx = pcl::getFieldIndex (in, "z");
int x_idx = pcl::getFieldIndex(in, "x");
int y_idx = pcl::getFieldIndex(in, "y");
int z_idx = pcl::getFieldIndex(in, "z");
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.");
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.");
return;
}
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[z_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[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;
}
// Check if distance is available
int dist_idx = pcl::getFieldIndex (in, "distance");
int dist_idx = pcl::getFieldIndex(in, "distance");
// Copy the other data
if (&in != &out)
{
if (&in != &out) {
out.header = in.header;
out.height = in.height;
out.width = in.width;
out.width = in.width;
out.fields = in.fields;
out.is_bigendian = in.is_bigendian;
out.point_step = in.point_step;
out.row_step = in.row_step;
out.is_dense = in.is_dense;
out.data.resize (in.data.size ());
out.point_step = in.point_step;
out.row_step = in.row_step;
out.is_dense = in.is_dense;
out.data.resize(in.data.size());
// 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)
{
Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
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]],
*(float *)&in.data[xyz_offset[2]], 1);
Eigen::Vector4f pt_out;
bool max_range_point = false;
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]));
if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
{
if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point
{
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]));
if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
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_out = transform * pt;
max_range_point = true;
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
}
}
else
{
} else {
pt_out = transform * pt;
}
if (max_range_point)
{
if (max_range_point) {
// 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();
}
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[2]], &pt_out[2], 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[2]], &pt_out[2], sizeof(float));
xyz_offset += in.point_step;
}
// Check if the viewpoint information is present
int vp_idx = pcl::getFieldIndex (in, "vp_x");
if (vp_idx != -1)
{
int vp_idx = pcl::getFieldIndex(in, "vp_x");
if (vp_idx != -1) {
// Transform the viewpoint info too
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];
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];
// 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;
pstep[0] = vp_out[0];
@ -209,73 +195,193 @@ transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointC
//////////////////////////////////////////////////////////////////////////////////////////////
void
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat)
{
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 (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 (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
out_mat (0, 3) = origin.x ();
out_mat (1, 3) = origin.y ();
out_mat (2, 3) = origin.z ();
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(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(0, 3) = origin.x();
out_mat(1, 3) = origin.y();
out_mat(2, 3) = origin.z();
}
} // 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::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 void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
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::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 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::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 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::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::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (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 void pcl_ros::transformPointCloud<pcl::PointXYZ>(
const pcl::PointCloud<pcl::PointXYZ> &,
pcl::PointCloud<pcl::PointXYZ> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZI>(
const pcl::PointCloud<pcl::PointXYZI> &,
pcl::PointCloud<pcl::PointXYZI> &,
const tf::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
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::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, 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 pcl::PointCloud<pcl::PointXYZ> &,
pcl::PointCloud<pcl::PointXYZ> &,
const tf::TransformListener &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
const std::string &,
const pcl::PointCloud<pcl::PointXYZI> &,
pcl::PointCloud<pcl::PointXYZI> &,
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::PointXYZI> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZI> &, 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 &);
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::PointXYZI>(
const std::string &, const ros::Time &,
const pcl::PointCloud<pcl::PointXYZI> &,
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
main (int argc, char** argv)
main(int argc, char ** argv)
{
ros::init (argc, argv, "bag_to_pcd");
if (argc < 4)
{
std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl;
return (-1);
ros::init(argc, argv, "bag_to_pcd");
if (argc < 4) {
std::cerr << "Syntax is: " << argv[0] <<
" <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;
return -1;
}
// TF
@ -79,87 +80,77 @@ int
rosbag::View view;
rosbag::View::iterator view_it;
try
{
bag.open (argv[1], rosbag::bagmode::Read);
}
catch (rosbag::BagException)
{
try {
bag.open(argv[1], rosbag::bagmode::Read);
} catch (rosbag::BagException) {
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 ("tf/tfMessage"));
view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage"));
view_it = view.begin ();
view.addQuery(bag, rosbag::TypeQuery("sensor_msgs/PointCloud2"));
view.addQuery(bag, rosbag::TypeQuery("tf/tfMessage"));
view.addQuery(bag, rosbag::TypeQuery("tf2_msgs/TFMessage"));
view_it = view.begin();
std::string output_dir = std::string (argv[3]);
boost::filesystem::path outpath (output_dir);
if (!boost::filesystem::exists (outpath))
{
if (!boost::filesystem::create_directories (outpath))
{
std::string output_dir = std::string(argv[3]);
boost::filesystem::path outpath(output_dir);
if (!boost::filesystem::exists(outpath)) {
if (!boost::filesystem::create_directories(outpath)) {
std::cerr << "Error creating directory " << output_dir << std::endl;
return (-1);
return -1;
}
std::cerr << "Creating directory " << output_dir << std::endl;
}
// Add the PointCloud2 handler
std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl;
std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " <<
output_dir << std::endl;
PointCloud cloud_t;
ros::Duration r (0.001);
ros::Duration r(0.001);
// Loop over the whole bag file
while (view_it != view.end ())
{
while (view_it != view.end()) {
// Handle TF messages first
tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> ();
if (tf != NULL)
{
tf_broadcaster.sendTransform (tf->transforms);
ros::spinOnce ();
r.sleep ();
}
else
{
tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage>();
if (tf != NULL) {
tf_broadcaster.sendTransform(tf->transforms);
ros::spinOnce();
r.sleep();
} else {
// Get the PointCloud2 message
PointCloudConstPtr cloud = view_it->instantiate<PointCloud> ();
if (cloud == NULL)
{
PointCloudConstPtr cloud = view_it->instantiate<PointCloud>();
if (cloud == NULL) {
++view_it;
continue;
}
// If a target_frame was specified
if(argc > 4)
{
if (argc > 4) {
// Transform it
if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener))
{
++view_it;
continue;
if (!pcl_ros::transformPointCloud(argv[4], *cloud, cloud_t, tf_listener)) {
++view_it;
continue;
}
}
else
{
} else {
// Else, don't transform it
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;
ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
std::cerr << "Data saved to " << ss.str () << std::endl;
pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
Eigen::Quaternionf::Identity (), true);
std::cerr << "Data saved to " << ss.str() << std::endl;
pcl::io::savePCDFile(
ss.str(), cloud_t, Eigen::Vector4f::Zero(),
Eigen::Quaternionf::Identity(), true);
}
// Increment the iterator
++view_it;
}
return (0);
return 0;
}
/* ]--- */

38
pcl_ros/tools/convert_pcd_to_image.cpp
View File

@ -56,41 +56,37 @@
/* ---[ */
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::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;
return 1;
}
sensor_msgs::Image image;
sensor_msgs::PointCloud2 cloud;
pcl::io::loadPCDFile (std::string (argv[1]), cloud);
pcl::io::loadPCDFile(std::string(argv[1]), cloud);
try
{
pcl::toROSMsg (cloud, image); //convert the cloud
}
catch (std::runtime_error &e)
{
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
try {
pcl::toROSMsg(cloud, image); //convert the cloud
} catch (std::runtime_error & e) {
ROS_ERROR_STREAM(
"Error in converting cloud to image message: " <<
e.what());
return 1; //fail!
}
ros::Rate loop_rate (5);
while (nh.ok ())
{
image_pub.publish (image);
ros::spinOnce ();
loop_rate.sleep ();
ros::Rate loop_rate(5);
while (nh.ok()) {
image_pub.publish(image);
ros::spinOnce();
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:
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!");
return;
}
try
{
pcl::toROSMsg (*cloud, image_); //convert the cloud
try {
pcl::toROSMsg(*cloud, image_); //convert the cloud
image_.header = cloud->header;
image_pub_.publish (image_); //publish our cloud image
}
catch (std::runtime_error &e)
{
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
image_pub_.publish(image_); //publish our cloud image
} catch (std::runtime_error & e) {
ROS_ERROR_STREAM(
"Error in converting cloud to image message: " <<
e.what());
}
}
PointCloudToImage () : cloud_topic_("input"),image_topic_("output")
PointCloudToImage()
: cloud_topic_("input"), image_topic_("output")
{
sub_ = nh_.subscribe (cloud_topic_, 30,
&PointCloudToImage::cloud_cb, this);
image_pub_ = nh_.advertise<sensor_msgs::Image> (image_topic_, 30);
sub_ = nh_.subscribe(
cloud_topic_, 30,
&PointCloudToImage::cloud_cb, this);
image_pub_ = nh_.advertise<sensor_msgs::Image>(image_topic_, 30);
//print some info about the node
std::string r_ct = nh_.resolveName (cloud_topic_);
std::string r_it = nh_.resolveName (image_topic_);
ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct );
ROS_INFO_STREAM("Publishing image on topic " << r_it );
std::string r_ct = nh_.resolveName(cloud_topic_);
std::string r_it = nh_.resolveName(image_topic_);
ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct);
ROS_INFO_STREAM("Publishing image on topic " << r_it);
}
private:
ros::NodeHandle nh_;
sensor_msgs::Image image_; //cache the image message
@ -95,10 +95,10 @@ private:
};
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
ros::spin (); //where she stops nobody knows
ros::spin(); //where she stops nobody knows
return 0;
}

167
pcl_ros/tools/pcd_to_pointcloud.cpp
View File

@ -60,113 +60,112 @@ using namespace std;
class PCDGenerator
{
protected:
string tf_frame_;
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
public:
protected:
string tf_frame_;
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
// ROS messages
sensor_msgs::PointCloud2 cloud_;
public:
// ROS messages
sensor_msgs::PointCloud2 cloud_;
string file_name_, cloud_topic_;
double wait_;
string file_name_, cloud_topic_;
double wait_;
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
////////////////////////////////////////////////////////////////////////////////
PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~")
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
////////////////////////////////////////////////////////////////////////////////
PCDGenerator()
: tf_frame_("/base_link"), private_nh_("~")
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
cloud_topic_ = "cloud_pcd";
pub_.advertise (nh_, cloud_topic_.c_str (), 1);
private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
cloud_topic_ = "cloud_pcd";
pub_.advertise(nh_, cloud_topic_.c_str(), 1);
private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
ROS_INFO(
"Publishing data on topic %s with frame_id %s.", nh_.resolveName(
cloud_topic_).c_str(), tf_frame_.c_str());
}
////////////////////////////////////////////////////////////////////////////////
// Start
int
start()
{
if (file_name_ == "" || pcl::io::loadPCDFile(file_name_, cloud_) == -1) {
return -1;
}
cloud_.header.frame_id = tf_frame_;
return 0;
}
////////////////////////////////////////////////////////////////////////////////
// Start
int
start ()
{
if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1)
return (-1);
cloud_.header.frame_id = tf_frame_;
return (0);
}
////////////////////////////////////////////////////////////////////////////////
// Spin (!)
bool spin()
{
int nr_points = cloud_.width * cloud_.height;
string fields_list = pcl::getFieldsList(cloud_);
double interval = wait_ * 1e+6;
while (nh_.ok()) {
ROS_DEBUG_ONCE(
"Publishing data with %d points (%s) on topic %s in frame %s.", nr_points,
fields_list.c_str(), nh_.resolveName(cloud_topic_).c_str(), cloud_.header.frame_id.c_str());
cloud_.header.stamp = ros::Time::now();
////////////////////////////////////////////////////////////////////////////////
// Spin (!)
bool spin ()
{
int nr_points = cloud_.width * cloud_.height;
string fields_list = pcl::getFieldsList (cloud_);
double interval = wait_ * 1e+6;
while (nh_.ok ())
{
ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ());
cloud_.header.stamp = ros::Time::now ();
if (pub_.getNumSubscribers () > 0)
{
ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ());
pub_.publish (cloud_);
}
else
{
// 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;
}
if (pub_.getNumSubscribers() > 0) {
ROS_DEBUG("Publishing data to %d subscribers.", pub_.getNumSubscribers());
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
main (int argc, char** argv)
main(int argc, char ** argv)
{
if (argc < 2)
{
std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
return (-1);
if (argc < 2) {
std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" <<
std::endl;
return -1;
}
ros::init (argc, argv, "pcd_to_pointcloud");
ros::init(argc, argv, "pcd_to_pointcloud");
PCDGenerator c;
c.file_name_ = string (argv[1]);
c.file_name_ = string(argv[1]);
// check if publishing interval is given
if (argc == 2)
{
c.wait_ = 0;
}
else
{
c.wait_ = atof (argv[2]);
}
if (c.start () == -1)
{
ROS_ERROR ("Could not load file %s. Exiting.", argv[1]);
return (-1);
if (argc == 2) {
c.wait_ = 0;
} else {
c.wait_ = atof(argv[2]);
}
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
{
protected:
ros::NodeHandle nh_;
protected:
ros::NodeHandle nh_;
private:
std::string prefix_;
bool binary_;
bool compressed_;
std::string fixed_frame_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
private:
std::string prefix_;
bool binary_;
bool compressed_;
std::string fixed_frame_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
public:
string cloud_topic_;
public:
string cloud_topic_;
ros::Subscriber sub_;
ros::Subscriber sub_;
////////////////////////////////////////////////////////////////////////////////
// Callback
void
cloud_cb (const boost::shared_ptr<const pcl::PCLPointCloud2>& cloud)
{
if ((cloud->width * cloud->height) == 0)
////////////////////////////////////////////////////////////////////////////////
// Callback
void
cloud_cb(const boost::shared_ptr<const pcl::PCLPointCloud2> & cloud)
{
if ((cloud->width * cloud->height) == 0) {
return;
}
ROS_INFO(
"Received %d data points in frame %s with the following fields: %s",
(int)cloud->width * cloud->height,
cloud->header.frame_id.c_str(),
pcl::getFieldsList(*cloud).c_str());
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;
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;
ss << prefix_ << cloud->header.stamp << ".pcd";
ROS_INFO ("Data saved to %s", ss.str ().c_str ());
pcl::PCDWriter writer;
if(binary_)
{
if(compressed_)
{
writer.writeBinaryCompressed (ss.str (), *cloud, v, q);
}
else
{
writer.writeBinary (ss.str (), *cloud, v, q);
}
}
else
{
writer.writeASCII (ss.str (), *cloud, v, q, 8);
}
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>();
}
////////////////////////////////////////////////////////////////////////////////
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_);
}
std::stringstream ss;
ss << prefix_ << cloud->header.stamp << ".pcd";
ROS_INFO("Data saved to %s", ss.str().c_str());
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 ());
pcl::PCDWriter writer;
if (binary_) {
if (compressed_) {
writer.writeBinaryCompressed(ss.str(), *cloud, v, q);
} else {
writer.writeBinary(ss.str(), *cloud, v, q);
}
} else {
writer.writeASCII(ss.str(), *cloud, v, q, 8);
}
}
////////////////////////////////////////////////////////////////////////////////
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
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;
ros::spin ();
ros::spin();
return (0);
return 0;
}
/* ]--- */