From 63cee139f1950f021191a3b1fe0dc8aeedf61770 Mon Sep 17 00:00:00 2001 From: Sean Kelly Date: Thu, 6 Aug 2020 15:28:29 -0400 Subject: [PATCH] Apply ament_uncrustify --reformat (#297) Signed-off-by: Sean Kelly --- pcl_ros/include/pcl_ros/features/boundary.hpp | 67 +- pcl_ros/include/pcl_ros/features/feature.hpp | 351 ++++---- pcl_ros/include/pcl_ros/features/fpfh.hpp | 95 ++- pcl_ros/include/pcl_ros/features/fpfh_omp.hpp | 88 +- .../pcl_ros/features/moment_invariants.hpp | 61 +- .../include/pcl_ros/features/normal_3d.hpp | 68 +- .../pcl_ros/features/normal_3d_omp.hpp | 55 +- .../pcl_ros/features/normal_3d_tbb.hpp | 57 +- pcl_ros/include/pcl_ros/features/pfh.hpp | 95 ++- .../pcl_ros/features/principal_curvatures.hpp | 63 +- pcl_ros/include/pcl_ros/features/shot.hpp | 57 +- pcl_ros/include/pcl_ros/features/shot_omp.hpp | 54 +- pcl_ros/include/pcl_ros/features/vfh.hpp | 63 +- pcl_ros/include/pcl_ros/filters/crop_box.hpp | 98 +-- .../pcl_ros/filters/extract_indices.hpp | 83 +- pcl_ros/include/pcl_ros/filters/filter.hpp | 187 ++-- .../include/pcl_ros/filters/passthrough.hpp | 90 +- .../pcl_ros/filters/project_inliers.hpp | 112 +-- .../filters/radius_outlier_removal.hpp | 85 +- .../filters/statistical_outlier_removal.hpp | 98 +-- .../include/pcl_ros/filters/voxel_grid.hpp | 68 +- pcl_ros/include/pcl_ros/impl/transforms.hpp | 210 +++-- pcl_ros/include/pcl_ros/io/bag_io.hpp | 131 +-- .../include/pcl_ros/io/concatenate_data.hpp | 160 ++-- .../include/pcl_ros/io/concatenate_fields.hpp | 78 +- pcl_ros/include/pcl_ros/io/pcd_io.hpp | 152 ++-- pcl_ros/include/pcl_ros/pcl_nodelet.hpp | 309 +++---- pcl_ros/include/pcl_ros/point_cloud.hpp | 661 ++++++++------- pcl_ros/include/pcl_ros/publisher.hpp | 170 ++-- .../pcl_ros/segmentation/extract_clusters.hpp | 105 +-- .../extract_polygonal_prism_data.hpp | 132 +-- .../pcl_ros/segmentation/sac_segmentation.hpp | 422 +++++----- .../segmentation/segment_differences.hpp | 107 +-- .../include/pcl_ros/surface/convex_hull.hpp | 75 +- .../pcl_ros/surface/moving_least_squares.hpp | 170 ++-- pcl_ros/include/pcl_ros/transforms.hpp | 232 ++--- pcl_ros/src/pcl_ros/features/boundary.cpp | 29 +- pcl_ros/src/pcl_ros/features/feature.cpp | 690 ++++++++------- pcl_ros/src/pcl_ros/features/fpfh.cpp | 34 +- pcl_ros/src/pcl_ros/features/fpfh_omp.cpp | 34 +- .../pcl_ros/features/moment_invariants.cpp | 30 +- pcl_ros/src/pcl_ros/features/normal_3d.cpp | 30 +- .../src/pcl_ros/features/normal_3d_omp.cpp | 30 +- .../src/pcl_ros/features/normal_3d_tbb.cpp | 34 +- pcl_ros/src/pcl_ros/features/pfh.cpp | 34 +- .../pcl_ros/features/principal_curvatures.cpp | 34 +- pcl_ros/src/pcl_ros/features/shot.cpp | 34 +- pcl_ros/src/pcl_ros/features/shot_omp.cpp | 34 +- pcl_ros/src/pcl_ros/features/vfh.cpp | 34 +- pcl_ros/src/pcl_ros/filters/crop_box.cpp | 70 +- .../src/pcl_ros/filters/extract_indices.cpp | 29 +- .../src/pcl_ros/filters/features/boundary.cpp | 39 +- .../src/pcl_ros/filters/features/feature.cpp | 604 +++++++------ pcl_ros/src/pcl_ros/filters/features/fpfh.cpp | 40 +- .../src/pcl_ros/filters/features/fpfh_omp.cpp | 40 +- .../filters/features/moment_invariants.cpp | 36 +- .../pcl_ros/filters/features/normal_3d.cpp | 36 +- .../filters/features/normal_3d_omp.cpp | 36 +- .../filters/features/normal_3d_tbb.cpp | 36 +- pcl_ros/src/pcl_ros/filters/features/pfh.cpp | 40 +- .../filters/features/principal_curvatures.cpp | 40 +- pcl_ros/src/pcl_ros/filters/features/vfh.cpp | 40 +- pcl_ros/src/pcl_ros/filters/filter.cpp | 194 +++-- pcl_ros/src/pcl_ros/filters/passthrough.cpp | 79 +- .../src/pcl_ros/filters/project_inliers.cpp | 132 +-- .../filters/radius_outlier_removal.cpp | 40 +- .../filters/statistical_outlier_removal.cpp | 51 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 94 ++- pcl_ros/src/pcl_ros/io/bag_io.cpp | 83 +- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 326 +++---- pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 119 +-- pcl_ros/src/pcl_ros/io/io.cpp | 8 +- pcl_ros/src/pcl_ros/io/pcd_io.cpp | 163 ++-- .../pcl_ros/segmentation/extract_clusters.cpp | 239 +++--- .../extract_polygonal_prism_data.cpp | 222 ++--- .../pcl_ros/segmentation/sac_segmentation.cpp | 795 ++++++++++-------- .../segmentation/segment_differences.cpp | 130 +-- .../src/pcl_ros/segmentation/segmentation.cpp | 2 - pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 164 ++-- .../pcl_ros/surface/moving_least_squares.cpp | 225 ++--- pcl_ros/src/pcl_ros/surface/surface.cpp | 2 - .../src/test/test_tf_message_filter_pcl.cpp | 72 +- pcl_ros/src/transforms.cpp | 372 +++++--- pcl_ros/tools/bag_to_pcd.cpp | 101 +-- pcl_ros/tools/convert_pcd_to_image.cpp | 38 +- pcl_ros/tools/convert_pointcloud_to_image.cpp | 46 +- pcl_ros/tools/pcd_to_pointcloud.cpp | 167 ++-- pcl_ros/tools/pointcloud_to_pcd.cpp | 197 +++-- 88 files changed, 6019 insertions(+), 5318 deletions(-) diff --git a/pcl_ros/include/pcl_ros/features/boundary.hpp b/pcl_ros/include/pcl_ros/features/boundary.hpp index 350426e8..e226359e 100644 --- a/pcl_ros/include/pcl_ros/features/boundary.hpp +++ b/pcl_ros/include/pcl_ros/features/boundary.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::BoundaryEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - - diff --git a/pcl_ros/include/pcl_ros/features/feature.hpp b/pcl_ros/include/pcl_ros/features/feature.hpp index 155e6b61..bf1a9d83 100644 --- a/pcl_ros/include/pcl_ros/features/feature.hpp +++ b/pcl_ros/include/pcl_ros/features/feature.hpp @@ -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 KdTree; + typedef pcl::KdTree::Ptr KdTreePtr; + + typedef pcl::PointCloud PointCloudIn; + typedef boost::shared_ptr PointCloudInPtr; + typedef boost::shared_ptr 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 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> 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 nf_pi_; + message_filters::PassThrough 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 KdTree; - typedef pcl::KdTree::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(indices)); + } - typedef pcl::PointCloud PointCloudIn; - typedef boost::shared_ptr PointCloudInPtr; - typedef boost::shared_ptr PointCloudInConstPtr; +private: + /** \brief Synchronized input, surface, and point indices.*/ + boost::shared_ptr>> sync_input_surface_indices_a_; + boost::shared_ptr>> 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 PointCloudN; + typedef boost::shared_ptr PointCloudNPtr; + typedef boost::shared_ptr 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 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 > srv_; +private: + /** \brief The normals PointCloud subscriber filter. */ + message_filters::Subscriber 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>> sync_input_normals_surface_indices_a_; + boost::shared_ptr>> 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 nf_pi_; - message_filters::PassThrough 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 (indices)); - } - - private: - /** \brief Synchronized input, surface, and point indices.*/ - boost::shared_ptr > > sync_input_surface_indices_a_; - boost::shared_ptr > > 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 PointCloudN; - typedef boost::shared_ptr PointCloudNPtr; - typedef boost::shared_ptr 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 sub_normals_filter_; - - /** \brief Synchronized input, normals, surface, and point indices.*/ - boost::shared_ptr > > sync_input_normals_surface_indices_a_; - boost::shared_ptr > > 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_ diff --git a/pcl_ros/include/pcl_ros/features/fpfh.hpp b/pcl_ros/include/pcl_ros/features/fpfh.hpp index 8e3647a0..98d45ab6 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh.hpp +++ b/pcl_ros/include/pcl_ros/features/fpfh.hpp @@ -43,58 +43,57 @@ namespace pcl_ros { - /** \brief @b FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud - * dataset containing points and normals. - * - * @note If you use this code in any academic work, please cite: - * - *
    - *
  • 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. - *
    • - *
  • 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. - *
    • - *
- * - * @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 Fast Point Feature Histogram (FPFH) descriptor for a given point cloud + * dataset containing points and normals. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • 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. + *
    • + *
  • 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. + *
    • + *
+ * + * @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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::FPFHEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - - diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp index 5710120a..c3c88cf0 100644 --- a/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp @@ -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: - * - *
    - *
  • 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. - *
    • - *
  • 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. - *
    • - *
- * \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: + * + *
    + *
  • 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. + *
    • + *
  • 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. + *
    • + *
+ * \author Radu Bogdan Rusu + */ +class FPFHEstimationOMP : public FeatureFromNormals +{ +private: + pcl::FPFHEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::FPFHEstimationOMP impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.hpp b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp index c5b255c6..1c48ec47 100644 --- a/pcl_ros/include/pcl_ros/features/moment_invariants.hpp +++ b/pcl_ros/include/pcl_ros/features/moment_invariants.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::MomentInvariantsEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - - diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.hpp b/pcl_ros/include/pcl_ros/features/normal_3d.hpp index 82ff188a..8c6d5626 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - /** \brief PCL implementation object. */ - pcl::NormalEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp index 624d780f..4b96a87c 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::NormalEstimationOMP impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp index 15282141..fcc41029 100644 --- a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp +++ b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::NormalEstimationTBB impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - - diff --git a/pcl_ros/include/pcl_ros/features/pfh.hpp b/pcl_ros/include/pcl_ros/features/pfh.hpp index 9ce4d101..ff6490dc 100644 --- a/pcl_ros/include/pcl_ros/features/pfh.hpp +++ b/pcl_ros/include/pcl_ros/features/pfh.hpp @@ -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: - * - *
    - *
  • 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. - *
    • - *
  • 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. - *
    • - *
- * - * @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: + * + *
    + *
  • 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. + *
    • + *
  • 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. + *
    • + *
+ * + * @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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::PFHEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - - diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp index 98cd4134..e150189d 100644 --- a/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp +++ b/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::PrincipalCurvaturesEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ diff --git a/pcl_ros/include/pcl_ros/features/shot.hpp b/pcl_ros/include/pcl_ros/features/shot.hpp index 214736be..a3cdb3e1 100644 --- a/pcl_ros/include/pcl_ros/features/shot.hpp +++ b/pcl_ros/include/pcl_ros/features/shot.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::SHOTEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - - diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.hpp b/pcl_ros/include/pcl_ros/features/shot_omp.hpp index c02ca498..77d6a3f8 100644 --- a/pcl_ros/include/pcl_ros/features/shot_omp.hpp +++ b/pcl_ros/include/pcl_ros/features/shot_omp.hpp @@ -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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::SHOTEstimationOMP impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ - diff --git a/pcl_ros/include/pcl_ros/features/vfh.hpp b/pcl_ros/include/pcl_ros/features/vfh.hpp index 526cdcdf..efe5d631 100644 --- a/pcl_ros/include/pcl_ros/features/vfh.hpp +++ b/pcl_ros/include/pcl_ros/features/vfh.hpp @@ -43,41 +43,42 @@ namespace pcl_ros { - /** \brief @b VFHEstimation estimates the Viewpoint Feature Histogram (VFH) 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 Viewpoint Feature Histogram (VFH) 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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit(ros::NodeHandle & nh) { - private: - pcl::VFHEstimation impl_; + // Create the output publisher + pub_output_ = advertise(nh, "output", max_queue_size_); + return true; + } - typedef pcl::PointCloud 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 (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_ diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index cb376caf..234a9a07 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -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> 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 > 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 impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +private: + /** \brief The PCL filter implementation used. */ + pcl::CropBox impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_ diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index a3b12678..1eeddb38 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -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> 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 > 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 impl_; - private: - /** \brief The PCL filter implementation used. */ - pcl::ExtractIndices impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ - diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index cd622991..40955e25 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -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 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 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> 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>> sync_input_indices_e_; + boost::shared_ptr>> 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 > srv_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - /** \brief Dynamic reconfigure service callback. */ - virtual void - config_callback (pcl_ros::FilterConfig &config, uint32_t level); - - /** \brief PointCloud2 + Indices data callback. */ - void - input_indices_callback (const PointCloud2::ConstPtr &cloud, - const PointIndicesConstPtr &indices); - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTER_H_ diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index 1be1ef97..515fb897 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -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> 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 > 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 impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +private: + /** \brief The PCL filter implementation used. */ + pcl::PassThrough impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp index f72a8c2d..d17d2466 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -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 sub_model_; - /** \brief The message filter subscriber for model coefficients. */ - message_filters::Subscriber sub_model_; + /** \brief Synchronized input, indices, and model coefficients.*/ + boost::shared_ptr>> sync_input_indices_model_e_; + boost::shared_ptr>> sync_input_indices_model_a_; + /** \brief The PCL filter implementation used. */ + pcl::ProjectInliers impl_; - /** \brief Synchronized input, indices, and model coefficients.*/ - boost::shared_ptr > > sync_input_indices_model_e_; - boost::shared_ptr > > sync_input_indices_model_a_; - /** \brief The PCL filter implementation used. */ - pcl::ProjectInliers impl_; + /** \brief Nodelet initialization routine. */ + virtual void + onInit(); - /** \brief 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_ diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp index 3b1120ad..c773ccc4 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -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> 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 > 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 impl_; - - private: - /** \brief The PCL filter implementation used. */ - pcl::RadiusOutlierRemoval impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp index 6776cc19..2ed4d167 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -47,60 +47,62 @@ namespace pcl_ros { - /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more - * information check: - *
    - *
  • R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. - * Towards 3D Point Cloud Based Object Maps for Household Environments - * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. - *
- * - * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. - * \author Radu Bogdan Rusu +/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more + * information check: + *
    + *
  • R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. + * Towards 3D Point Cloud Based Object Maps for Household Environments + * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. + *
+ * + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + */ +class StatisticalOutlierRemoval : public Filter +{ +protected: + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; + + /** \brief Call the actual filter. + * \param input the input point cloud dataset + * \param indices the input set of indices to use from \a input + * \param output the resultant filtered dataset */ - 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 > 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 impl_; - private: - /** \brief The PCL filter implementation used. */ - pcl::StatisticalOutlierRemoval impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 6f900f23..398be306 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -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> srv_; + + /** \brief The PCL filter implementation used. */ + pcl::VoxelGrid impl_; + + /** \brief Call the actual filter. + * \param input the input point cloud dataset + * \param indices the input set of indices to use from \a input + * \param output the resultant filtered dataset */ - class VoxelGrid : public Filter - { - protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > srv_; + virtual void + filter( + const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output); - /** \brief The PCL filter implementation used. */ - pcl::VoxelGrid 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_ diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index a4ab2121..794a8d93 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -46,178 +46,176 @@ using pcl_conversions::toPCL; namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////// -template void -transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, const tf::Transform &transform) +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 void -transformPointCloud (const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, const tf::Transform &transform) +template +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 bool -transformPointCloudWithNormals (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 bool -transformPointCloud (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloud( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 bool -transformPointCloud (const std::string &target_frame, - const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloud( + const std::string & target_frame, + const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & 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 bool -transformPointCloudWithNormals (const std::string &target_frame, - const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &cloud_out, - const tf::TransformListener &tf_listener) +template +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & 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 diff --git a/pcl_ros/include/pcl_ros/io/bag_io.hpp b/pcl_ros/include/pcl_ros/io/bag_io.hpp index f526f2ea..17a936da 100644 --- a/pcl_ros/include/pcl_ros/io/bag_io.hpp +++ b/pcl_ros/include/pcl_ros/io/bag_io.hpp @@ -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(); + ++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 (); - ++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 +}; } diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp index 89603d2a..00f54fdd 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_data.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_data.hpp @@ -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>> 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 nf_; + + /** \brief Synchronizer. + * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer. + */ + boost::shared_ptr>> ts_a_; + boost::shared_ptr>> ts_e_; + + /** \brief Input point cloud callback. + * Because we want to use the same synchronizer object, we push back + * empty elements with the same timestamp. + */ + inline void + input_callback(const PointCloud2ConstPtr & input) { - 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(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 > > 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 nf_; - - /** \brief Synchronizer. - * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer. - */ - boost::shared_ptr > > ts_a_; - boost::shared_ptr > > ts_e_; - - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback (const PointCloud2ConstPtr &input) - { - PointCloud2 cloud; - cloud.header.stamp = input->header.stamp; - nf_.add (boost::make_shared (cloud)); - } - - /** \brief Input callback for 8 synchronized topics. */ - void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, - const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, - const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, - const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8); - - void combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out); - }; + void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out); +}; } #endif //#ifndef PCL_ROS_IO_CONCATENATE_H_ diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp index efa1c458..2b81af4c 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp @@ -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 > queue_; - }; + /** \brief A queue for messages. */ + std::map> queue_; +}; } #endif //#ifndef PCL_IO_CONCATENATE_H_ diff --git a/pcl_ros/include/pcl_ros/io/pcd_io.hpp b/pcl_ros/include/pcl_ros/io/pcd_io.hpp index f551817b..afcde06e 100644 --- a/pcl_ros/include/pcl_ros/io/pcd_io.hpp +++ b/pcl_ros/include/pcl_ros/io/pcd_io.hpp @@ -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_ diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp index fc4b64c8..28d8304a 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp @@ -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 PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr 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 sub_input_filter_; + + /** \brief The message filter subscriber for PointIndices. */ + message_filters::Subscriber sub_indices_filter_; + + /** \brief The output PointCloud publisher. */ + ros::Publisher pub_output_; + + /** \brief The maximum queue size (default: 3). */ + int max_queue_size_; + + /** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ + bool approximate_sync_; + + /** \brief TF listener object. */ + tf::TransformListener tf_listener_; + + /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). + * \param cloud the point cloud to test + * \param topic_name an optional topic name (only used for printing, defaults to "input") + */ + inline bool + isValid(const PointCloud2::ConstPtr & cloud, const std::string & topic_name = "input") { - 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 PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr 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 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 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_ diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index 32837d5c..b64b50de 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -11,286 +11,295 @@ #include #include -namespace pcl +namespace pcl { - namespace detail +namespace detail +{ +template +struct FieldStreamer +{ + FieldStreamer(Stream & stream) + : stream_(stream) {} + + template + void operator()() { - template - struct FieldStreamer - { - FieldStreamer(Stream& stream) : stream_(stream) {} + const char * name = traits::name::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 void operator() () - { - const char* name = traits::name::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::value; + stream_.next(offset); - std::uint32_t offset = traits::offset::value; - stream_.next(offset); + std::uint8_t datatype = traits::datatype::value; + stream_.next(datatype); - std::uint8_t datatype = traits::datatype::value; - stream_.next(datatype); + std::uint32_t count = traits::datatype::size; + stream_.next(count); + } - std::uint32_t count = traits::datatype::size; - stream_.next(count); - } + Stream & stream_; +}; - Stream& stream_; - }; +template +struct FieldsLength +{ + FieldsLength() + : length(0) {} - template - struct FieldsLength - { - FieldsLength() : length(0) {} + template + void operator()() + { + std::uint32_t name_length = strlen(traits::name::value); + length += name_length + 13; + } - template void operator() () - { - std::uint32_t name_length = strlen(traits::name::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 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 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 - struct DefaultMessageCreator > - { - boost::shared_ptr mapping_; +template +struct DefaultMessageCreator> +{ + boost::shared_ptr mapping_; - DefaultMessageCreator() - : mapping_( boost::make_shared() ) - { - } - - boost::shared_ptr > operator() () - { - boost::shared_ptr > msg (new pcl::PointCloud ()); - pcl::detail::getMapping(*msg) = mapping_; - return msg; - } - }; + DefaultMessageCreator() + : mapping_(boost::make_shared() ) + { + } + + boost::shared_ptr> operator()() + { + boost::shared_ptr> msg(new pcl::PointCloud()); + pcl::detail::getMapping(*msg) = mapping_; + return msg; + } +}; #endif - namespace message_traits +namespace message_traits +{ +template +struct MD5Sum> +{ + static const char * value() {return MD5Sum::value();} + static const char * value(const pcl::PointCloud &) {return value();} + + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::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 +struct DataType> +{ + static const char * value() {return DataType::value();} + static const char * value(const pcl::PointCloud &) {return value();} +}; + +template +struct Definition> +{ + static const char * value() {return Definition::value();} + static const char * value(const pcl::PointCloud &) {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 +struct HasHeader>: FalseType {}; + +template +struct TimeStamp> +{ + // 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 & m) { - template struct MD5Sum > - { - static const char* value() { return MD5Sum::value(); } - static const char* value(const pcl::PointCloud&) { return value(); } - - static const uint64_t static_value1 = MD5Sum::static_value1; - static const uint64_t static_value2 = MD5Sum::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 struct DataType > - { - static const char* value() { return DataType::value(); } - static const char* value(const pcl::PointCloud&) { return value(); } - }; - - template struct Definition > - { - static const char* value() { return Definition::value(); } - static const char* value(const pcl::PointCloud&) { 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 struct HasHeader > : FalseType {}; - - template - struct TimeStamp > - { - // 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 &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& 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& m) { - return pcl_conversions::fromPCL(m.header).stamp; - } - private: - static boost::shared_ptr header_; - static boost::shared_ptr header_const_; - }; - - template - struct FrameId > - { - static std::string* pointer(pcl::PointCloud& m) { return &m.header.frame_id; } - static std::string const* pointer(const pcl::PointCloud& m) { return &m.header.frame_id; } - static std::string value(const pcl::PointCloud& 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 & m) { - template - struct Serializer > + 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 & m) + { + return pcl_conversions::fromPCL(m.header).stamp; + } + +private: + static boost::shared_ptr header_; + static boost::shared_ptr header_const_; +}; + +template +struct FrameId> +{ + static std::string * pointer(pcl::PointCloud & m) {return &m.header.frame_id;} + static std::string const * pointer(const pcl::PointCloud & m) {return &m.header.frame_id;} + static std::string value(const pcl::PointCloud & m) {return m.header.frame_id;} +}; + +} // namespace ros::message_traits + +namespace serialization +{ +template +struct Serializer> +{ + template + inline static void write(Stream & stream, const pcl::PointCloud & 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::type FieldList; + uint32_t fields_size = boost::mpl::size::value; + stream.next(fields_size); + pcl::for_each_type(pcl::detail::FieldStreamer(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 + inline static void read(Stream & stream, pcl::PointCloud & 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 fields; + stream.next(fields); + + // Construct field mapping if deserializing for the first time + boost::shared_ptr & 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 & mapping = *mapping_ptr; + if (mapping.empty()) { + pcl::createMapping(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(&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 - inline static void write(Stream& stream, const pcl::PointCloud& 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::type FieldList; - uint32_t fields_size = boost::mpl::size::value; - stream.next(fields_size); - pcl::for_each_type(pcl::detail::FieldStreamer(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 - inline static void read(Stream& stream, pcl::PointCloud& 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 fields; - stream.next(fields); - - // Construct field mapping if deserializing for the first time - boost::shared_ptr& 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& mapping = *mapping_ptr; - if (mapping.empty()) - pcl::createMapping (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(&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& 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 & m) + { + uint32_t length = 0; - pcl::detail::FieldsLength fl; - typedef typename pcl::traits::fieldList::type FieldList; - pcl::for_each_type(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 fl; + typedef typename pcl::traits::fieldList::type FieldList; + pcl::for_each_type(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 - constexpr static bool pcl_uses_boost = std::is_same, - pcl::shared_ptr>::value; +template +constexpr static bool pcl_uses_boost = std::is_same, + pcl::shared_ptr>::value; #else - template - constexpr static bool pcl_uses_boost = true; +template +constexpr static bool pcl_uses_boost = true; #endif - template struct Holder - { - SharedPointer p; +template +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 - inline std::shared_ptr to_std_ptr(const boost::shared_ptr &p) - { - typedef Holder> H; - if(H *h = boost::get_deleter(p)) - { - return h->p; - } - else - { - return std::shared_ptr(p.get(), Holder>(p)); - } - } +template +inline std::shared_ptr to_std_ptr(const boost::shared_ptr & p) +{ + typedef Holder> H; + if (H * h = boost::get_deleter(p)) { + return h->p; + } else { + return std::shared_ptr(p.get(), Holder>(p)); + } +} - template - inline boost::shared_ptr to_boost_ptr(const std::shared_ptr &p) - { - typedef Holder> H; - if(H * h = std::get_deleter(p)) - { - return h->p; - } - else - { - return boost::shared_ptr(p.get(), Holder>(p)); - } - } +template +inline boost::shared_ptr to_boost_ptr(const std::shared_ptr & p) +{ + typedef Holder> H; + if (H * h = std::get_deleter(p)) { + return h->p; + } else { + return boost::shared_ptr(p.get(), Holder>(p)); + } +} #endif - } // namespace pcl::detail +} // namespace pcl::detail // add functions to convert to smart pointer used by ROS - template - inline boost::shared_ptr ros_ptr(const boost::shared_ptr &p) - { - return p; - } +template +inline boost::shared_ptr ros_ptr(const boost::shared_ptr & p) +{ + return p; +} #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED - template - inline boost::shared_ptr ros_ptr(const std::shared_ptr &p) - { - return detail::to_boost_ptr(p); - } +template +inline boost::shared_ptr ros_ptr(const std::shared_ptr & p) +{ + return detail::to_boost_ptr(p); +} // add functions to convert to smart pointer used by PCL, based on PCL's own pointer - template >::type> - inline std::shared_ptr pcl_ptr(const std::shared_ptr &p) - { - return p; - } +template>::type> +inline std::shared_ptr pcl_ptr(const std::shared_ptr & p) +{ + return p; +} - template >::type> - inline std::shared_ptr pcl_ptr(const boost::shared_ptr &p) - { - return detail::to_std_ptr(p); - } +template>::type> +inline std::shared_ptr pcl_ptr(const boost::shared_ptr & p) +{ + return detail::to_std_ptr(p); +} - template >::type> - inline boost::shared_ptr pcl_ptr(const std::shared_ptr &p) - { - return detail::to_boost_ptr(p); - } +template>::type> +inline boost::shared_ptr pcl_ptr(const std::shared_ptr & p) +{ + return detail::to_boost_ptr(p); +} - template >::type> - inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) - { - return p; - } +template>::type> +inline boost::shared_ptr pcl_ptr(const boost::shared_ptr & p) +{ + return p; +} #else - template - inline boost::shared_ptr pcl_ptr(const boost::shared_ptr &p) - { - return p; - } +template +inline boost::shared_ptr pcl_ptr(const boost::shared_ptr & p) +{ + return p; +} #endif } // namespace pcl diff --git a/pcl_ros/include/pcl_ros/publisher.hpp b/pcl_ros/include/pcl_ros/publisher.hpp index b752418c..9ef707af 100644 --- a/pcl_ros/include/pcl_ros/publisher.hpp +++ b/pcl_ros/include/pcl_ros/publisher.hpp @@ -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 -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(topic, queue_size); - } + pub_ = nh.advertise(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 - 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 > &point_cloud) const - { - publish (*point_cloud); - } - - inline void - publish (const pcl::PointCloud& point_cloud) const - { - // Fill point cloud binary data - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg (point_cloud, msg); - pub_.publish (boost::make_shared (msg)); - } - }; - - template <> - class Publisher : 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 (point_cloud)); - //pub_.publish (point_cloud); - } - }; +template +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> & point_cloud) const + { + publish(*point_cloud); + } + + inline void + publish(const pcl::PointCloud & point_cloud) const + { + // Fill point cloud binary data + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(point_cloud, msg); + pub_.publish(boost::make_shared(msg)); + } +}; + +template<> +class Publisher: 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(point_cloud)); + //pub_.publish (point_cloud); + } +}; } #endif //#ifndef PCL_ROS_PUBLISHER_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp index b9c48425..cc189658 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp @@ -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::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> 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::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 > srv_; +private: + /** \brief The PCL implementation used. */ + pcl::EuclideanClusterExtraction 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>> sync_input_indices_e_; + boost::shared_ptr>> 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 impl_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp index 458e565d..147f41d4 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp @@ -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 PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + +protected: + /** \brief The output PointIndices publisher. */ + ros::Publisher pub_output_; + + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_hull_filter_; + + /** \brief Synchronized input, planar hull, and indices.*/ + boost::shared_ptr>> sync_input_hull_indices_e_; + boost::shared_ptr>> sync_input_hull_indices_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough 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 PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + PointIndices cloud; + cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp; + nf_.add(boost::make_shared(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 sub_hull_filter_; + /** \brief LazyNodelet connection routine. */ + void subscribe(); + void unsubscribe(); - /** \brief Synchronized input, planar hull, and indices.*/ - boost::shared_ptr > > sync_input_hull_indices_e_; - boost::shared_ptr > > 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 > 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 nf_; +private: + /** \brief The PCL implementation used. */ + pcl::ExtractPolygonalPrismData 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 (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 impl_; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index e0f68690..475adf02 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -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 PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr 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 PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr 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 > 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 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 impl_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr > > sync_input_indices_e_; - boost::shared_ptr > > 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> 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 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 PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + indices_ = *indices; + } - typedef pcl::PointCloud PointCloudN; - typedef boost::shared_ptr PointCloudNPtr; - typedef boost::shared_ptr 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 impl_; - protected: - // ROS nodelet attributes - /** \brief The normals PointCloud subscriber filter. */ - message_filters::Subscriber sub_normals_filter_; + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr>> sync_input_indices_e_; + boost::shared_ptr>> 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 > 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 (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 PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_; + typedef pcl::PointCloud PointCloudN; + typedef boost::shared_ptr PointCloudNPtr; + typedef boost::shared_ptr 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 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 impl_; + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; - /** \brief Synchronized input, normals, and indices.*/ - boost::shared_ptr > > sync_input_normals_indices_a_; - boost::shared_ptr > > 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(indices)); + } - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough 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 impl_; + + /** \brief Synchronized input, normals, and indices.*/ + boost::shared_ptr>> sync_input_normals_indices_a_; + boost::shared_ptr>> sync_input_normals_indices_e_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp index ee7fb28c..ba0cd5b1 100644 --- a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp @@ -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 PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr PointCloudConstPtr; + +public: + /** \brief Empty constructor. */ + SegmentDifferences() {} + +protected: + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_target_filter_; + + /** \brief Synchronized input, and planar hull.*/ + boost::shared_ptr>> sync_input_target_e_; + boost::shared_ptr>> sync_input_target_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> 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 PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr 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 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 > > sync_input_target_e_; - boost::shared_ptr > > sync_input_target_a_; +private: + /** \brief The PCL implementation used. */ + pcl::SegmentDifferences impl_; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr > 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 impl_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.hpp b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp index 066d7a16..726584a3 100644 --- a/pcl_ros/include/pcl_ros/surface/convex_hull.hpp +++ b/pcl_ros/include/pcl_ros/surface/convex_hull.hpp @@ -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 PointCloud; + typedef boost::shared_ptr PointCloudPtr; + typedef boost::shared_ptr 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 PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr 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 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 impl_; + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr>> sync_input_indices_e_; + boost::shared_ptr>> 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 > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp index 36542087..0c0bb060 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp @@ -46,105 +46,107 @@ // Dynamic reconfigure #include -#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 PointCloudIn; + typedef boost::shared_ptr PointCloudInPtr; + typedef boost::shared_ptr PointCloudInConstPtr; + typedef pcl::PointCloud NormalCloudOut; + + typedef pcl::KdTree KdTree; + typedef pcl::KdTree::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 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 PointCloudIn; - typedef boost::shared_ptr PointCloudInPtr; - typedef boost::shared_ptr PointCloudInConstPtr; - typedef pcl::PointCloud NormalCloudOut; + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr> srv_; - typedef pcl::KdTree KdTree; - typedef pcl::KdTree::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 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>> sync_input_indices_e_; + boost::shared_ptr>> sync_input_indices_a_; - // ROS nodelet attributes - /** \brief The surface PointCloud subscriber filter. */ - message_filters::Subscriber 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 > 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 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 > > sync_input_indices_e_; - boost::shared_ptr > > sync_input_indices_a_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; } #endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ diff --git a/pcl_ros/include/pcl_ros/transforms.hpp b/pcl_ros/include/pcl_ros/transforms.hpp index 36e5d805..28d85a62 100644 --- a/pcl_ros/include/pcl_ros/transforms.hpp +++ b/pcl_ros/include/pcl_ros/transforms.hpp @@ -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 void - transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, - pcl::PointCloud &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 +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 bool - transformPointCloudWithNormals (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &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 +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 bool - transformPointCloudWithNormals (const std::string &target_frame, - const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &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 +bool +transformPointCloudWithNormals( + const std::string & target_frame, + const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & 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 void - transformPointCloud (const pcl::PointCloud &cloud_in, - pcl::PointCloud &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 +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 bool - transformPointCloud (const std::string &target_frame, - const pcl::PointCloud &cloud_in, - pcl::PointCloud &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 +bool +transformPointCloud( + const std::string & target_frame, + const pcl::PointCloud & cloud_in, + pcl::PointCloud & 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 bool - transformPointCloud (const std::string &target_frame, const ros::Time & target_time, - const pcl::PointCloud &cloud_in, - const std::string &fixed_frame, - pcl::PointCloud &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 +bool +transformPointCloud( + const std::string & target_frame, const ros::Time & target_time, + const pcl::PointCloud & cloud_in, + const std::string & fixed_frame, + pcl::PointCloud & 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_ - diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp index 6571ed30..6ac61f4a 100644 --- a/pcl_ros/src/pcl_ros/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/features/boundary.cpp @@ -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; diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp index fcb9516d..146b20d0 100644 --- a/pcl_ros/src/pcl_ros/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/features/feature.cpp @@ -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 >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + 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 > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::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 > >(max_queue_size_); - else - sync_input_surface_indices_e_ = boost::make_shared > >(max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - if (use_indices_) - { + 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 ("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( + "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 (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(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 >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + 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 > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::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 > > (max_queue_size_); - else - sync_input_normals_surface_indices_e_ = boost::make_shared > > (max_queue_size_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_normals_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages - if (use_indices_ || use_surface_) - { - if (use_indices_) - { + if (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 (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud, cloud_normals, cloud_surface, vindices); + computePublish(cloud, cloud_normals, cloud_surface, vindices); } - diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp index c1ac221a..e6c05b34 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh.cpp @@ -38,39 +38,39 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp index 8dfaf044..d3e96fc4 100644 --- a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp @@ -38,39 +38,39 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp index a6652dec..5a5c1452 100644 --- a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp @@ -38,37 +38,37 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp index 489fa473..7ab774bc 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d.cpp @@ -38,37 +38,37 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp index b5aadc94..c7cd2124 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp @@ -38,37 +38,37 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp index b76bf1aa..abb16d8b 100644 --- a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp @@ -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 - diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp index b7210316..e55a7fea 100644 --- a/pcl_ros/src/pcl_ros/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/features/pfh.cpp @@ -38,39 +38,39 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp index a5332a62..ff44b843 100644 --- a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp @@ -38,39 +38,39 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/shot.cpp b/pcl_ros/src/pcl_ros/features/shot.cpp index eca60020..d0664724 100644 --- a/pcl_ros/src/pcl_ros/features/shot.cpp +++ b/pcl_ros/src/pcl_ros/features/shot.cpp @@ -37,39 +37,39 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/pcl_ros/src/pcl_ros/features/shot_omp.cpp index 70ed3974..0d10be21 100644 --- a/pcl_ros/src/pcl_ros/features/shot_omp.cpp +++ b/pcl_ros/src/pcl_ros/features/shot_omp.cpp @@ -37,39 +37,39 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp index c7cb6cc3..25c6911b 100644 --- a/pcl_ros/src/pcl_ros/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/features/vfh.cpp @@ -38,39 +38,39 @@ #include #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) - diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index c7f1d517..83dec1eb 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -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 > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&CropBox::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::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); diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 59554f0e..7ee29be0 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -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 > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractIndices::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &ExtractIndices::config_callback, this, _1, _2); + srv_->setCallback(f); use_indices_ = true; - return (true); + 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); diff --git a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp index 13a66c84..13a1139e 100644 --- a/pcl_ros/src/pcl_ros/filters/features/boundary.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/boundary.cpp @@ -38,40 +38,41 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/feature.cpp b/pcl_ros/src/pcl_ros/filters/features/feature.cpp index 1e36d828..85204ff9 100644 --- a/pcl_ros/src/pcl_ros/filters/features/feature.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/feature.cpp @@ -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 >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + 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 > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &Feature::config_callback, this, _1, _2); + srv_->setCallback(f); // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages - if (use_indices_ || use_surface_) - { + if (use_indices_ || use_surface_) { // Create the objects here - if (approximate_sync_) - sync_input_surface_indices_a_ = boost::make_shared > >(max_queue_size_); - else - sync_input_surface_indices_e_ = boost::make_shared > >(max_queue_size_); + if (approximate_sync_) { + sync_input_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // Subscribe to the input using a filter - sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); - if (use_indices_) - { + 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 ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "input", max_queue_size_, + bind( + &Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(), + PointIndicesConstPtr())); + } - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - use_surface : %s\n" - " - k_search : %d\n" - " - radius_search : %f\n" - " - spatial_locator: %d", - getName ().c_str (), - (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_); + 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 (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(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 >, as NormalEstimation can publish PointCloud, etc //pub_output_ = pnh_->template advertise ("output", max_queue_size_); // ---[ Mandatory parameters - if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_)) - { - NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); + 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 > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &FeatureFromNormals::config_callback, this, _1, _2); + srv_->setCallback(f); // Create the objects here - if (approximate_sync_) - sync_input_normals_surface_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_normals_surface_indices_e_ = boost::make_shared > > (max_queue_size_); + if (approximate_sync_) { + sync_input_normals_surface_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_normals_surface_indices_e_ = boost::make_shared>>(max_queue_size_); + } // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages - if (use_indices_ || use_surface_) - { - if (use_indices_) - { + if (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 (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud, cloud_normals, cloud_surface, vindices); + computePublish(cloud, cloud_normals, cloud_surface, vindices); } - diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp index 728f0d50..dac0f28f 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp @@ -38,42 +38,42 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp index 822817a2..80cdde16 100644 --- a/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp @@ -38,42 +38,42 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp index d8be14ac..7391856b 100644 --- a/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp @@ -38,40 +38,40 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp index 4d62f381..1e393358 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp @@ -38,40 +38,40 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp index c64e85bd..688782ba 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp @@ -38,40 +38,40 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp index 5874eea8..72f94c7f 100644 --- a/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp @@ -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 - diff --git a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp index 01b1c8f2..12f4a7c5 100644 --- a/pcl_ros/src/pcl_ros/filters/features/pfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/pfh.cpp @@ -38,42 +38,42 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp index cb7ce548..0d5fa4e6 100644 --- a/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/principal_curvatures.cpp @@ -38,42 +38,42 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp index 5f77d67a..eb2f58cf 100644 --- a/pcl_ros/src/pcl_ros/filters/features/vfh.cpp +++ b/pcl_ros/src/pcl_ros/filters/features/vfh.cpp @@ -38,42 +38,42 @@ #include #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); diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 2970504a..dfa6eef5 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -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 > >(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>>(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); + } else { + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); } - else - { - sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); - } - } - else + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "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 (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*pnh_, "output", max_queue_size_); // Enable the dynamic reconfigure service - if (!has_service) - { - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2); - srv_->setCallback (f); + if (!has_service) { + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::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 (cloud_transformed); - } - else + cloud_tf = boost::make_shared(cloud_transformed); + } else { cloud_tf = cloud; + } // Need setInputCloud () here because we have to extract x/y/z IndicesPtr vindices; - if (indices) - vindices.reset (new std::vector (indices->indices)); + if (indices) { + vindices.reset(new std::vector(indices->indices)); + } - computePublish (cloud_tf, vindices); + computePublish(cloud_tf, vindices); } - diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index 693442e8..f03ec564 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -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 > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::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); diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 2065f415..f1b8525c 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -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 (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*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 > > (max_queue_size_); - sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); - sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); - } - else - { - sync_input_indices_model_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); - sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); + if (approximate_sync_) { + sync_input_indices_model_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_a_->registerCallback( + bind( + &ProjectInliers::input_indices_model_callback, + this, _1, _2, _3)); + } else { + sync_input_indices_model_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); + sync_input_indices_model_e_->registerCallback( + bind( + &ProjectInliers::input_indices_model_callback, + this, _1, _2, _3)); } } ////////////////////////////////////////////////////////////////////////////////////////////// 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 (indices->indices)); + if (indices) { + vindices.reset(new std::vector(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); diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index 34e0240a..2125a669 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -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 > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&RadiusOutlierRemoval::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::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); diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index 282419c6..00803e82 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -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 > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&StatisticalOutlierRemoval::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>( + nh); + dynamic_reconfigure::Server::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); diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index be00eaa5..37c3adbd 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -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 > (nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::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); diff --git a/pcl_ros/src/pcl_ros/io/bag_io.cpp b/pcl_ros/src/pcl_ros/io/bag_io.cpp index 12930fc2..e0a62e86 100644 --- a/pcl_ros/src/pcl_ros/io/bag_io.cpp +++ b/pcl_ros/src/pcl_ros/io/bag_io.cpp @@ -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 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 ("output", max_queue_size); + ros::Publisher pub_output = pnh_->advertise("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 (output); - output_->header.stamp = ros::Time::now (); + output_ = boost::make_shared(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); diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index 13027d51..00cea9d9 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -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 (*pnh_, "output", maximum_queue_size_); + pub_output_ = advertise(*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 - > (maximum_queue_size_)); - else - ts_e_.reset (new message_filters::Synchronizer - > (maximum_queue_size_)); + if (approximate_sync_) { + ts_a_.reset( + new message_filters::Synchronizer + >(maximum_queue_size_)); + } else { + ts_e_.reset( + new message_filters::Synchronizer + >(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 ()); - 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()); + 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 (in1); + if (output_frame_ != in1.header.frame_id) { + pcl_ros::transformPointCloud(output_frame_, in1, *in1_t, tf_); + } else { + in1_t = boost::make_shared(in1); + } - if (output_frame_ != in2.header.frame_id) - pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_); - else - in2_t = boost::make_shared (in2); + if (output_frame_ != in2.header.frame_id) { + pcl_ros::transformPointCloud(output_frame_, in2, *in2_t, tf_); + } else { + in2_t = boost::make_shared(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 (*out1)); + pub_output_.publish(boost::make_shared(*out1)); } typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer; -PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer,nodelet::Nodelet); - +PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer, nodelet::Nodelet); diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index 35618ea1..99524dd0 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -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 (*pnh_, "output", maximum_queue_size_); + pnh_->getParam("max_queue_size", maximum_queue_size_); + pnh_->getParam("maximum_seconds", maximum_seconds_); + pub_output_ = advertise(*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 &clouds = queue_[cloud->header.stamp]; + std::vector & 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 (cloud_out)); - queue_.erase (cloud->header.stamp); + pub_output_.publish(boost::make_shared(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); diff --git a/pcl_ros/src/pcl_ros/io/io.cpp b/pcl_ros/src/pcl_ros/io/io.cpp index 48fa54d6..7cfde645 100644 --- a/pcl_ros/src/pcl_ros/io/io.cpp +++ b/pcl_ros/src/pcl_ros/io/io.cpp @@ -42,7 +42,8 @@ #include #include -typedef nodelet::NodeletMUX > NodeletMUX; +typedef nodelet::NodeletMUX> NodeletMUX; //typedef nodelet::NodeletDEMUX > NodeletDEMUX; typedef nodelet::NodeletDEMUX NodeletDEMUX; @@ -51,7 +52,6 @@ typedef nodelet::NodeletDEMUX 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); - diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index df1fc24a..487d8cb4 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -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 ("output", max_queue_size_, true); + ros::Publisher pub_output = pnh_->advertise("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 (); - output_new = boost::make_shared (); + output_ = boost::make_shared(); + output_new = boost::make_shared(); // 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 (cloud->header.stamp.toSec ()) + ".pcd"; - else + if (file_name_.empty()) { + fname = boost::lexical_cast(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(cloud.get())), pcl_cloud); - impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); + pcl_conversions::moveToPCL(*(const_cast(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); diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index b953497b..43170cea 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -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 (*pnh_, "output", max_queue_size_); - else - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + if (publish_indices_) { + pub_output_ = advertise(*pnh_, "output", max_queue_size_); + } else { + pub_output_ = advertise(*pnh_, "output", max_queue_size_); + } // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::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 > > (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>>(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>>(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 > > (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 ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "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 (indices->indices)); + if (indices) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setIndices (indices_ptr); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setIndices(indices_ptr); std::vector 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) - diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index b05770b7..3dac6ab7 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -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 > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &ExtractPolygonalPrismData::config_callback, this, _1, _2); + srv_->setCallback(f); // Advertise the output topics - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*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 > > (max_queue_size_); - else - sync_input_hull_indices_e_ = boost::make_shared > > (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>>(max_queue_size_); + } else { + sync_input_hull_indices_e_ = boost::make_shared>>(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 (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + indices_ptr.reset(new std::vector(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) - diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index cfb46d9b..1defe32d 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -45,230 +45,243 @@ using pcl_conversions::fromPCL; ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::onInit () +pcl_ros::SACSegmentation::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Advertise the output topics - pub_indices_ = advertise (*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise (*pnh_, "model", max_queue_size_); + pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); + pub_model_ = advertise(*pnh_, "model", max_queue_size_); // ---[ Mandatory parameters int model_type; - if (!pnh_->getParam ("model_type", model_type)) - { - NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!"); + if (!pnh_->getParam("model_type", model_type)) { + NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!"); return; } double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam ("distance_threshold", threshold)) - { - NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); + if (!pnh_->getParam("distance_threshold", threshold)) { + NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); return; } // ---[ Optional parameters int method_type = 0; - pnh_->getParam ("method_type", method_type); + pnh_->getParam("method_type", method_type); XmlRpc::XmlRpcValue axis_param; - pnh_->getParam ("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero (); + pnh_->getParam("axis", axis_param); + Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - switch (axis_param.getType ()) - { + switch (axis_param.getType()) { case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size () != 3) { - NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ()); - return; - } - for (int i = 0; i < 3; ++i) - { - if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble) - { - NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ()); + if (axis_param.size() != 3) { + NODELET_ERROR( + "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", + getName().c_str(), axis_param.size()); return; } - double value = axis_param[i]; axis[i] = value; + for (int i = 0; i < 3; ++i) { + if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { + NODELET_ERROR( + "[%s::onInit] Need floating point values for 'axis' parameter.", + getName().c_str()); + return; + } + double value = axis_param[i]; axis[i] = value; + } + break; } - break; - } default: - { - break; - } + { + break; + } } // Initialize the random number generator - srand (time (0)); + srand(time(0)); // Enable the dynamic reconfigure service - srv_ = boost::make_shared >(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentation::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &SACSegmentation::config_callback, this, _1, _2); + srv_->setCallback(f); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName ().c_str (), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - method_type : %d\n" + " - model_threshold : %f\n" + " - axis : [%f, %f, %f]\n", + getName().c_str(), model_type, method_type, threshold, + axis[0], axis[1], axis[2]); // Set given parameters here - impl_.setModelType (model_type); - impl_.setMethodType (method_type); - impl_.setAxis (axis); + impl_.setModelType(model_type); + impl_.setMethodType(method_type); + impl_.setAxis(axis); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::subscribe () +pcl_ros::SACSegmentation::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_); // when "use_indices" is set to true, and "latched_indices" is set to true, - // we'll subscribe and get a separate callback for PointIndices that will - // save the indices internally, and a PointCloud + PointIndices callback - // will take care of meshing the new PointClouds with the old saved indices. - if (latched_indices_) - { + // we'll subscribe and get a separate callback for PointIndices that will + // save the indices internally, and a PointCloud + PointIndices callback + // will take care of meshing the new PointClouds with the old saved indices. + if (latched_indices_) { // Subscribe to a callback that saves the indices - sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1)); + sub_indices_filter_.registerCallback(bind(&SACSegmentation::indices_callback, this, _1)); // Subscribe to a callback that sets the header of the saved indices to the cloud header - sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1)); + sub_input_filter_.registerCallback(bind(&SACSegmentation::input_callback, this, _1)); // Synchronize the two topics. No need for an approximate synchronizer here, as we'll // match the timestamps exactly - sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_); - sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_); + sync_input_indices_e_->registerCallback( + bind( + &SACSegmentation::input_indices_callback, this, + _1, _2)); } // "latched_indices" not set, proceed with regular pairs - else - { - if (approximate_sync_) - { - sync_input_indices_a_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); - } - else - { - sync_input_indices_e_ = boost::make_shared > > (max_queue_size_); - sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2)); + else { + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback( + bind( + &SACSegmentation::input_indices_callback, this, + _1, _2)); + } else { + sync_input_indices_e_ = boost::make_shared>>(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback( + bind( + &SACSegmentation::input_indices_callback, this, + _1, _2)); } } - } - else + } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ())); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::unsubscribe () -{ - if (use_indices_) - { - sub_input_filter_.unsubscribe (); - sub_indices_filter_.unsubscribe (); + sub_input_ = + pnh_->subscribe( + "input", max_queue_size_, + bind(&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr())); } - else - sub_input_.shutdown (); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t level) +pcl_ros::SACSegmentation::unsubscribe() { - boost::mutex::scoped_lock lock (mutex_); + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + } else { + sub_input_.shutdown(); + } +} - if (impl_.getDistanceThreshold () != config.distance_threshold) - { +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32_t level) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (impl_.getDistanceThreshold() != config.distance_threshold) { //sac_->setDistanceThreshold (threshold_); - done in initSAC - impl_.setDistanceThreshold (config.distance_threshold); - NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold); + impl_.setDistanceThreshold(config.distance_threshold); + NODELET_DEBUG( + "[%s::config_callback] Setting new distance to model threshold to: %f.", + getName().c_str(), config.distance_threshold); } // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle () != config.eps_angle) - { - impl_.setEpsAngle (config.eps_angle); - NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI); - } - - // Number of inliers - if (min_inliers_ != config.min_inliers) - { - min_inliers_ = config.min_inliers; - NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_); + if (impl_.getEpsAngle() != config.eps_angle) { + impl_.setEpsAngle(config.eps_angle); + NODELET_DEBUG( + "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", + getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); } - if (impl_.getMaxIterations () != config.max_iterations) - { + // Number of inliers + if (min_inliers_ != config.min_inliers) { + min_inliers_ = config.min_inliers; + NODELET_DEBUG( + "[%s::config_callback] Setting new minimum number of inliers to: %d.", + getName().c_str(), min_inliers_); + } + + if (impl_.getMaxIterations() != config.max_iterations) { //sac_->setMaxIterations (max_iterations_); - done in initSAC - impl_.setMaxIterations (config.max_iterations); - NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations); + impl_.setMaxIterations(config.max_iterations); + NODELET_DEBUG( + "[%s::config_callback] Setting new maximum number of iterations to: %d.", + getName().c_str(), config.max_iterations); } - if (impl_.getProbability () != config.probability) - { + if (impl_.getProbability() != config.probability) { //sac_->setProbability (probability_); - done in initSAC - impl_.setProbability (config.probability); - NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability); + impl_.setProbability(config.probability); + NODELET_DEBUG( + "[%s::config_callback] Setting new probability to: %f.", + getName().c_str(), config.probability); } - if (impl_.getOptimizeCoefficients () != config.optimize_coefficients) - { - impl_.setOptimizeCoefficients (config.optimize_coefficients); - NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false"); + if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { + impl_.setOptimizeCoefficients(config.optimize_coefficients); + NODELET_DEBUG( + "[%s::config_callback] Setting coefficient optimization to: %s.", + getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); } double radius_min, radius_max; - impl_.getRadiusLimits (radius_min, radius_max); - if (radius_min != config.radius_min) - { + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_min != config.radius_min) { radius_min = config.radius_min; - NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG("[config_callback] Setting minimum allowable model radius to: %f.", radius_min); + impl_.setRadiusLimits(radius_min, radius_max); } - if (radius_max != config.radius_max) - { + if (radius_max != config.radius_max) { radius_max = config.radius_max; - NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG("[config_callback] Setting maximum allowable model radius to: %f.", radius_max); + impl_.setRadiusLimits(radius_min, radius_max); } - 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_WARN ("input_frame TF not implemented yet!"); + NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); + NODELET_WARN("input_frame TF not implemented yet!"); } - 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_WARN ("output_frame TF not implemented yet!"); + NODELET_DEBUG( + "[config_callback] Setting the output TF frame to: %s.", + tf_output_frame_.c_str()); + NODELET_WARN("output_frame TF not implemented yet!"); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud, - const PointIndicesConstPtr &indices) +pcl_ros::SACSegmentation::input_indices_callback( + const PointCloudConstPtr & cloud, + const PointIndicesConstPtr & indices) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); pcl_msgs::PointIndices inliers; pcl_msgs::ModelCoefficients model; @@ -276,33 +289,39 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou inliers.header = model.header = fromPCL(cloud->header); // If cloud is given, check if it's valid - if (!isValid (cloud)) - { - NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); - pub_indices_.publish (inliers); - pub_model_.publish (model); + if (!isValid(cloud)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); + pub_indices_.publish(inliers); + pub_model_.publish(model); 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 ()); - pub_indices_.publish (inliers); - pub_model_.publish (model); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); + pub_indices_.publish(inliers); + pub_model_.publish(model); return; } /// DEBUG - if (indices && !indices->header.frame_id.empty ()) - 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 (), 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 ("[%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 (), pnh_->resolveName ("input").c_str ()); + if (indices && !indices->header.frame_id.empty()) { + 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(), 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( + "[%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(), pnh_->resolveName( + "input").c_str()); + } /// // Check whether the user has given a different input TF frame @@ -319,22 +338,23 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou cloud_tf.reset (new PointCloud (cloud_transformed)); } else*/ - cloud_tf = cloud; + cloud_tf = cloud; IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty ()) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setInputCloud (pcl_ptr(cloud_tf)); - impl_.setIndices (indices_ptr); + impl_.setInputCloud(pcl_ptr(cloud_tf)); + 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; pcl::ModelCoefficients pcl_model; pcl_conversions::moveToPCL(inliers, pcl_inliers); pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment (pcl_inliers, pcl_model); + impl_.segment(pcl_inliers, pcl_model); pcl_conversions::moveFromPCL(pcl_inliers, inliers); pcl_conversions::moveFromPCL(pcl_model, model); } @@ -342,355 +362,402 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou // Probably need to transform the model of the plane here // Check if we have enough inliers, clear inliers + model if not - if ((int)inliers.indices.size () <= min_inliers_) - { - inliers.indices.clear (); - model.values.clear (); + if ((int)inliers.indices.size() <= min_inliers_) { + inliers.indices.clear(); + model.values.clear(); } // Publish - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); - NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", - getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), - model.values.size (), pnh_->resolveName ("model").c_str ()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); + NODELET_DEBUG( + "[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), + model.values.size(), pnh_->resolveName("model").c_str()); - if (inliers.indices.empty ()) - NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); + if (inliers.indices.empty()) { + NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::onInit () +pcl_ros::SACSegmentationFromNormals::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); // Enable the dynamic reconfigure service - srv_ = boost::make_shared > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &SACSegmentationFromNormals::config_callback, this, _1, _2); + srv_->setCallback(f); // Advertise the output topics - pub_indices_ = advertise (*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise (*pnh_, "model", max_queue_size_); + pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); + pub_model_ = advertise(*pnh_, "model", max_queue_size_); // ---[ Mandatory parameters 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; } double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam ("distance_threshold", threshold)) - { - NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ()); + if (!pnh_->getParam("distance_threshold", threshold)) { + NODELET_ERROR( + "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", + getName().c_str()); return; } // ---[ Optional parameters int method_type = 0; - pnh_->getParam ("method_type", method_type); + pnh_->getParam("method_type", method_type); XmlRpc::XmlRpcValue axis_param; - pnh_->getParam ("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero (); + pnh_->getParam("axis", axis_param); + Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - switch (axis_param.getType ()) - { + switch (axis_param.getType()) { case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size () != 3) { - NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ()); - return; - } - for (int i = 0; i < 3; ++i) - { - if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble) - { - NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ()); + if (axis_param.size() != 3) { + NODELET_ERROR( + "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", + getName().c_str(), axis_param.size()); return; } - double value = axis_param[i]; axis[i] = value; + for (int i = 0; i < 3; ++i) { + if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { + NODELET_ERROR( + "[%s::onInit] Need floating point values for 'axis' parameter.", + getName().c_str()); + return; + } + double value = axis_param[i]; axis[i] = value; + } + break; } - break; - } default: - { - break; - } + { + break; + } } // Initialize the random number generator - srand (time (0)); + srand(time(0)); - NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName ().c_str (), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); + NODELET_DEBUG( + "[%s::onInit] Nodelet successfully created with the following parameters:\n" + " - model_type : %d\n" + " - method_type : %d\n" + " - model_threshold : %f\n" + " - axis : [%f, %f, %f]\n", + getName().c_str(), model_type, method_type, threshold, + axis[0], axis[1], axis[2]); // Set given parameters here - impl_.setModelType (model_type); - impl_.setMethodType (method_type); - impl_.setAxis (axis); + impl_.setModelType(model_type); + impl_.setMethodType(method_type); + impl_.setAxis(axis); - onInitPostProcess (); + onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::subscribe () +pcl_ros::SACSegmentationFromNormals::subscribe() { // Subscribe to the input and normals using filters - 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_); // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) - sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); - if (approximate_sync_) - sync_input_normals_indices_a_ = boost::make_shared > > (max_queue_size_); - else - sync_input_normals_indices_e_ = boost::make_shared > > (max_queue_size_); + if (approximate_sync_) { + sync_input_normals_indices_a_ = boost::make_shared>>(max_queue_size_); + } else { + sync_input_normals_indices_e_ = boost::make_shared>>(max_queue_size_); + } // If we're supposed to look for PointIndices (indices) - if (use_indices_) - { + if (use_indices_) { // Subscribe to the input using a filter - sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); + sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_); - } - else - { + if (approximate_sync_) { + sync_input_normals_indices_a_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_indices_filter_); + } else { + sync_input_normals_indices_e_->connectInput( + sub_input_filter_, sub_normals_filter_, + sub_indices_filter_); + } + } else { // Create a different callback for copying over the timestamp to fake indices - sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1)); + sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1)); - if (approximate_sync_) - sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); - else - sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_); + if (approximate_sync_) { + sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); + } else { + sync_input_normals_indices_e_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); + } } - if (approximate_sync_) - sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); - else - sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3)); + if (approximate_sync_) { + sync_input_normals_indices_a_->registerCallback( + bind( + &SACSegmentationFromNormals:: + input_normals_indices_callback, this, _1, _2, _3)); + } else { + sync_input_normals_indices_e_->registerCallback( + bind( + &SACSegmentationFromNormals:: + input_normals_indices_callback, this, _1, _2, _3)); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::unsubscribe () +pcl_ros::SACSegmentationFromNormals::unsubscribe() { - sub_input_filter_.unsubscribe (); - sub_normals_filter_.unsubscribe (); + sub_input_filter_.unsubscribe(); + sub_normals_filter_.unsubscribe(); - sub_axis_.shutdown (); + sub_axis_.shutdown(); - if (use_indices_) - sub_indices_filter_.unsubscribe (); + if (use_indices_) { + sub_indices_filter_.unsubscribe(); + } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model) +pcl_ros::SACSegmentationFromNormals::axis_callback( + const pcl_msgs::ModelCoefficientsConstPtr & model) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (model->values.size () < 3) - { - NODELET_ERROR ("[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", getName ().c_str (), model->values.size (), pnh_->resolveName ("axis").c_str ()); + if (model->values.size() < 3) { + NODELET_ERROR( + "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", + getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str()); return; } - NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]); + NODELET_DEBUG( + "[%s::axis_callback] Received axis direction: %f %f %f", + getName().c_str(), model->values[0], model->values[1], model->values[2]); - Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]); - impl_.setAxis (axis); + Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]); + impl_.setAxis(axis); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level) +pcl_ros::SACSegmentationFromNormals::config_callback( + SACSegmentationFromNormalsConfig & config, + uint32_t level) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (impl_.getDistanceThreshold () != config.distance_threshold) - { - impl_.setDistanceThreshold (config.distance_threshold); - NODELET_DEBUG ("[%s::config_callback] Setting distance to model 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 distance to model threshold to: %f.", + getName().c_str(), config.distance_threshold); } // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle () != config.eps_angle) - { - impl_.setEpsAngle (config.eps_angle); - NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI); + if (impl_.getEpsAngle() != config.eps_angle) { + impl_.setEpsAngle(config.eps_angle); + NODELET_DEBUG( + "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", + getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); } - if (impl_.getMaxIterations () != config.max_iterations) - { - impl_.setMaxIterations (config.max_iterations); - NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations); + if (impl_.getMaxIterations() != config.max_iterations) { + impl_.setMaxIterations(config.max_iterations); + NODELET_DEBUG( + "[%s::config_callback] Setting new maximum number of iterations to: %d.", + getName().c_str(), config.max_iterations); } - + // Number of inliers - if (min_inliers_ != config.min_inliers) - { + if (min_inliers_ != config.min_inliers) { min_inliers_ = config.min_inliers; - NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_); + NODELET_DEBUG( + "[%s::config_callback] Setting new minimum number of inliers to: %d.", + getName().c_str(), min_inliers_); } - if (impl_.getProbability () != config.probability) - { - impl_.setProbability (config.probability); - NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability); + if (impl_.getProbability() != config.probability) { + impl_.setProbability(config.probability); + NODELET_DEBUG( + "[%s::config_callback] Setting new probability to: %f.", + getName().c_str(), config.probability); } - if (impl_.getOptimizeCoefficients () != config.optimize_coefficients) - { - impl_.setOptimizeCoefficients (config.optimize_coefficients); - NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false"); + if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { + impl_.setOptimizeCoefficients(config.optimize_coefficients); + NODELET_DEBUG( + "[%s::config_callback] Setting coefficient optimization to: %s.", + getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); } - if (impl_.getNormalDistanceWeight () != config.normal_distance_weight) - { - impl_.setNormalDistanceWeight (config.normal_distance_weight); - NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight); + if (impl_.getNormalDistanceWeight() != config.normal_distance_weight) { + impl_.setNormalDistanceWeight(config.normal_distance_weight); + NODELET_DEBUG( + "[%s::config_callback] Setting new distance weight to: %f.", + getName().c_str(), config.normal_distance_weight); } double radius_min, radius_max; - impl_.getRadiusLimits (radius_min, radius_max); - if (radius_min != config.radius_min) - { + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_min != config.radius_min) { radius_min = config.radius_min; - NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG( + "[%s::config_callback] Setting minimum allowable model radius to: %f.", + getName().c_str(), radius_min); + impl_.setRadiusLimits(radius_min, radius_max); } - if (radius_max != config.radius_max) - { + if (radius_max != config.radius_max) { radius_max = config.radius_max; - NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max); - impl_.setRadiusLimits (radius_min, radius_max); + NODELET_DEBUG( + "[%s::config_callback] Setting maximum allowable model radius to: %f.", + getName().c_str(), radius_max); + impl_.setRadiusLimits(radius_min, radius_max); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( - const PointCloudConstPtr &cloud, - const PointCloudNConstPtr &cloud_normals, - const PointIndicesConstPtr &indices - ) +pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( + const PointCloudConstPtr & cloud, + const PointCloudNConstPtr & cloud_normals, + const PointIndicesConstPtr & indices +) { - boost::mutex::scoped_lock lock (mutex_); + boost::mutex::scoped_lock lock(mutex_); PointIndices inliers; ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied inliers.header = model.header = fromPCL(cloud->header); - if (impl_.getModelType () < 0) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (impl_.getModelType() < 0) { + NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } - if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals")) + NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } // If indices are given, check if they are valid - if (indices && !isValid (indices)) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (indices && !isValid(indices)) { + NODELET_ERROR("[%s::input_normals_indices_callback] Invalid indices!", getName().c_str()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } /// DEBUG - if (indices && !indices->header.frame_id.empty ()) - NODELET_DEBUG ("[%s::input_normals_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_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 (indices && !indices->header.frame_id.empty()) { + NODELET_DEBUG( + "[%s::input_normals_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_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()); + } /// // Extra checks for safety - int cloud_nr_points = cloud->width * cloud->height; + int cloud_nr_points = cloud->width * cloud->height; int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; - if (cloud_nr_points != cloud_normals_nr_points) - { - NODELET_ERROR ("[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", getName ().c_str (), cloud_nr_points, cloud_normals_nr_points); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + if (cloud_nr_points != cloud_normals_nr_points) { + NODELET_ERROR( + "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", + getName().c_str(), cloud_nr_points, cloud_normals_nr_points); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); return; } - impl_.setInputCloud (pcl_ptr(cloud)); - impl_.setInputNormals (pcl_ptr(cloud_normals)); + impl_.setInputCloud(pcl_ptr(cloud)); + impl_.setInputNormals(pcl_ptr(cloud_normals)); IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty ()) - indices_ptr.reset (new std::vector (indices->indices)); + if (indices && !indices->header.frame_id.empty()) { + indices_ptr.reset(new std::vector(indices->indices)); + } - impl_.setIndices (indices_ptr); + impl_.setIndices(indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) { + if (!cloud->points.empty()) { pcl::PointIndices pcl_inliers; pcl::ModelCoefficients pcl_model; pcl_conversions::moveToPCL(inliers, pcl_inliers); pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment (pcl_inliers, pcl_model); + impl_.segment(pcl_inliers, pcl_model); pcl_conversions::moveFromPCL(pcl_inliers, inliers); pcl_conversions::moveFromPCL(pcl_model, model); } // Check if we have enough inliers, clear inliers + model if not - if ((int)inliers.indices.size () <= min_inliers_) - { - inliers.indices.clear (); - model.values.clear (); + if ((int)inliers.indices.size() <= min_inliers_) { + inliers.indices.clear(); + model.values.clear(); } // Publish - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); - NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", - getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (), - model.values.size (), pnh_->resolveName ("model").c_str ()); - if (inliers.indices.empty ()) - NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ()); + pub_indices_.publish(boost::make_shared(inliers)); + pub_model_.publish(boost::make_shared(model)); + NODELET_DEBUG( + "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", + getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), + model.values.size(), pnh_->resolveName("model").c_str()); + if (inliers.indices.empty()) { + NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); + } } typedef pcl_ros::SACSegmentation SACSegmentation; typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) - diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index 79d01d12..93bc8f8b 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -41,103 +41,119 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SegmentDifferences::onInit () +pcl_ros::SegmentDifferences::onInit() { // Call the super onInit () - PCLNodelet::onInit (); + PCLNodelet::onInit(); - pub_output_ = advertise (*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*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 > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind( + &SegmentDifferences::config_callback, this, _1, _2); + srv_->setCallback(f); - if (approximate_sync_) - { - sync_input_target_a_ = boost::make_shared > > (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 > > (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>>(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>>(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) - diff --git a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp index 8c261926..09fa772a 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp @@ -40,5 +40,3 @@ //#include "extract_polygonal_prism_data.cpp" //#include "sac_segmentation.cpp" //#include "segment_differences.cpp" - - diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 22cc2b98..8a80217e 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -42,20 +42,21 @@ ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ConvexHull2D::onInit () +pcl_ros::ConvexHull2D::onInit() { - PCLNodelet::onInit (); + PCLNodelet::onInit(); - pub_output_ = advertise (*pnh_, "output", max_queue_size_); - pub_plane_ = advertise (*pnh_, "output_polygon", max_queue_size_); + pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_plane_ = advertise(*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 > >(max_queue_size_); + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(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 > >(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>>(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 ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ())); + sub_input_ = + pnh_->subscribe( + "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 (indices->indices)); + if (indices) { + indices_ptr.reset(new std::vector(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 (poly)); + pub_plane_.publish(boost::make_shared(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) - diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 26b6f3f6..a7daf9e3 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -40,192 +40,211 @@ #include ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::MovingLeastSquares::onInit () +pcl_ros::MovingLeastSquares::onInit() { - PCLNodelet::onInit (); - + PCLNodelet::onInit(); + //ros::NodeHandle private_nh = getMTPrivateNodeHandle (); - pub_output_ = advertise (*pnh_, "output", max_queue_size_); - pub_normals_ = advertise (*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(*pnh_, "output", max_queue_size_); + pub_normals_ = advertise(*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 > (*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 ); - srv_->setCallback (f); + srv_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::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 > >(max_queue_size_); + if (approximate_sync_) { + sync_input_indices_a_ = boost::make_shared>>(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 > >(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>>(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 ("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( + "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 (indices->indices)); + if (indices) { + indices_ptr.reset(new std::vector(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_); } } diff --git a/pcl_ros/src/pcl_ros/surface/surface.cpp b/pcl_ros/src/pcl_ros/surface/surface.cpp index 13976f90..a73b242d 100644 --- a/pcl_ros/src/pcl_ros/surface/surface.cpp +++ b/pcl_ros/src/pcl_ros/surface/surface.cpp @@ -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" - - diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp index e3f4930e..ba92d794 100644 --- a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -58,7 +58,7 @@ typedef pcl::PointCloud 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_listener; - boost::scoped_ptr > tf_filter; + boost::scoped_ptr> 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(*tf_listener, - "map", 5, threaded_nh, - ros::Duration(0.000001))); + new tf::MessageFilter( + *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); diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index 76a93c4e..4eb23c3d 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -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]<<","< "<::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 (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloudWithNormals (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloudWithNormals (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloudWithNormals( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); -template void pcl_ros::transformPointCloud (const pcl::PointCloud &, pcl::PointCloud &, const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const tf::Transform &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const pcl::PointCloud &, pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const pcl::PointCloud &, pcl::PointCloud &, + const tf::TransformListener &); ////////////////////////////////////////////////////////////////////////////////////////////// -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); -template bool pcl_ros::transformPointCloud (const std::string &, const ros::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, const tf::TransformListener &); - +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, + const std::string &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, + const std::string &, + pcl::PointCloud &, + const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); +template bool pcl_ros::transformPointCloud( + const std::string &, + const ros::Time &, + const pcl::PointCloud &, const std::string &, + pcl::PointCloud &, const tf::TransformListener &); diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index d48f2c58..7dd0dec9 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -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] << " []" << 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] << + " []" << 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 (); - if (tf != NULL) - { - tf_broadcaster.sendTransform (tf->transforms); - ros::spinOnce (); - r.sleep (); - } - else - { + tf::tfMessage::ConstPtr tf = view_it->instantiate(); + if (tf != NULL) { + tf_broadcaster.sendTransform(tf->transforms); + ros::spinOnce(); + r.sleep(); + } else { // Get the PointCloud2 message - PointCloudConstPtr cloud = view_it->instantiate (); - if (cloud == NULL) - { + PointCloudConstPtr cloud = view_it->instantiate(); + 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; } /* ]--- */ diff --git a/pcl_ros/tools/convert_pcd_to_image.cpp b/pcl_ros/tools/convert_pcd_to_image.cpp index ff8e28af..e14249f0 100644 --- a/pcl_ros/tools/convert_pcd_to_image.cpp +++ b/pcl_ros/tools/convert_pcd_to_image.cpp @@ -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 ("output", 1); + ros::Publisher image_pub = nh.advertise("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; } /* ]--- */ diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp index a7a4e1c1..3db4d31f 100644 --- a/pcl_ros/tools/convert_pointcloud_to_image.cpp +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -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 (image_topic_, 30); + sub_ = nh_.subscribe( + cloud_topic_, 30, + &PointCloudToImage::cloud_cb, this); + image_pub_ = nh_.advertise(image_topic_, 30); //print some info about the node - std::string r_ct = nh_.resolveName (cloud_topic_); - std::string r_it = nh_.resolveName (image_topic_); - ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct ); - ROS_INFO_STREAM("Publishing image on topic " << r_it ); + 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; } diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index 02ef1bbc..27b5628e 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -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 pub_; + pcl_ros::Publisher 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(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(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] << " [publishing_interval (in seconds)]" << std::endl; - return (-1); + if (argc < 2) { + std::cerr << "Syntax is: " << argv[0] << " [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; } /* ]--- */ diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 2044b880..e2fb436f 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -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& cloud) - { - if ((cloud->width * cloud->height) == 0) + //////////////////////////////////////////////////////////////////////////////// + // Callback + void + cloud_cb(const boost::shared_ptr & 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 (); - q = transform.rotation ().cast (); } - 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(); + q = transform.rotation().cast(); } - //////////////////////////////////////////////////////////////////////////////// - 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; } /* ]--- */