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

Make ament_cpplint pass (#298)

Browse Source 
Collection of hand-made changes to make ament_cpplint pass consisting of:
- whitespace between comments
- line length
- header ordering
- include guard formats
- remove a couple `using namespace std;`
- using c++ casts instead of c-style casts

Signed-off-by: Sean Kelly <sean@seankelly.dev>
This commit is contained in:
Sean Kelly 2020-08-09 19:47:21 -04:00 committed by GitHub
parent 9689971aee
commit 420f5b032b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
67 changed files with 831 additions and 593 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_BOUNDARY_H_ #ifndef PCL_ROS__FEATURES__BOUNDARY_HPP_
#define PCL_ROS_BOUNDARY_H_ #define PCL_ROS__FEATURES__BOUNDARY_HPP_
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
@ -82,6 +82,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_BOUNDARY_H_ #endif // PCL_ROS__FEATURES__BOUNDARY_HPP_

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

@ -35,23 +35,24 @@
* *
*/ */
#ifndef PCL_ROS_FEATURE_H_ #ifndef PCL_ROS__FEATURES__FEATURE_HPP_
#define PCL_ROS_FEATURE_H_ #define PCL_ROS__FEATURES__FEATURE_HPP_
// PCL includes // PCL includes
#include <pcl/features/feature.h> #include <pcl/features/feature.h>
#include <pcl_msgs/PointIndices.h> #include <pcl_msgs/PointIndices.h>
#include "pcl_ros/pcl_nodelet.hpp"
#include <message_filters/pass_through.h> #include <message_filters/pass_through.h>
// Dynamic reconfigure // Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include "pcl_ros/FeatureConfig.hpp"
// PCL conversions // PCL conversions
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/FeatureConfig.hpp"
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
@ -89,7 +90,9 @@ protected:
/** \brief A pointer to the vector of point indices to use. */ /** \brief A pointer to the vector of point indices to use. */
// IndicesConstPtr indices_; // IndicesConstPtr indices_;
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ /** \brief An input point cloud describing the surface that is to be used
* for nearest neighbors estimation.
*/
// PointCloudInConstPtr surface_; // PointCloudInConstPtr surface_;
/** \brief A pointer to the spatial search object. */ /** \brief A pointer to the spatial search object. */
@ -108,7 +111,9 @@ protected:
/** \brief The input PointCloud subscriber. */ /** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_; ros::Subscriber sub_input_;
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */ /** \brief Set to true if the nodelet needs to listen for incoming point
* clouds representing the search surface.
*/
bool use_surface_; bool use_surface_;
/** \brief Parameter for the spatial locator tree. By convention, the values represent: /** \brief Parameter for the spatial locator tree. By convention, the values represent:
@ -257,6 +262,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FEATURE_H_ #endif // PCL_ROS__FEATURES__FEATURE_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FPFH_H_ #ifndef PCL_ROS__FEATURES__FPFH_HPP_
#define PCL_ROS_FPFH_H_ #define PCL_ROS__FEATURES__FPFH_HPP_
#include <pcl/features/fpfh.h> #include <pcl/features/fpfh.h>
#include "pcl_ros/features/pfh.hpp" #include "pcl_ros/features/pfh.hpp"
@ -94,6 +94,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_FPFH_H_ #endif // PCL_ROS__FEATURES__FPFH_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FPFH_OMP_H_ #ifndef PCL_ROS__FEATURES__FPFH_OMP_HPP_
#define PCL_ROS_FPFH_OMP_H_ #define PCL_ROS__FEATURES__FPFH_OMP_HPP_
#include <pcl/features/fpfh_omp.h> #include <pcl/features/fpfh_omp.h>
#include "pcl_ros/features/fpfh.hpp" #include "pcl_ros/features/fpfh.hpp"
@ -91,6 +91,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FPFH_OMP_H_ #endif // PCL_ROS__FEATURES__FPFH_OMP_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ #ifndef PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
#define PCL_ROS_MOMENT_INVARIANTS_H_ #define PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
#include <pcl/features/moment_invariants.h> #include <pcl/features/moment_invariants.h>
#include "pcl_ros/features/feature.hpp" #include "pcl_ros/features/feature.hpp"
@ -77,6 +77,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_ #endif // PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_NORMAL_3D_H_ #ifndef PCL_ROS__FEATURES__NORMAL_3D_HPP_
#define PCL_ROS_NORMAL_3D_H_ #define PCL_ROS__FEATURES__NORMAL_3D_HPP_
#include <pcl/features/normal_3d.h> #include <pcl/features/normal_3d.h>
#include "pcl_ros/features/feature.hpp" #include "pcl_ros/features/feature.hpp"
@ -81,6 +81,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_NORMAL_3D_H_ #endif // PCL_ROS__FEATURES__NORMAL_3D_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_NORMAL_3D_OMP_H_ #ifndef PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
#define PCL_ROS_NORMAL_3D_OMP_H_ #define PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
#include <pcl/features/normal_3d_omp.h> #include <pcl/features/normal_3d_omp.h>
#include "pcl_ros/features/normal_3d.hpp" #include "pcl_ros/features/normal_3d.hpp"
@ -75,6 +75,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_ #endif // PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_NORMAL_3D_TBB_H_ #ifndef PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
#define PCL_ROS_NORMAL_3D_TBB_H_ #define PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
// #include "pcl_ros/pcl_ros_config.hpp" // #include "pcl_ros/pcl_ros_config.hpp"
// #if defined(HAVE_TBB) // #if defined(HAVE_TBB)
@ -78,8 +78,8 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
// #endif // HAVE_TBB // #endif // HAVE_TBB
#endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_ #endif // PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_PFH_H_ #ifndef PCL_ROS__FEATURES__PFH_HPP_
#define PCL_ROS_PFH_H_ #define PCL_ROS__FEATURES__PFH_HPP_
#include <pcl/features/pfh.h> #include <pcl/features/pfh.h>
#include "pcl_ros/features/feature.hpp" #include "pcl_ros/features/feature.hpp"
@ -94,6 +94,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_PFH_H_ #endif // PCL_ROS__FEATURES__PFH_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ #ifndef PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
#define PCL_ROS_PRINCIPAL_CURVATURES_H_ #define PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
#include <pcl/features/principal_curvatures.h> #include <pcl/features/principal_curvatures.h>
@ -80,6 +80,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_ #endif // PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_

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

@ -34,8 +34,8 @@
* *
*/ */
#ifndef PCL_ROS_SHOT_H_ #ifndef PCL_ROS__FEATURES__SHOT_HPP_
#define PCL_ROS_SHOT_H_ #define PCL_ROS__FEATURES__SHOT_HPP_
#include <pcl/features/shot.h> #include <pcl/features/shot.h>
#include "pcl_ros/features/pfh.hpp" #include "pcl_ros/features/pfh.hpp"
@ -74,6 +74,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_SHOT_H_ #endif // PCL_ROS__FEATURES__SHOT_HPP_

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

@ -34,8 +34,8 @@
* *
*/ */
#ifndef PCL_ROS_SHOT_OMP_H_ #ifndef PCL_ROS__FEATURES__SHOT_OMP_HPP_
#define PCL_ROS_SHOT_OMP_H_ #define PCL_ROS__FEATURES__SHOT_OMP_HPP_
#include <pcl/features/shot_omp.h> #include <pcl/features/shot_omp.h>
#include "pcl_ros/features/shot.hpp" #include "pcl_ros/features/shot.hpp"
@ -73,6 +73,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_SHOT_OMP_H_ #endif // PCL_ROS__FEATURES__SHOT_OMP_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FEATURES_VFH_H_ #ifndef PCL_ROS__FEATURES__VFH_HPP_
#define PCL_ROS_FEATURES_VFH_H_ #define PCL_ROS__FEATURES__VFH_HPP_
#include <pcl/features/vfh.h> #include <pcl/features/vfh.h>
#include "pcl_ros/features/fpfh.hpp" #include "pcl_ros/features/fpfh.hpp"
@ -79,6 +79,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FEATURES_VFH_H_ #endif // PCL_ROS__FEATURES__VFH_HPP_

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

@ -36,8 +36,8 @@
* *
*/ */
#ifndef PCL_ROS_FILTERS_CROPBOX_H_ #ifndef PCL_ROS__FILTERS__CROP_BOX_HPP_
#define PCL_ROS_FILTERS_CROPBOX_H_ #define PCL_ROS__FILTERS__CROP_BOX_HPP_
// PCL includes // PCL includes
#include <pcl/filters/crop_box.h> #include <pcl/filters/crop_box.h>
@ -58,7 +58,7 @@ class CropBox : public Filter
{ {
protected: protected:
/** \brief Pointer to a dynamic reconfigure service. */ /** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>> srv_; // TODO boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>> srv_; // TODO(xxx)
/** \brief Call the actual filter. /** \brief Call the actual filter.
* \param input the input point cloud dataset * \param input the input point cloud dataset
@ -101,6 +101,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_ #endif // PCL_ROS__FILTERS__CROP_BOX_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ #ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
#define PCL_ROS_FILTERS_EXTRACTINDICES_H_ #define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
// PCL includes // PCL includes
#include <pcl/filters/extract_indices.h> #include <pcl/filters/extract_indices.h>
@ -94,6 +94,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_ #endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_

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

@ -35,23 +35,21 @@
* *
*/ */
#ifndef PCL_ROS_FILTER_H_ #ifndef PCL_ROS__FILTERS__FILTER_HPP_
#define PCL_ROS_FILTER_H_ #define PCL_ROS__FILTERS__FILTER_HPP_
// PCL includes
#include <pcl/filters/filter.h> #include <pcl/filters/filter.h>
#include "pcl_ros/pcl_nodelet.hpp"
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/FilterConfig.hpp" #include "pcl_ros/FilterConfig.hpp"
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters /** \brief @b Filter represents the base filter class. Some generic 3D operations that are
* are defined here as static methods. * applicable to all filters are defined here as static methods.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class Filter : public PCLNodelet class Filter : public PCLNodelet
@ -79,16 +77,22 @@ protected:
/** \brief The maximum allowed filter value a point will be considered from. */ /** \brief The maximum allowed filter value a point will be considered from. */
double filter_limit_max_; 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. */ /** \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_; bool filter_limit_negative_;
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ /** \brief The input TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_input_frame_; std::string tf_input_frame_;
/** \brief The original data input TF frame. */ /** \brief The original data input TF frame. */
std::string tf_input_orig_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. */ /** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_output_frame_; std::string tf_output_frame_;
/** \brief Internal mutex. */ /** \brief Internal mutex. */
@ -157,6 +161,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FILTER_H_ #endif // PCL_ROS__FILTERS__FILTER_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ #ifndef PCL_ROS__FILTERS__PASSTHROUGH_HPP_
#define PCL_ROS_FILTERS_PASSTHROUGH_H_ #define PCL_ROS__FILTERS__PASSTHROUGH_HPP_
// PCL includes // PCL includes
#include <pcl/filters/passthrough.h> #include <pcl/filters/passthrough.h>
@ -95,6 +95,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_ #endif // PCL_ROS__FILTERS__PASSTHROUGH_HPP_

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

@ -35,14 +35,13 @@
* *
*/ */
#ifndef PCL_ROS_FILTERS_PROJECT_INLIERS_H_ #ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
#define PCL_ROS_FILTERS_PROJECT_INLIERS_H_ #define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
// PCL includes
#include <pcl/filters/project_inliers.h> #include <pcl/filters/project_inliers.h>
#include <message_filters/subscriber.h>
#include "pcl_ros/filters/filter.hpp" #include "pcl_ros/filters/filter.hpp"
#include <message_filters/subscriber.h>
namespace pcl_ros namespace pcl_ros
{ {
@ -115,6 +114,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_ #endif // PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ #ifndef PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
#define PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_ #define PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
// PCL includes // PCL includes
#include <pcl/filters/radius_outlier_removal.h> #include <pcl/filters/radius_outlier_removal.h>
@ -96,6 +96,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_ #endif // PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ #ifndef PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
#define PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_ #define PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
// PCL includes // PCL includes
#include <pcl/filters/statistical_outlier_removal.h> #include <pcl/filters/statistical_outlier_removal.h>
@ -103,6 +103,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_ #endif // PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_ROS_FILTERS_VOXEL_H_ #ifndef PCL_ROS__FILTERS__VOXEL_GRID_HPP_
#define PCL_ROS_FILTERS_VOXEL_H_ #define PCL_ROS__FILTERS__VOXEL_GRID_HPP_
// PCL includes // PCL includes
#include <pcl/filters/voxel_grid.h> #include <pcl/filters/voxel_grid.h>
@ -86,6 +86,6 @@ protected:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_ #endif // PCL_ROS__FILTERS__VOXEL_GRID_HPP_

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

@ -34,10 +34,11 @@
* *
*/ */
#ifndef pcl_ros_IMPL_TRANSFORMS_H_ #ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_
#define pcl_ros_IMPL_TRANSFORMS_H_ #define PCL_ROS__IMPL__TRANSFORMS_HPP_
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <string>
#include "pcl_ros/transforms.hpp" #include "pcl_ros/transforms.hpp"
using pcl_conversions::fromPCL; using pcl_conversions::fromPCL;
@ -217,6 +218,5 @@ transformPointCloudWithNormals(
cloud_out.header = toPCL(header); cloud_out.header = toPCL(header);
return true; return true;
} }
} // namespace pcl_ros } // namespace pcl_ros
#endif #endif // PCL_ROS__IMPL__TRANSFORMS_HPP_

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

@ -35,13 +35,14 @@
* *
*/ */
#ifndef PCL_ROS_IO_BAG_IO_H_ #ifndef PCL_ROS__IO__BAG_IO_HPP_
#define PCL_ROS_IO_BAG_IO_H_ #define PCL_ROS__IO__BAG_IO_HPP_
#include <pcl_ros/pcl_nodelet.hpp> #include <pcl_ros/pcl_nodelet.hpp>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h> #include <rosbag/bag.h>
#include <rosbag/view.h> #include <rosbag/view.h>
#include <string>
namespace pcl_ros namespace pcl_ros
{ {
@ -125,7 +126,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} // namespace pcl_ros
} #endif // PCL_ROS__IO__BAG_IO_HPP_
#endif //#ifndef PCL_ROS_IO_BAG_IO_H_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_IO_CONCATENATE_DATA_H_ #ifndef PCL_ROS__IO__CONCATENATE_DATA_HPP_
#define PCL_IO_CONCATENATE_DATA_H_ #define PCL_ROS__IO__CONCATENATE_DATA_HPP_
// ROS includes // ROS includes
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
@ -46,6 +46,8 @@
#include <message_filters/pass_through.h> #include <message_filters/pass_through.h>
#include <message_filters/sync_policies/exact_time.h> #include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
#include <string>
#include <vector>
namespace pcl_ros namespace pcl_ros
{ {
@ -71,7 +73,7 @@ public:
/** \brief Empty constructor. /** \brief Empty constructor.
* \param queue_size the maximum queue size * \param queue_size the maximum queue size
*/ */
PointCloudConcatenateDataSynchronizer(int queue_size) explicit PointCloudConcatenateDataSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), approximate_sync_(false) {} : maximum_queue_size_(queue_size), approximate_sync_(false) {}
/** \brief Empty destructor. */ /** \brief Empty destructor. */
@ -88,7 +90,9 @@ private:
/** \brief The maximum number of messages that we can store in the queue. */ /** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_; int maximum_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ /** \brief True if we use an approximate time synchronizer
* versus an exact one (false by default).
*/
bool approximate_sync_; bool approximate_sync_;
/** \brief A vector of message filters. */ /** \brief A vector of message filters. */
@ -137,6 +141,6 @@ private:
void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out); void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out);
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_IO_CONCATENATE_H_ #endif // PCL_ROS__IO__CONCATENATE_DATA_HPP_

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

@ -35,8 +35,8 @@
* *
*/ */
#ifndef PCL_IO_CONCATENATE_FIELDS_H_ #ifndef PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
#define PCL_IO_CONCATENATE_FIELDS_H_ #define PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
// ROS includes // ROS includes
#include <nodelet_topic_tools/nodelet_lazy.h> #include <nodelet_topic_tools/nodelet_lazy.h>
@ -47,6 +47,9 @@
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <map>
#include <vector>
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
@ -68,7 +71,7 @@ public:
/** \brief Empty constructor. /** \brief Empty constructor.
* \param queue_size the maximum queue size * \param queue_size the maximum queue size
*/ */
PointCloudConcatenateFieldsSynchronizer(int queue_size) explicit PointCloudConcatenateFieldsSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), maximum_seconds_(0) {} : maximum_queue_size_(queue_size), maximum_seconds_(0) {}
/** \brief Empty destructor. */ /** \brief Empty destructor. */
@ -98,6 +101,6 @@ private:
/** \brief A queue for messages. */ /** \brief A queue for messages. */
std::map<ros::Time, std::vector<PointCloudConstPtr>> queue_; std::map<ros::Time, std::vector<PointCloudConstPtr>> queue_;
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_IO_CONCATENATE_H_ #endif // PCL_ROS__IO__CONCATENATE_FIELDS_HPP_

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

@ -35,10 +35,11 @@
* *
*/ */
#ifndef PCL_ROS_IO_PCD_IO_H_ #ifndef PCL_ROS__IO__PCD_IO_HPP_
#define PCL_ROS_IO_PCD_IO_H_ #define PCL_ROS__IO__PCD_IO_HPP_
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp" #include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros namespace pcl_ros
@ -131,6 +132,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_IO_PCD_IO_H_ #endif // PCL_ROS__IO__PCD_IO_HPP_

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

@ -41,8 +41,8 @@
**/ **/
#ifndef PCL_NODELET_H_ #ifndef PCL_ROS__PCL_NODELET_HPP_
#define PCL_NODELET_H_ #define PCL_ROS__PCL_NODELET_HPP_
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
// PCL includes // PCL includes
@ -51,7 +51,6 @@
#include <pcl/pcl_base.h> #include <pcl/pcl_base.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/point_cloud.hpp"
// ROS Nodelet includes // ROS Nodelet includes
#include <nodelet_topic_tools/nodelet_lazy.h> #include <nodelet_topic_tools/nodelet_lazy.h>
#include <message_filters/subscriber.h> #include <message_filters/subscriber.h>
@ -62,6 +61,11 @@
// Include TF // Include TF
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
// STL
#include <string>
#include "pcl_ros/point_cloud.hpp"
using pcl_conversions::fromPCL; using pcl_conversions::fromPCL;
namespace pcl_ros namespace pcl_ros
@ -69,7 +73,9 @@ namespace pcl_ros
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */ /** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should
* inherit from this class.
**/
class PCLNodelet : public nodelet_topic_tools::NodeletLazy class PCLNodelet : public nodelet_topic_tools::NodeletLazy
{ {
public: public:
@ -128,7 +134,9 @@ protected:
/** \brief The maximum queue size (default: 3). */ /** \brief The maximum queue size (default: 3). */
int max_queue_size_; int max_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */ /** \brief True if we use an approximate time synchronizer
* versus an exact one (false by default).
**/
bool approximate_sync_; bool approximate_sync_;
/** \brief TF listener object. */ /** \brief TF listener object. */
@ -143,7 +151,8 @@ protected:
{ {
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
NODELET_WARN( NODELET_WARN(
"[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!", "[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) "
"with stamp %f, and frame %s on topic %s received!",
getName().c_str(), getName().c_str(),
cloud->data.size(), cloud->width, cloud->height, cloud->point_step, cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -163,7 +172,8 @@ protected:
{ {
if (cloud->width * cloud->height != cloud->points.size()) { if (cloud->width * cloud->height != cloud->points.size()) {
NODELET_WARN( NODELET_WARN(
"[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!", "[%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, getName().c_str(), cloud->points.size(), cloud->width, cloud->height,
fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
pnh_->resolveName(topic_name).c_str()); pnh_->resolveName(topic_name).c_str());
@ -182,7 +192,10 @@ protected:
{ {
/*if (indices->indices.empty ()) /*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 ()); 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);
}*/ }*/
return true; return true;
@ -197,13 +210,18 @@ protected:
{ {
/*if (model->values.empty ()) /*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 ()); 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 (false);
}*/ }*/
return true; return true;
} }
/** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */ /** \brief Lazy transport subscribe/unsubscribe routine.
* It is optional for backward compatibility.
**/
virtual void subscribe() {} virtual void subscribe() {}
virtual void unsubscribe() {} virtual void unsubscribe() {}
@ -237,6 +255,6 @@ protected:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_NODELET_H_ #endif // PCL_ROS__PCL_NODELET_HPP_

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

@ -1,15 +1,71 @@
#ifndef pcl_ROS_POINT_CLOUD_H_ /*
#define pcl_ROS_POINT_CLOUD_H_ * Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef PCL_ROS__POINT_CLOUD_HPP__
#define PCL_ROS__POINT_CLOUD_HPP__
// test if testing machinery can be implemented
#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr)
#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1
#else
#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0
#endif
#include <ros/ros.h> #include <ros/ros.h>
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/point_traits.h> #include <pcl/point_traits.h>
#include <pcl/for_each_type.h> #include <pcl/for_each_type.h>
#include <pcl/conversions.h> #include <pcl/conversions.h>
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#include <pcl/pcl_config.h>
#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
#include <pcl/memory.h>
#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
#include <pcl/make_shared.h>
#endif
#endif
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <boost/mpl/size.hpp> #include <boost/mpl/size.hpp>
#include <boost/ref.hpp> #include <boost/ref.hpp>
#include <string>
#include <utility>
#include <vector>
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#include <type_traits>
#include <memory>
#endif
namespace pcl namespace pcl
{ {
@ -18,7 +74,7 @@ namespace detail
template<typename Stream, typename PointT> template<typename Stream, typename PointT>
struct FieldStreamer struct FieldStreamer
{ {
FieldStreamer(Stream & stream) explicit FieldStreamer(Stream & stream)
: stream_(stream) {} : stream_(stream) {}
template<typename U> template<typename U>
@ -59,7 +115,7 @@ struct FieldsLength
std::uint32_t length; std::uint32_t length;
}; };
} // namespace pcl::detail } // namespace detail
} // namespace pcl } // namespace pcl
namespace ros namespace ros
@ -161,7 +217,7 @@ struct FrameId<pcl::PointCloud<T>>
static std::string value(const pcl::PointCloud<T> & m) {return m.header.frame_id;} static std::string value(const pcl::PointCloud<T> & m) {return m.header.frame_id;}
}; };
} // namespace ros::message_traits } // namespace message_traits
namespace serialization namespace serialization
{ {
@ -297,31 +353,12 @@ struct Serializer<pcl::PointCloud<T>>
return length; return length;
} }
}; };
} // namespace ros::serialization } // namespace serialization
/// @todo Printer specialization in message_operations /// @todo Printer specialization in message_operations
} // namespace ros } // namespace ros
// test if testing machinery can be implemented
#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr)
#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1
#else
#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0
#endif
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#include <type_traits> // for std::is_same
#include <memory> // for std::shared_ptr
#include <pcl/pcl_config.h>
#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
#include <pcl/memory.h>
#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
#include <pcl/make_shared.h>
#endif
#endif
namespace pcl namespace pcl
{ {
namespace detail namespace detail
@ -341,7 +378,7 @@ struct Holder
{ {
SharedPointer p; SharedPointer p;
Holder(const SharedPointer & p) explicit Holder(const SharedPointer & p)
: p(p) {} : p(p) {}
Holder(const Holder & other) Holder(const Holder & other)
: p(other.p) {} : p(other.p) {}
@ -373,7 +410,7 @@ inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
} }
} }
#endif #endif
} // namespace pcl::detail } // namespace detail
// add functions to convert to smart pointer used by ROS // add functions to convert to smart pointer used by ROS
template<class T> template<class T>
@ -422,4 +459,4 @@ inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
#endif #endif
} // namespace pcl } // namespace pcl
#endif #endif // PCL_ROS__POINT_CLOUD_HPP__

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

@ -42,8 +42,8 @@
**/ **/
#ifndef PCL_ROS_PUBLISHER_H_ #ifndef PCL_ROS__PUBLISHER_HPP_
#define PCL_ROS_PUBLISHER_H_ #define PCL_ROS__PUBLISHER_HPP_
#include <ros/ros.h> #include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
@ -51,6 +51,8 @@
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <string>
namespace pcl_ros namespace pcl_ros
{ {
class BasePublisher class BasePublisher
@ -82,7 +84,7 @@ public:
operator void *() const operator void *() const
{ {
return (pub_) ? (void *)1 : (void *)0; return (pub_) ? reinterpret_cast<void *>(1) : reinterpret_cast<void *>(0);
} }
protected: protected:
@ -145,5 +147,5 @@ public:
// pub_.publish (point_cloud); // pub_.publish (point_cloud);
} }
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_PUBLISHER_H_ #endif // PCL_ROS__PUBLISHER_HPP_

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

@ -35,14 +35,13 @@
* *
*/ */
#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ #ifndef PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_
#define PCL_ROS_EXTRACT_CLUSTERS_H_ #define PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_
#include <pcl/segmentation/extract_clusters.h>
#include "pcl_ros/pcl_nodelet.hpp"
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include <pcl/segmentation/extract_clusters.h>
#include <limits>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/EuclideanClusterExtractionConfig.hpp" #include "pcl_ros/EuclideanClusterExtractionConfig.hpp"
namespace pcl_ros namespace pcl_ros
@ -109,6 +108,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ #endif // PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_

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

@ -35,20 +35,16 @@
* *
*/ */
#ifndef PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ #ifndef PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_
#define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_ #define PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_
#include "pcl_ros/pcl_nodelet.hpp"
#include <message_filters/sync_policies/exact_time.h> #include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/pass_through.h> #include <message_filters/pass_through.h>
// PCL includes
#include <pcl/segmentation/extract_polygonal_prism_data.h> #include <pcl/segmentation/extract_polygonal_prism_data.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp" #include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp"
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros namespace pcl_ros
{ {
@ -129,6 +125,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ #endif // PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_

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

@ -35,17 +35,14 @@
* *
*/ */
#ifndef PCL_ROS_SAC_SEGMENTATION_H_ #ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
#define PCL_ROS_SAC_SEGMENTATION_H_ #define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
#include "pcl_ros/pcl_nodelet.hpp"
#include <message_filters/pass_through.h> #include <message_filters/pass_through.h>
// PCL includes
#include <pcl/segmentation/sac_segmentation.h> #include <pcl/segmentation/sac_segmentation.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/SACSegmentationConfig.hpp" #include "pcl_ros/SACSegmentationConfig.hpp"
#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" #include "pcl_ros/SACSegmentationFromNormalsConfig.hpp"
@ -54,8 +51,9 @@ namespace pcl_ros
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in /** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus
* the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation. * methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose
* SAC-based segmentation.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class SACSegmentation : public PCLNodelet class SACSegmentation : public PCLNodelet
@ -69,7 +67,8 @@ public:
SACSegmentation() SACSegmentation()
: min_inliers_(0) {} : min_inliers_(0) {}
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. /** \brief Set the 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 * \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;} inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
@ -102,13 +101,17 @@ protected:
/** \brief Pointer to a dynamic reconfigure service. */ /** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig>> srv_; boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig>> srv_;
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ /** \brief The input TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_input_frame_; std::string tf_input_frame_;
/** \brief The original data input TF frame. */ /** \brief The original data input TF frame. */
std::string tf_input_orig_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. */ /** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_output_frame_; std::string tf_output_frame_;
/** \brief Null passthrough filter, used for pushing empty elements in the /** \brief Null passthrough filter, used for pushing empty elements in the
@ -180,8 +183,8 @@ public:
}; };
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for
* models that require the use of surface normals for estimation. * Sample Consensus methods and models that require the use of surface normals for estimation.
*/ */
class SACSegmentationFromNormals : public SACSegmentation class SACSegmentationFromNormals : public SACSegmentation
{ {
@ -194,7 +197,8 @@ class SACSegmentationFromNormals : public SACSegmentation
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr; typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
public: public:
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different. /** \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 * \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;} inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
@ -237,11 +241,15 @@ protected:
* synchronizer */ * synchronizer */
message_filters::PassThrough<PointIndices> nf_; message_filters::PassThrough<PointIndices> nf_;
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ /** \brief The input TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_input_frame_; std::string tf_input_frame_;
/** \brief The original data input TF frame. */ /** \brief The original data input TF frame. */
std::string tf_input_orig_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. */ /** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_output_frame_; std::string tf_output_frame_;
/** \brief Nodelet initialization routine. */ /** \brief Nodelet initialization routine. */
@ -288,6 +296,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ #endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_

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

@ -35,15 +35,13 @@
* *
*/ */
#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ #ifndef PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
#define PCL_ROS_SEGMENT_DIFFERENCES_H_ #define PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
#include <pcl/segmentation/segment_differences.h> #include <pcl/segmentation/segment_differences.h>
#include "pcl_ros/pcl_nodelet.hpp"
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include "pcl_ros/SegmentDifferencesConfig.hpp" #include "pcl_ros/SegmentDifferencesConfig.hpp"
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros namespace pcl_ros
@ -108,6 +106,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ #endif // PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_

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

@ -35,16 +35,12 @@
* *
*/ */
#ifndef PCL_ROS_CONVEX_HULL_2D_H_ #ifndef PCL_ROS__SURFACE__CONVEX_HULL_HPP_
#define PCL_ROS_CONVEX_HULL_2D_H_ #define PCL_ROS__SURFACE__CONVEX_HULL_HPP_
#include "pcl_ros/pcl_nodelet.hpp"
// PCL includes
#include <pcl/surface/convex_hull.h> #include <pcl/surface/convex_hull.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros namespace pcl_ros
{ {
@ -94,6 +90,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ #endif // PCL_ROS__SURFACE__CONVEX_HULL_HPP_

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

@ -35,25 +35,21 @@
* *
*/ */
#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ #ifndef PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
#define PCL_ROS_MOVING_LEAST_SQUARES_H_ #define PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
#include "pcl_ros/pcl_nodelet.hpp"
// PCL includes
#include <pcl/surface/mls.h> #include <pcl/surface/mls.h>
#include <pcl/kdtree/kdtree.h> // for KdTree
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include "pcl_ros/MLSConfig.h" #include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/MLSConfig.hpp"
namespace pcl_ros namespace pcl_ros
{ {
namespace sync_policies = message_filters::sync_policies; namespace sync_policies = message_filters::sync_policies;
/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation. /** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
* The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters. * The type of the output is the same as the input, it only smooths the XYZ coordinates according
* to the parameters.
* Normals are estimated at each point as well and published on a separate topic. * Normals are estimated at each point as well and published on a separate topic.
* \author Radu Bogdan Rusu, Zoltan-Csaba Marton * \author Radu Bogdan Rusu, Zoltan-Csaba Marton
*/ */
@ -71,7 +67,9 @@ class MovingLeastSquares : public PCLNodelet
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr; typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
protected: protected:
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ /** \brief An input point cloud describing the surface that is to be used for
* nearest neighbors estimation.
*/
PointCloudInConstPtr surface_; PointCloudInConstPtr surface_;
/** \brief A pointer to the spatial search object. */ /** \brief A pointer to the spatial search object. */
@ -89,7 +87,9 @@ protected:
/** \brief The order of the polynomial to be fit. */ /** \brief The order of the polynomial to be fit. */
int polynomial_order_; int polynomial_order_;
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */ /** \brief How 'flat' should the neighbor weighting gaussian be
* the smaller, the more local the fit).
*/
double gaussian_parameter_; double gaussian_parameter_;
// ROS nodelet attributes // ROS nodelet attributes
@ -147,6 +147,6 @@ private:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} } // namespace pcl_ros
#endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ #endif // PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_

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

@ -34,13 +34,14 @@
* *
*/ */
#ifndef pcl_ROS_TRANSFORMS_H_ #ifndef PCL_ROS__TRANSFORMS_HPP_
#define pcl_ROS_TRANSFORMS_H_ #define PCL_ROS__TRANSFORMS_HPP_
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <pcl/common/transforms.h> #include <pcl/common/transforms.h>
#include <tf/transform_datatypes.h> #include <tf/transform_datatypes.h>
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
#include <string>
namespace pcl_ros namespace pcl_ros
{ {
@ -176,6 +177,6 @@ transformPointCloud(
*/ */
void void
transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat); transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat);
} } // namespace pcl_ros
#endif // PCL_ROS_TRANSFORMS_H_ #endif // PCL_ROS__TRANSFORMS_HPP_

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

@ -48,8 +48,9 @@
// #include "principal_curvatures.cpp" // #include "principal_curvatures.cpp"
// #include "vfh.cpp" // #include "vfh.cpp"
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include "pcl_ros/features/feature.hpp"
#include <message_filters/null_types.h> #include <message_filters/null_types.h>
#include <vector>
#include "pcl_ros/features/feature.hpp"
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
@ -62,13 +63,15 @@ pcl_ros::Feature::onInit()
childInit(*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR( NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str()); getName().c_str());
return; return;
} }
@ -108,11 +111,14 @@ pcl_ros::Feature::subscribe()
if (use_indices_ || use_surface_) { if (use_indices_ || use_surface_) {
// Create the objects here // Create the objects here
if (approximate_sync_) { if (approximate_sync_) {
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, sync_input_surface_indices_a_ =
PointCloudIn, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} else { } else {
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, sync_input_surface_indices_e_ =
PointCloudIn, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} }
// Subscribe to the input using a filter // Subscribe to the input using a filter
@ -121,7 +127,8 @@ pcl_ros::Feature::subscribe()
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // If surface is enabled, subscribe to the surface,
// connect the input-indices-surface trio and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput( sync_input_surface_indices_a_->connectInput(
@ -251,9 +258,12 @@ pcl_ros::Feature::input_surface_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[input_surface_indices_callback]\n" "[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
@ -266,8 +276,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[input_surface_indices_callback]\n" "[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "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->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
@ -279,8 +291,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else if (indices) { } else if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[input_surface_indices_callback]\n" "[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and frame "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "%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->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
@ -288,16 +302,18 @@ pcl_ros::Feature::input_surface_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( 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( "[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( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str()); "input").c_str());
} }
/// ///
if ((int)(cloud->width * cloud->height) < k_) { if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR( NODELET_ERROR(
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger "
"than the PointCloud size (%d)!", k_,
(int)(cloud->width * cloud->height)); (int)(cloud->width * cloud->height));
emptyPublish(cloud); emptyPublish(cloud);
return; return;
@ -327,13 +343,15 @@ pcl_ros::FeatureFromNormals::onInit()
childInit(*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR( NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str()); getName().c_str());
return; return;
} }
@ -373,10 +391,13 @@ pcl_ros::FeatureFromNormals::subscribe()
// Create the objects here // Create the objects here
if (approximate_sync_) { if (approximate_sync_) {
sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, sync_input_normals_surface_indices_a_ =
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloudIn, PointCloudN,
PointCloudIn, PointIndices>>>(max_queue_size_);
} else { } else {
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, sync_input_normals_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_); PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
} }
@ -386,7 +407,8 @@ pcl_ros::FeatureFromNormals::subscribe()
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput( sync_input_normals_surface_indices_a_->connectInput(
@ -519,10 +541,14 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "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), "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -540,9 +566,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "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.", " - 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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -559,9 +588,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else if (indices) { } else if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -575,8 +607,10 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -588,9 +622,10 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} }
/// ///
if ((int)(cloud->width * cloud->height) < k_) { if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR( NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", "[%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)); getName().c_str(), k_, (int)(cloud->width * cloud->height));
emptyPublish(cloud); emptyPublish(cloud);
return; return;

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

@ -36,8 +36,8 @@
* *
*/ */
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/filters/crop_box.hpp" #include "pcl_ros/filters/crop_box.hpp"
#include <pluginlib/class_list_macros.h>
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
@ -102,7 +102,8 @@ pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t leve
impl_.setNegative(config.negative); impl_.setNegative(config.negative);
} }
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL // The following parameters are updated automatically for all PCL_ROS Nodelet Filters
// as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame) { if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG( NODELET_DEBUG(

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

@ -48,8 +48,9 @@
// #include "principal_curvatures.cpp" // #include "principal_curvatures.cpp"
// #include "vfh.cpp" // #include "vfh.cpp"
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include "pcl_ros/features/feature.hpp"
#include <message_filters/null_types.h> #include <message_filters/null_types.h>
#include <vector>
#include "pcl_ros/features/feature.hpp"
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
void void
@ -62,13 +63,15 @@ pcl_ros::Feature::onInit()
childInit(*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR( NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str()); getName().c_str());
return; return;
} }
@ -92,11 +95,15 @@ pcl_ros::Feature::onInit()
if (use_indices_ || use_surface_) { if (use_indices_ || use_surface_) {
// Create the objects here // Create the objects here
if (approximate_sync_) { if (approximate_sync_) {
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, sync_input_surface_indices_a_ =
PointCloudIn, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} else { } else {
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, sync_input_surface_indices_e_ =
PointCloudIn, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} }
// Subscribe to the input using a filter // Subscribe to the input using a filter
@ -105,7 +112,8 @@ pcl_ros::Feature::onInit()
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput( sync_input_surface_indices_a_->connectInput(
@ -225,9 +233,12 @@ pcl_ros::Feature::input_surface_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[input_surface_indices_callback]\n" "[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, "
"and frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
@ -240,8 +251,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[input_surface_indices_callback]\n" "[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "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->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
@ -253,8 +266,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else if (indices) { } else if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[input_surface_indices_callback]\n" "[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "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->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
@ -262,16 +277,18 @@ pcl_ros::Feature::input_surface_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, "[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( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str()); "input").c_str());
} }
/// ///
if ((int)(cloud->width * cloud->height) < k_) { if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR( NODELET_ERROR(
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger "
"than the PointCloud size (%d)!", k_,
(int)(cloud->width * cloud->height)); (int)(cloud->width * cloud->height));
emptyPublish(cloud); emptyPublish(cloud);
return; return;
@ -301,13 +318,15 @@ pcl_ros::FeatureFromNormals::onInit()
childInit(*pnh_); childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher // Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_); // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters // ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) { if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR( NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str()); getName().c_str());
return; return;
} }
@ -331,11 +350,15 @@ pcl_ros::FeatureFromNormals::onInit()
// Create the objects here // Create the objects here
if (approximate_sync_) { if (approximate_sync_) {
sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, sync_input_normals_surface_indices_a_ =
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
} else { } else {
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, sync_input_normals_surface_indices_e_ =
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<
PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
} }
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
@ -344,7 +367,8 @@ pcl_ros::FeatureFromNormals::onInit()
// If indices are enabled, subscribe to the indices // If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_); sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput( sync_input_normals_surface_indices_a_->connectInput(
@ -468,10 +492,14 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "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), "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -489,9 +517,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "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.", " - 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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -508,38 +539,44 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else if (indices) { } else if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList( cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), *cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName( cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
"normals").c_str(), pnh_->resolveName("normals").c_str(),
indices->indices.size(), indices->header.stamp.toSec(), indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n" "[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(), "input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList( cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(), *cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName( cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
"normals").c_str()); pnh_->resolveName("normals").c_str());
} }
/// ///
if ((int)(cloud->width * cloud->height) < k_) { if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR( NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", "[%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)); getName().c_str(), k_, (int)(cloud->width * cloud->height));
emptyPublish(cloud); emptyPublish(cloud);
return; return;

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

@ -35,9 +35,10 @@
* *
*/ */
#include <pcl/io/io.h>
#include "pcl_ros/transforms.hpp"
#include "pcl_ros/filters/filter.hpp" #include "pcl_ros/filters/filter.hpp"
#include <pcl/io/io.h>
#include <vector>
#include "pcl_ros/transforms.hpp"
/*//#include <pcl/filters/pixel_grid.h> /*//#include <pcl/filters/pixel_grid.h>
//#include <pcl/filters/filter_dimension.h> //#include <pcl/filters/filter_dimension.h>
@ -59,7 +60,7 @@
//PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet); //PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet);
*/ */
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices) pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices)
{ {
@ -120,12 +121,14 @@ pcl_ros::Filter::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
pcl_msgs::PointIndices>>>(max_queue_size_); pcl_msgs::PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
} else { } else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
pcl_msgs::PointIndices>>>(max_queue_size_); pcl_msgs::PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
@ -182,7 +185,8 @@ pcl_ros::Filter::onInit()
void void
pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level) pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
{ {
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are
// inexistent in PCL
if (tf_input_frame_ != config.input_frame) { if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG( NODELET_DEBUG(
@ -218,8 +222,10 @@ pcl_ros::Filter::input_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback]\n" "[%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 "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -228,7 +234,8 @@ pcl_ros::Filter::input_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", "[%s::input_indices_callback] PointCloud with %d data points and frame %s on "
"topic %s received.",
getName().c_str(), cloud->width * cloud->height, getName().c_str(), cloud->width * cloud->height,
cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str()); cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str());
} }

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

@ -35,8 +35,8 @@
* *
*/ */
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/filters/passthrough.hpp" #include "pcl_ros/filters/passthrough.hpp"
#include <pluginlib/class_list_macros.h>
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
bool bool
@ -65,7 +65,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
if (filter_min != config.filter_limit_min) { if (filter_min != config.filter_limit_min) {
filter_min = config.filter_limit_min; filter_min = config.filter_limit_min;
NODELET_DEBUG( NODELET_DEBUG(
"[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", "[%s::config_callback] Setting the minimum filtering value a point will be "
"considered from to: %f.",
getName().c_str(), filter_min); getName().c_str(), filter_min);
// Set the filter min-max if different // Set the filter min-max if different
impl_.setFilterLimits(filter_min, filter_max); impl_.setFilterLimits(filter_min, filter_max);
@ -74,7 +75,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
if (filter_max != config.filter_limit_max) { if (filter_max != config.filter_limit_max) {
filter_max = config.filter_limit_max; filter_max = config.filter_limit_max;
NODELET_DEBUG( NODELET_DEBUG(
"[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", "[%s::config_callback] Setting the maximum filtering value a point will be "
"considered from to: %f.",
getName().c_str(), filter_max); getName().c_str(), filter_max);
// Set the filter min-max if different // Set the filter min-max if different
impl_.setFilterLimits(filter_min, filter_max); impl_.setFilterLimits(filter_min, filter_max);
@ -108,7 +110,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
impl_.setFilterLimitsNegative(config.filter_limit_negative); impl_.setFilterLimitsNegative(config.filter_limit_negative);
} }
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL // The following parameters are updated automatically for all PCL_ROS Nodelet Filters
// as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame) { if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG( NODELET_DEBUG(

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

@ -35,9 +35,10 @@
* *
*/ */
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/filters/project_inliers.hpp" #include "pcl_ros/filters/project_inliers.hpp"
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <vector>
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
@ -99,16 +100,20 @@ pcl_ros::ProjectInliers::subscribe()
sub_model_.subscribe(*pnh_, "model", max_queue_size_); sub_model_.subscribe(*pnh_, "model", max_queue_size_);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_indices_model_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, sync_input_indices_model_a_ =
PointIndices, ModelCoefficients>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
sync_input_indices_model_a_->registerCallback( sync_input_indices_model_a_->registerCallback(
bind( bind(
&ProjectInliers::input_indices_model_callback, &ProjectInliers::input_indices_model_callback,
this, _1, _2, _3)); this, _1, _2, _3));
} else { } else {
sync_input_indices_model_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2, sync_input_indices_model_e_ =
PointIndices, ModelCoefficients>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
sync_input_indices_model_e_->registerCallback( sync_input_indices_model_e_->registerCallback(
bind( bind(
@ -148,9 +153,12 @@ pcl_ros::ProjectInliers::input_indices_model_callback(
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_model_callback]\n" "[%s::input_indices_model_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 "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" "frame %s on topic %s received.\n"
" - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.", " - 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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).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->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(),

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

@ -73,7 +73,6 @@ pcl_ros::RadiusOutlierRemoval::config_callback(
"[%s::config_callback] Setting the radius to search neighbors: %f.", "[%s::config_callback] Setting the radius to search neighbors: %f.",
getName().c_str(), config.radius_search); getName().c_str(), config.radius_search);
} }
} }

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

@ -63,7 +63,8 @@ pcl_ros::StatisticalOutlierRemoval::config_callback(
if (impl_.getMeanK() != config.mean_k) { if (impl_.getMeanK() != config.mean_k) {
impl_.setMeanK(config.mean_k); impl_.setMeanK(config.mean_k);
NODELET_DEBUG( NODELET_DEBUG(
"[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", "[%s::config_callback] Setting the number of points (k) to use for mean "
"distance estimation to: %d.",
getName().c_str(), config.mean_k); getName().c_str(), config.mean_k);
} }

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

@ -88,13 +88,15 @@ pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t
if (filter_min != config.filter_limit_min) { if (filter_min != config.filter_limit_min) {
filter_min = config.filter_limit_min; filter_min = config.filter_limit_min;
NODELET_DEBUG( NODELET_DEBUG(
"[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", "[config_callback] Setting the minimum filtering value a point will be considered "
"from to: %f.",
filter_min); filter_min);
} }
if (filter_max != config.filter_limit_max) { if (filter_max != config.filter_limit_max) {
filter_max = config.filter_limit_max; filter_max = config.filter_limit_max;
NODELET_DEBUG( NODELET_DEBUG(
"[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", "[config_callback] Setting the maximum filtering value a point will be considered "
"from to: %f.",
filter_max); filter_max);
} }
impl_.setFilterLimits(filter_min, filter_max); impl_.setFilterLimits(filter_min, filter_max);
@ -113,7 +115,8 @@ pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t
config.filter_field_name.c_str()); config.filter_field_name.c_str());
} }
// ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves,
// we'll remove them and inherit from Filter
if (tf_input_frame_ != config.input_frame) { if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame; tf_input_frame_ = config.input_frame;
NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str());

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

@ -36,6 +36,7 @@
*/ */
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <string>
#include "pcl_ros/io/bag_io.hpp" #include "pcl_ros/io/bag_io.hpp"
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////

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

@ -37,10 +37,11 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include "pcl_ros/transforms.hpp" #include "pcl_ros/transforms.hpp"
#include "pcl_ros/io/concatenate_data.hpp" #include "pcl_ros/io/concatenate_data.hpp"
#include <pcl_conversions/pcl_conversions.h>
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void

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

@ -39,11 +39,12 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/io/concatenate_fields.hpp" #include "pcl_ros/io/concatenate_fields.hpp"
#include <pcl_conversions/pcl_conversions.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit() pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
{ {
@ -66,7 +67,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
onInitPostProcess(); onInitPostProcess();
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe() pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
{ {
@ -75,19 +76,20 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
&PointCloudConcatenateFieldsSynchronizer::input_callback, this); &PointCloudConcatenateFieldsSynchronizer::input_callback, this);
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe() pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe()
{ {
sub_input_.shutdown(); sub_input_.shutdown();
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud) pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud)
{ {
NODELET_DEBUG( NODELET_DEBUG(
"[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "[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->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(),
pnh_->resolveName("input").c_str()); pnh_->resolveName("input").c_str());
@ -98,7 +100,8 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
queue_.size() > 0) queue_.size() > 0)
{ {
NODELET_WARN( NODELET_WARN(
"[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_, "[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message "
"in queue with stamp %f.", maximum_seconds_,
(*queue_.begin()).first.toSec(), (*queue_.begin()).first.toSec(),
fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() )); fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ));
queue_.erase(queue_.begin()); queue_.erase(queue_.begin());
@ -107,7 +110,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
// Push back new data // Push back new data
queue_[cloud->header.stamp].push_back(cloud); queue_[cloud->header.stamp].push_back(cloud);
if ((int)queue_[cloud->header.stamp].size() >= input_messages_) { if (static_cast<int>(queue_[cloud->header.stamp].size()) >= input_messages_) {
// Concatenate together and publish // Concatenate together and publish
std::vector<PointCloudConstPtr> & clouds = queue_[cloud->header.stamp]; std::vector<PointCloudConstPtr> & clouds = queue_[cloud->header.stamp];
PointCloud cloud_out = *clouds[0]; PointCloud cloud_out = *clouds[0];
@ -122,7 +125,8 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) { if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) {
NODELET_ERROR( NODELET_ERROR(
"[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!", "[input_callback] Width/height of pointcloud %zu (%dx%d) differs "
"from the others (%dx%d)!",
i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height); i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
break; break;
} }
@ -162,11 +166,10 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
// Clean the queue to avoid overflowing // Clean the queue to avoid overflowing
if (maximum_queue_size_ > 0) { if (maximum_queue_size_ > 0) {
while ((int)queue_.size() > maximum_queue_size_) { while (static_cast<int>(queue_.size()) > maximum_queue_size_) {
queue_.erase(queue_.begin()); queue_.erase(queue_.begin());
} }
} }
} }
typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer; typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;

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

@ -44,7 +44,8 @@
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2, typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2,
message_filters::Subscriber<sensor_msgs::PointCloud2>> NodeletMUX; message_filters::Subscriber<sensor_msgs::PointCloud2>> NodeletMUX;
//typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2, pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX; // typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2,
// pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX;
typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX; typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
// #include "pcd_io.cpp" // #include "pcd_io.cpp"

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

@ -37,6 +37,7 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <pcl_ros/io/pcd_io.hpp> #include <pcl_ros/io/pcd_io.hpp>
#include <string>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void

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

@ -38,9 +38,10 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl/PointIndices.h> #include <pcl/PointIndices.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/segmentation/extract_clusters.hpp" #include "pcl_ros/segmentation/extract_clusters.hpp"
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL; using pcl_conversions::fromPCL;
using pcl_conversions::moveFromPCL; using pcl_conversions::moveFromPCL;
@ -110,16 +111,19 @@ pcl_ros::EuclideanClusterExtraction::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, sync_input_indices_a_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback( sync_input_indices_a_->registerCallback(
bind( bind(
&EuclideanClusterExtraction:: &EuclideanClusterExtraction::
input_indices_callback, this, _1, _2)); input_indices_callback, this, _1, _2));
} else { } else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, sync_input_indices_e_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback( sync_input_indices_e_->registerCallback(
bind( bind(
@ -206,8 +210,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
std_msgs::Header indices_header = indices->header; std_msgs::Header indices_header = indices->header;
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback]\n" "[%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 "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).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_header.stamp.toSec(), cloud_header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
@ -215,7 +221,8 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", "[%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( getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str()); "input").c_str());
@ -235,10 +242,11 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
if (publish_indices_) { if (publish_indices_) {
for (size_t i = 0; i < clusters.size(); ++i) { for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) { if (static_cast<int>(i) >= max_clusters_) {
break; break;
} }
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. // TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental
// sequence ID number.
pcl_msgs::PointIndices ros_pi; pcl_msgs::PointIndices ros_pi;
moveFromPCL(clusters[i], ros_pi); moveFromPCL(clusters[i], ros_pi);
ros_pi.header.stamp += ros::Duration(i * 0.001); ros_pi.header.stamp += ros::Duration(i * 0.001);
@ -250,7 +258,7 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
clusters.size(), pnh_->resolveName("output").c_str()); clusters.size(), pnh_->resolveName("output").c_str());
} else { } else {
for (size_t i = 0; i < clusters.size(); ++i) { for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) { if (static_cast<int>(i) >= max_clusters_) {
break; break;
} }
PointCloud output; PointCloud output;
@ -258,7 +266,8 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
// PointCloud output_blob; // Convert from the templated output to the PointCloud blob // PointCloud output_blob; // Convert from the templated output to the PointCloud blob
// pcl::toROSMsg (output, output_blob); // pcl::toROSMsg (output, output_blob);
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. // TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental
// sequence ID number.
std_msgs::Header header = fromPCL(output.header); std_msgs::Header header = fromPCL(output.header);
header.stamp += ros::Duration(i * 0.001); header.stamp += ros::Duration(i * 0.001);
toPCL(header, output.header); toPCL(header, output.header);

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

@ -36,11 +36,11 @@
*/ */
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/transforms.hpp" #include "pcl_ros/transforms.hpp"
#include "pcl_ros/segmentation/extract_polygonal_prism_data.hpp" #include "pcl_ros/segmentation/extract_polygonal_prism_data.hpp"
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::moveFromPCL; using pcl_conversions::moveFromPCL;
using pcl_conversions::moveToPCL; using pcl_conversions::moveToPCL;
@ -73,11 +73,13 @@ pcl_ros::ExtractPolygonalPrismData::subscribe()
// Create the objects here // Create the objects here
if (approximate_sync_) { if (approximate_sync_) {
sync_input_hull_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, sync_input_hull_indices_a_ =
PointCloud, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices>>>(max_queue_size_);
} else { } else {
sync_input_hull_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, sync_input_hull_indices_e_ =
PointCloud, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloud, PointIndices>>>(max_queue_size_);
} }
if (use_indices_) { if (use_indices_) {
@ -183,9 +185,12 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_hull_callback]\n" "[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -198,8 +203,10 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_hull_callback]\n" "[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -212,7 +219,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
if (cloud->header.frame_id != hull->header.frame_id) { if (cloud->header.frame_id != hull->header.frame_id) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", "[%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()); getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str());
PointCloud planar_hull; PointCloud planar_hull;
if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) { if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) {
@ -233,7 +241,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
impl_.setInputCloud(pcl_ptr(cloud)); impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices_ptr); impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) // Final check if the data is empty
// (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty()) { if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers; pcl::PointIndices pcl_inliers;
moveToPCL(inliers, pcl_inliers); moveToPCL(inliers, pcl_inliers);

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

@ -36,10 +36,10 @@
*/ */
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include "pcl_ros/segmentation/sac_segmentation.hpp"
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/segmentation/sac_segmentation.hpp"
using pcl_conversions::fromPCL; using pcl_conversions::fromPCL;
@ -80,7 +80,8 @@ pcl_ros::SACSegmentation::onInit()
{ {
if (axis_param.size() != 3) { if (axis_param.size() != 3) {
NODELET_ERROR( NODELET_ERROR(
"[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) "
"than required (3)!",
getName().c_str(), axis_param.size()); getName().c_str(), axis_param.size());
return; return;
} }
@ -149,27 +150,28 @@ pcl_ros::SACSegmentation::subscribe()
// Synchronize the two topics. No need for an approximate synchronizer here, as we'll // Synchronize the two topics. No need for an approximate synchronizer here, as we'll
// match the timestamps exactly // match the timestamps exactly
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, sync_input_indices_e_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_); sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_);
sync_input_indices_e_->registerCallback( sync_input_indices_e_->registerCallback(
bind( bind(
&SACSegmentation::input_indices_callback, this, &SACSegmentation::input_indices_callback, this,
_1, _2)); _1, _2));
} } else { // "latched_indices" not set, proceed with regular <input,indices> pairs
// "latched_indices" not set, proceed with regular <input,indices> pairs
else {
if (approximate_sync_) { if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, sync_input_indices_a_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback( sync_input_indices_a_->registerCallback(
bind( bind(
&SACSegmentation::input_indices_callback, this, &SACSegmentation::input_indices_callback, this,
_1, _2)); _1, _2));
} else { } else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, sync_input_indices_e_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback( sync_input_indices_e_->registerCallback(
bind( bind(
@ -307,8 +309,10 @@ pcl_ros::SACSegmentation::input_indices_callback(
if (indices && !indices->header.frame_id.empty()) { if (indices && !indices->header.frame_id.empty()) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback]\n" "[%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 "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -317,7 +321,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", "[%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( getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str()); "input").c_str());
@ -329,11 +334,13 @@ pcl_ros::SACSegmentation::input_indices_callback(
PointCloudConstPtr cloud_tf; PointCloudConstPtr cloud_tf;
/* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) /* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
{ {
NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.",
// cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
// Save the original frame ID // Save the original frame ID
// Convert the cloud into the different frame // Convert the cloud into the different frame
PointCloud cloud_transformed; PointCloud cloud_transformed;
if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_)) if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud,
cloud_transformed, tf_listener_))
return; return;
cloud_tf.reset (new PointCloud (cloud_transformed)); cloud_tf.reset (new PointCloud (cloud_transformed));
} }
@ -348,7 +355,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
impl_.setInputCloud(pcl_ptr(cloud_tf)); impl_.setInputCloud(pcl_ptr(cloud_tf));
impl_.setIndices(indices_ptr); impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) // Final check if the data is empty
// (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty()) { if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers; pcl::PointIndices pcl_inliers;
pcl::ModelCoefficients pcl_model; pcl::ModelCoefficients pcl_model;
@ -362,7 +370,7 @@ pcl_ros::SACSegmentation::input_indices_callback(
// Probably need to transform the model of the plane here // Probably need to transform the model of the plane here
// Check if we have enough inliers, clear inliers + model if not // Check if we have enough inliers, clear inliers + model if not
if ((int)inliers.indices.size() <= min_inliers_) { if (static_cast<int>(inliers.indices.size()) <= min_inliers_) {
inliers.indices.clear(); inliers.indices.clear();
model.values.clear(); model.values.clear();
} }
@ -371,7 +379,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers)); pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model)); pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", "[%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(), getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(),
model.values.size(), pnh_->resolveName("model").c_str()); model.values.size(), pnh_->resolveName("model").c_str());
@ -426,7 +435,8 @@ pcl_ros::SACSegmentationFromNormals::onInit()
{ {
if (axis_param.size() != 3) { if (axis_param.size() != 3) {
NODELET_ERROR( NODELET_ERROR(
"[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than "
"required (3)!",
getName().c_str(), axis_param.size()); getName().c_str(), axis_param.size());
return; return;
} }
@ -475,15 +485,18 @@ pcl_ros::SACSegmentationFromNormals::subscribe()
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
// Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked) // 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_) { if (approximate_sync_) {
sync_input_normals_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, sync_input_normals_indices_a_ =
PointCloudN, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices>>>(max_queue_size_);
} else { } else {
sync_input_normals_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, sync_input_normals_indices_e_ =
PointCloudN, PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices>>>(max_queue_size_);
} }
// If we're supposed to look for PointIndices (indices) // If we're supposed to look for PointIndices (indices)
@ -676,9 +689,12 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
if (indices && !indices->header.frame_id.empty()) { if (indices && !indices->header.frame_id.empty()) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_indices_callback]\n" "[%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 "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" "frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -692,8 +708,10 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_indices_callback]\n" "[%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 "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -711,7 +729,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
if (cloud_nr_points != cloud_normals_nr_points) { if (cloud_nr_points != cloud_normals_nr_points) {
NODELET_ERROR( 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)!", "[%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); getName().c_str(), cloud_nr_points, cloud_normals_nr_points);
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers)); pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model)); pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
@ -728,7 +747,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
impl_.setIndices(indices_ptr); impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) // Final check if the data is empty
// (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty()) { if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers; pcl::PointIndices pcl_inliers;
pcl::ModelCoefficients pcl_model; pcl::ModelCoefficients pcl_model;
@ -740,7 +760,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
} }
// Check if we have enough inliers, clear inliers + model if not // Check if we have enough inliers, clear inliers + model if not
if ((int)inliers.indices.size() <= min_inliers_) { if (static_cast<int>(inliers.indices.size()) <= min_inliers_) {
inliers.indices.clear(); inliers.indices.clear();
model.values.clear(); model.values.clear();
} }
@ -749,7 +769,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers)); pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model)); pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", "[%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(), getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(),
model.values.size(), pnh_->resolveName("model").c_str()); model.values.size(), pnh_->resolveName("model").c_str());
if (inliers.indices.empty()) { if (inliers.indices.empty()) {

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

@ -36,10 +36,10 @@
*/ */
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include "pcl_ros/segmentation/segment_differences.hpp"
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include "pcl_ros/segmentation/segment_differences.hpp"
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::onInit() pcl_ros::SegmentDifferences::onInit()
{ {
@ -57,7 +57,7 @@ pcl_ros::SegmentDifferences::onInit()
onInitPostProcess(); onInitPostProcess();
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::subscribe() pcl_ros::SegmentDifferences::subscribe()
{ {
@ -72,16 +72,18 @@ pcl_ros::SegmentDifferences::subscribe()
srv_->setCallback(f); srv_->setCallback(f);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_target_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, sync_input_target_a_ =
PointCloud>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloud>>>(max_queue_size_);
sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_); sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_a_->registerCallback( sync_input_target_a_->registerCallback(
bind( bind(
&SegmentDifferences::input_target_callback, this, &SegmentDifferences::input_target_callback, this,
_1, _2)); _1, _2));
} else { } else {
sync_input_target_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, sync_input_target_e_ =
PointCloud>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloud>>>(max_queue_size_);
sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_); sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_e_->registerCallback( sync_input_target_e_->registerCallback(
bind( bind(
@ -90,7 +92,7 @@ pcl_ros::SegmentDifferences::subscribe()
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::unsubscribe() pcl_ros::SegmentDifferences::unsubscribe()
{ {
@ -98,7 +100,7 @@ pcl_ros::SegmentDifferences::unsubscribe()
sub_target_filter_.unsubscribe(); sub_target_filter_.unsubscribe();
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level) pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level)
{ {
@ -131,8 +133,10 @@ pcl_ros::SegmentDifferences::input_target_callback(
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback]\n" "[%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 "
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", "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(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(

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

@ -37,8 +37,9 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <pcl/common/io.h> #include <pcl/common/io.h>
#include "pcl_ros/surface/convex_hull.hpp"
#include <geometry_msgs/PolygonStamped.h> #include <geometry_msgs/PolygonStamped.h>
#include <vector>
#include "pcl_ros/surface/convex_hull.hpp"
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
@ -73,8 +74,9 @@ pcl_ros::ConvexHull2D::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", 1); sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, sync_input_indices_a_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback( sync_input_indices_a_->registerCallback(
@ -82,8 +84,9 @@ pcl_ros::ConvexHull2D::subscribe()
&ConvexHull2D::input_indices_callback, this, _1, &ConvexHull2D::input_indices_callback, this, _1,
_2)); _2));
} else { } else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, sync_input_indices_e_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback( sync_input_indices_e_->registerCallback(
@ -146,8 +149,10 @@ pcl_ros::ConvexHull2D::input_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_model_callback]\n" "[%s::input_indices_model_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 "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
@ -156,7 +161,8 @@ pcl_ros::ConvexHull2D::input_indices_callback(
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str()); indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", "[%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( getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
getMTPrivateNodeHandle().resolveName("input").c_str()); getMTPrivateNodeHandle().resolveName("input").c_str());

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

@ -36,8 +36,10 @@
*/ */
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include "pcl_ros/surface/moving_least_squares.hpp"
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <vector>
#include "pcl_ros/surface/moving_least_squares.hpp"
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::MovingLeastSquares::onInit() pcl_ros::MovingLeastSquares::onInit()
@ -50,7 +52,8 @@ pcl_ros::MovingLeastSquares::onInit()
// if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_)) // if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
if (!pnh_->getParam("search_radius", search_radius_)) { if (!pnh_->getParam("search_radius", search_radius_)) {
//NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ()); // NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set "
// "at least one of these parameters before continuing.", getName ().c_str ());
NODELET_ERROR( NODELET_ERROR(
"[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", "[%s::onInit] Need a 'search_radius' parameter to be set before continuing!",
getName().c_str()); getName().c_str());
@ -93,8 +96,10 @@ pcl_ros::MovingLeastSquares::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", 1); sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_) { if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, sync_input_indices_a_ =
PointIndices>>>(max_queue_size_); boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloudIn, PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback( sync_input_indices_a_->registerCallback(
@ -102,7 +107,9 @@ pcl_ros::MovingLeastSquares::subscribe()
&MovingLeastSquares::input_indices_callback, &MovingLeastSquares::input_indices_callback,
this, _1, _2)); this, _1, _2));
} else { } else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<PointCloudIn,
PointIndices>>>(max_queue_size_); PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register // surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
@ -168,8 +175,10 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
if (indices) { if (indices) {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_indices_model_callback]\n" "[%s::input_indices_model_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 "
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.", "frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
@ -178,7 +187,8 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str()); indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", "[%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( getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
getMTPrivateNodeHandle().resolveName("input").c_str()); getMTPrivateNodeHandle().resolveName("input").c_str());

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

@ -48,7 +48,9 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
using namespace tf; #include <algorithm>
#include <string>
#include <vector>
// using a random point type, as we want to make sure that it does work with // using a random point type, as we want to make sure that it does work with
// other points than just XYZ // other points than just XYZ
@ -78,7 +80,7 @@ void setStamp(ros::Time & stamp, std::uint64_t & pcl_stamp)
class Notification class Notification
{ {
public: public:
Notification(int expected_count) explicit Notification(int expected_count)
: count_(0), : count_(0),
expected_count_(expected_count), expected_count_(expected_count),
failure_count_(0) failure_count_(0)
@ -90,7 +92,7 @@ public:
++count_; ++count_;
} }
void failure(const PCDType::ConstPtr & message, FilterFailureReason reason) void failure(const PCDType::ConstPtr & message, tf::FilterFailureReason reason)
{ {
++failure_count_; ++failure_count_;
} }
@ -104,7 +106,7 @@ TEST(MessageFilter, noTransforms)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);
@ -120,7 +122,7 @@ TEST(MessageFilter, noTransformsSameFrame)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);
@ -136,7 +138,7 @@ TEST(MessageFilter, preexistingTransforms)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);
@ -161,7 +163,7 @@ TEST(MessageFilter, postTransforms)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp = ros::Time::now(); ros::Time stamp = ros::Time::now();
@ -190,7 +192,7 @@ TEST(MessageFilter, queueSize)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(10); Notification n(10);
MessageFilter<PCDType> filter(tf_client, "frame1", 10); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 10);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
@ -226,7 +228,7 @@ TEST(MessageFilter, setTargetFrame)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.setTargetFrame("frame1000"); filter.setTargetFrame("frame1000");
@ -252,7 +254,7 @@ TEST(MessageFilter, multipleTargetFrames)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "", 1); tf::MessageFilter<PCDType> filter(tf_client, "", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
std::vector<std::string> target_frames; std::vector<std::string> target_frames;
@ -295,7 +297,7 @@ TEST(MessageFilter, tolerance)
ros::Duration offset(0.2); ros::Duration offset(0.2);
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.setTolerance(offset); filter.setTolerance(offset);
@ -339,7 +341,7 @@ TEST(MessageFilter, outTheBackFailure)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
ros::Time stamp = ros::Time::now(); ros::Time stamp = ros::Time::now();
@ -366,7 +368,7 @@ TEST(MessageFilter, emptyFrameIDFailure)
{ {
tf::TransformListener tf_client; tf::TransformListener tf_client;
Notification n(1); Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1); tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
PCDType::Ptr msg(new PCDType); PCDType::Ptr msg(new PCDType);

18
pcl_ros/src/transforms.cpp
View File

@ -38,6 +38,8 @@
#include <pcl/common/io.h> #include <pcl/common/io.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <limits>
#include <string>
#include "pcl_ros/transforms.hpp" #include "pcl_ros/transforms.hpp"
#include "pcl_ros/impl/transforms.hpp" #include "pcl_ros/impl/transforms.hpp"
@ -142,13 +144,15 @@ transformPointCloud(
in.fields[z_idx].offset, 0); in.fields[z_idx].offset, 0);
for (size_t i = 0; i < in.width * in.height; ++i) { for (size_t i = 0; i < in.width * in.height; ++i) {
Eigen::Vector4f pt(*(float *)&in.data[xyz_offset[0]], *(float *)&in.data[xyz_offset[1]], Eigen::Vector4f pt(*reinterpret_cast<float *>(&in.data[xyz_offset[0]]),
*(float *)&in.data[xyz_offset[2]], 1); *reinterpret_cast<float *>(&in.data[xyz_offset[1]]),
*reinterpret_cast<float *>(&in.data[xyz_offset[2]], 1));
Eigen::Vector4f pt_out; Eigen::Vector4f pt_out;
bool max_range_point = false; bool max_range_point = false;
int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset; int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset;
float * distance_ptr = (dist_idx < 0 ? NULL : (float *)(&in.data[distance_ptr_offset])); float * distance_ptr =
(dist_idx < 0 ? NULL : reinterpret_cast<float *>(&in.data[distance_ptr_offset]));
if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
pt_out = pt; pt_out = pt;
@ -156,7 +160,8 @@ transformPointCloud(
pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt[0] = *distance_ptr; // Replace x with the x value saved in distance
pt_out = transform * pt; pt_out = transform * pt;
max_range_point = true; max_range_point = true;
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n"; // std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<",
// "<<pt_out[1]<<","<<pt_out[2]<<"\n";
} }
} else { } else {
pt_out = transform * pt; pt_out = transform * pt;
@ -164,7 +169,7 @@ transformPointCloud(
if (max_range_point) { if (max_range_point) {
// Save x value in distance again // Save x value in distance again
*(float *)(&out.data[distance_ptr_offset]) = pt_out[0]; *reinterpret_cast<float *>(&out.data[distance_ptr_offset]) = pt_out[0];
pt_out[0] = std::numeric_limits<float>::quiet_NaN(); pt_out[0] = std::numeric_limits<float>::quiet_NaN();
} }
@ -181,7 +186,8 @@ transformPointCloud(
if (vp_idx != -1) { if (vp_idx != -1) {
// Transform the viewpoint info too // Transform the viewpoint info too
for (size_t i = 0; i < out.width * out.height; ++i) { for (size_t i = 0; i < out.width * out.height; ++i) {
float * pstep = (float *)&out.data[i * out.point_step + out.fields[vp_idx].offset]; float * pstep =
reinterpret_cast<float *>(&out.data[i * out.point_step + out.fields[vp_idx].offset]);
// Assume vp_x, vp_y, vp_z are consecutive // Assume vp_x, vp_y, vp_z are consecutive
Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1); Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1);
Eigen::Vector4f vp_out = transform * vp_in; Eigen::Vector4f vp_out = transform * vp_in;

7
pcl_ros/tools/bag_to_pcd.cpp
View File

@ -44,16 +44,17 @@ Cloud Data) format.
**/ **/
#include <sstream>
#include <boost/filesystem.hpp>
#include <rosbag/bag.h> #include <rosbag/bag.h>
#include <rosbag/view.h> #include <rosbag/view.h>
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/transforms.hpp"
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h> #include <tf/transform_broadcaster.h>
#include <boost/filesystem.hpp>
#include <sstream>
#include <string>
#include "pcl_ros/transforms.hpp"
typedef sensor_msgs::PointCloud2 PointCloud; typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr; typedef PointCloud::Ptr PointCloudPtr;

1
pcl_ros/tools/convert_pcd_to_image.cpp
View File

@ -53,6 +53,7 @@
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <string>
/* ---[ */ /* ---[ */
int int

21
pcl_ros/tools/pcd_to_pointcloud.cpp
View File

@ -43,10 +43,6 @@
**/ **/
// STL
#include <chrono>
#include <thread>
// ROS core // ROS core
#include <ros/ros.h> #include <ros/ros.h>
#include <pcl/io/io.h> #include <pcl/io/io.h>
@ -54,14 +50,17 @@
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/publisher.hpp" // STL
#include <chrono>
#include <string>
#include <thread>
using namespace std; #include "pcl_ros/publisher.hpp"
class PCDGenerator class PCDGenerator
{ {
protected: protected:
string tf_frame_; std::string tf_frame_;
ros::NodeHandle nh_; ros::NodeHandle nh_;
ros::NodeHandle private_nh_; ros::NodeHandle private_nh_;
@ -69,7 +68,7 @@ public:
// ROS messages // ROS messages
sensor_msgs::PointCloud2 cloud_; sensor_msgs::PointCloud2 cloud_;
string file_name_, cloud_topic_; std::string file_name_, cloud_topic_;
double wait_; double wait_;
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_; pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
@ -105,7 +104,7 @@ public:
bool spin() bool spin()
{ {
int nr_points = cloud_.width * cloud_.height; int nr_points = cloud_.width * cloud_.height;
string fields_list = pcl::getFieldsList(cloud_); std::string fields_list = pcl::getFieldsList(cloud_);
double interval = wait_ * 1e+6; double interval = wait_ * 1e+6;
while (nh_.ok()) { while (nh_.ok()) {
ROS_DEBUG_ONCE( ROS_DEBUG_ONCE(
@ -132,8 +131,6 @@ public:
} }
return true; return true;
} }
}; };
/* ---[ */ /* ---[ */
@ -149,7 +146,7 @@ main(int argc, char ** argv)
ros::init(argc, argv, "pcd_to_pointcloud"); ros::init(argc, argv, "pcd_to_pointcloud");
PCDGenerator c; PCDGenerator c;
c.file_name_ = string(argv[1]); c.file_name_ = std::string(argv[1]);
// check if publishing interval is given // check if publishing interval is given
if (argc == 2) { if (argc == 2) {
c.wait_ = 0; c.wait_ = 0;

6
pcl_ros/tools/pointcloud_to_pcd.cpp
View File

@ -53,7 +53,8 @@
#include <Eigen/Geometry> #include <Eigen/Geometry>
using namespace std; // STL
#include <string>
/** /**
\author Radu Bogdan Rusu \author Radu Bogdan Rusu
@ -76,7 +77,7 @@ private:
tf2_ros::TransformListener tf_listener_; tf2_ros::TransformListener tf_listener_;
public: public:
string cloud_topic_; std::string cloud_topic_;
ros::Subscriber sub_; ros::Subscriber sub_;
@ -131,7 +132,6 @@ public:
} else { } else {
writer.writeASCII(ss.str(), *cloud, v, q, 8); writer.writeASCII(ss.str(), *cloud, v, q, 8);
} }
} }
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////