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_
#define PCL_ROS_BOUNDARY_H_
#ifndef PCL_ROS__FEATURES__BOUNDARY_HPP_
#define PCL_ROS__FEATURES__BOUNDARY_HPP_
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
@ -82,6 +82,6 @@ private:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
} // namespace pcl_ros
#endif //#ifndef PCL_ROS_BOUNDARY_H_
#endif // PCL_ROS__FEATURES__BOUNDARY_HPP_

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

@ -35,23 +35,24 @@
*
*/
#ifndef PCL_ROS_FEATURE_H_
#define PCL_ROS_FEATURE_H_
#ifndef PCL_ROS__FEATURES__FEATURE_HPP_
#define PCL_ROS__FEATURES__FEATURE_HPP_
// PCL includes
#include <pcl/features/feature.h>
#include <pcl_msgs/PointIndices.h>
#include "pcl_ros/pcl_nodelet.hpp"
#include <message_filters/pass_through.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/FeatureConfig.hpp"
// PCL conversions
#include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/FeatureConfig.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
@ -84,13 +85,15 @@ public:
protected:
/** \brief The input point cloud dataset. */
//PointCloudInConstPtr input_;
// PointCloudInConstPtr input_;
/** \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. */
//PointCloudInConstPtr surface_;
/** \brief An input point cloud describing the surface that is to be used
* for nearest neighbors estimation.
*/
// PointCloudInConstPtr surface_;
/** \brief A pointer to the spatial search object. */
KdTreePtr tree_;
@ -108,7 +111,9 @@ protected:
/** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_;
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */
/** \brief Set to true if the nodelet needs to listen for incoming point
* clouds representing the search surface.
*/
bool use_surface_;
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
@ -257,6 +262,6 @@ private:
public:
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_
#define PCL_ROS_FPFH_H_
#ifndef PCL_ROS__FEATURES__FPFH_HPP_
#define PCL_ROS__FEATURES__FPFH_HPP_
#include <pcl/features/fpfh.h>
#include "pcl_ros/features/pfh.hpp"
@ -94,6 +94,6 @@ private:
public:
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_
#define PCL_ROS_FPFH_OMP_H_
#ifndef PCL_ROS__FEATURES__FPFH_OMP_HPP_
#define PCL_ROS__FEATURES__FPFH_OMP_HPP_
#include <pcl/features/fpfh_omp.h>
#include "pcl_ros/features/fpfh.hpp"
@ -91,6 +91,6 @@ private:
public:
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_
#define PCL_ROS_MOMENT_INVARIANTS_H_
#ifndef PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
#define PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
#include <pcl/features/moment_invariants.h>
#include "pcl_ros/features/feature.hpp"
@ -77,6 +77,6 @@ private:
public:
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_
#define PCL_ROS_NORMAL_3D_H_
#ifndef PCL_ROS__FEATURES__NORMAL_3D_HPP_
#define PCL_ROS__FEATURES__NORMAL_3D_HPP_
#include <pcl/features/normal_3d.h>
#include "pcl_ros/features/feature.hpp"
@ -81,6 +81,6 @@ private:
public:
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_
#define PCL_ROS_NORMAL_3D_OMP_H_
#ifndef PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
#define PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
#include <pcl/features/normal_3d_omp.h>
#include "pcl_ros/features/normal_3d.hpp"
@ -75,6 +75,6 @@ private:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
} // namespace pcl_ros
#endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_
#endif // PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_

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

@ -35,11 +35,11 @@
*
*/
#ifndef PCL_ROS_NORMAL_3D_TBB_H_
#define PCL_ROS_NORMAL_3D_TBB_H_
#ifndef PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
#define PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
//#include "pcl_ros/pcl_ros_config.hpp"
//#if defined(HAVE_TBB)
// #include "pcl_ros/pcl_ros_config.hpp"
// #if defined(HAVE_TBB)
#include <pcl/features/normal_3d_tbb.h>
#include "pcl_ros/features/normal_3d.hpp"
@ -78,8 +78,8 @@ private:
public:
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_
#define PCL_ROS_PFH_H_
#ifndef PCL_ROS__FEATURES__PFH_HPP_
#define PCL_ROS__FEATURES__PFH_HPP_
#include <pcl/features/pfh.h>
#include "pcl_ros/features/feature.hpp"
@ -94,6 +94,6 @@ private:
public:
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_
#define PCL_ROS_PRINCIPAL_CURVATURES_H_
#ifndef PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
#define PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
#include <pcl/features/principal_curvatures.h>
@ -80,6 +80,6 @@ private:
public:
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_
#define PCL_ROS_SHOT_H_
#ifndef PCL_ROS__FEATURES__SHOT_HPP_
#define PCL_ROS__FEATURES__SHOT_HPP_
#include <pcl/features/shot.h>
#include "pcl_ros/features/pfh.hpp"
@ -74,6 +74,6 @@ private:
public:
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_
#define PCL_ROS_SHOT_OMP_H_
#ifndef PCL_ROS__FEATURES__SHOT_OMP_HPP_
#define PCL_ROS__FEATURES__SHOT_OMP_HPP_
#include <pcl/features/shot_omp.h>
#include "pcl_ros/features/shot.hpp"
@ -73,6 +73,6 @@ private:
public:
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_
#define PCL_ROS_FEATURES_VFH_H_
#ifndef PCL_ROS__FEATURES__VFH_HPP_
#define PCL_ROS__FEATURES__VFH_HPP_
#include <pcl/features/vfh.h>
#include "pcl_ros/features/fpfh.hpp"
@ -79,6 +79,6 @@ private:
public:
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_
#define PCL_ROS_FILTERS_CROPBOX_H_
#ifndef PCL_ROS__FILTERS__CROP_BOX_HPP_
#define PCL_ROS__FILTERS__CROP_BOX_HPP_
// PCL includes
#include <pcl/filters/crop_box.h>
@ -58,7 +58,7 @@ class CropBox : public Filter
{
protected:
/** \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.
* \param input the input point cloud dataset
@ -101,6 +101,6 @@ private:
public:
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_
#define PCL_ROS_FILTERS_EXTRACTINDICES_H_
#ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
#define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
// PCL includes
#include <pcl/filters/extract_indices.h>
@ -94,6 +94,6 @@ private:
public:
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_
#define PCL_ROS_FILTER_H_
#ifndef PCL_ROS__FILTERS__FILTER_HPP_
#define PCL_ROS__FILTERS__FILTER_HPP_
// PCL includes
#include <pcl/filters/filter.h>
#include "pcl_ros/pcl_nodelet.hpp"
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/FilterConfig.hpp"
namespace pcl_ros
{
namespace sync_policies = message_filters::sync_policies;
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters
* are defined here as static methods.
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are
* applicable to all filters are defined here as static methods.
* \author Radu Bogdan Rusu
*/
class Filter : public PCLNodelet
@ -79,16 +77,22 @@ protected:
/** \brief The maximum allowed filter value a point will be considered from. */
double filter_limit_max_;
/** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
/** \brief Set to true if we want to return the data outside
* (\a filter_limit_min_;\a filter_limit_max_). Default: false.
*/
bool filter_limit_negative_;
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
/** \brief The input TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_input_frame_;
/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
/** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_output_frame_;
/** \brief Internal mutex. */
@ -157,6 +161,6 @@ private:
public:
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_
#define PCL_ROS_FILTERS_PASSTHROUGH_H_
#ifndef PCL_ROS__FILTERS__PASSTHROUGH_HPP_
#define PCL_ROS__FILTERS__PASSTHROUGH_HPP_
// PCL includes
#include <pcl/filters/passthrough.h>
@ -95,6 +95,6 @@ private:
public:
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_
#define PCL_ROS_FILTERS_PROJECT_INLIERS_H_
#ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
#define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
// PCL includes
#include <pcl/filters/project_inliers.h>
#include <message_filters/subscriber.h>
#include "pcl_ros/filters/filter.hpp"
#include <message_filters/subscriber.h>
namespace pcl_ros
{
@ -115,6 +114,6 @@ private:
public:
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_
#define PCL_ROS_FILTERS_RADIUSOUTLIERREMOVAL_H_
#ifndef PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
#define PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
// PCL includes
#include <pcl/filters/radius_outlier_removal.h>
@ -96,6 +96,6 @@ private:
public:
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_
#define PCL_ROS_FILTERS_STATISTICALOUTLIERREMOVAL_H_
#ifndef PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
#define PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
// PCL includes
#include <pcl/filters/statistical_outlier_removal.h>
@ -103,6 +103,6 @@ private:
public:
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_
#define PCL_ROS_FILTERS_VOXEL_H_
#ifndef PCL_ROS__FILTERS__VOXEL_GRID_HPP_
#define PCL_ROS__FILTERS__VOXEL_GRID_HPP_
// PCL includes
#include <pcl/filters/voxel_grid.h>
@ -86,6 +86,6 @@ protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
} // namespace pcl_ros
#endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_
#endif // PCL_ROS__FILTERS__VOXEL_GRID_HPP_

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

@ -34,10 +34,11 @@
*
*/
#ifndef pcl_ros_IMPL_TRANSFORMS_H_
#define pcl_ros_IMPL_TRANSFORMS_H_
#ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_
#define PCL_ROS__IMPL__TRANSFORMS_HPP_
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include "pcl_ros/transforms.hpp"
using pcl_conversions::fromPCL;
@ -58,13 +59,13 @@ transformPointCloudWithNormals(
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
// the conversion of the point cloud anyway. Idem for the origin.
tf::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v);
// Assemble an Eigen Transform
//Eigen::Transform3f t;
//t = translation * rotation;
// Eigen::Transform3f t;
// t = translation * rotation;
transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation);
}
@ -81,13 +82,13 @@ transformPointCloud(
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
// the conversion of the point cloud anyway. Idem for the origin.
tf::Quaternion q = transform.getRotation();
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
tf::Vector3 v = transform.getOrigin();
Eigen::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v);
// Assemble an Eigen Transform
//Eigen::Transform3f t;
//t = translation * rotation;
// Eigen::Transform3f t;
// t = translation * rotation;
transformPointCloud(cloud_in, cloud_out, origin, rotation);
}
@ -217,6 +218,5 @@ transformPointCloudWithNormals(
cloud_out.header = toPCL(header);
return true;
}
} // namespace pcl_ros
#endif
#endif // PCL_ROS__IMPL__TRANSFORMS_HPP_

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

@ -35,13 +35,14 @@
*
*/
#ifndef PCL_ROS_IO_BAG_IO_H_
#define PCL_ROS_IO_BAG_IO_H_
#ifndef PCL_ROS__IO__BAG_IO_HPP_
#define PCL_ROS__IO__BAG_IO_HPP_
#include <pcl_ros/pcl_nodelet.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <string>
namespace pcl_ros
{
@ -120,12 +121,11 @@ private:
PointCloudPtr output_;
/** \brief Signals that a new PointCloud2 message has been read from the file. */
//bool cloud_received_;
// bool cloud_received_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros
}
#endif //#ifndef PCL_ROS_IO_BAG_IO_H_
#endif // PCL_ROS__IO__BAG_IO_HPP_

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

@ -35,8 +35,8 @@
*
*/
#ifndef PCL_IO_CONCATENATE_DATA_H_
#define PCL_IO_CONCATENATE_DATA_H_
#ifndef PCL_ROS__IO__CONCATENATE_DATA_HPP_
#define PCL_ROS__IO__CONCATENATE_DATA_HPP_
// ROS includes
#include <tf/transform_listener.h>
@ -46,6 +46,8 @@
#include <message_filters/pass_through.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <string>
#include <vector>
namespace pcl_ros
{
@ -71,7 +73,7 @@ public:
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateDataSynchronizer(int queue_size)
explicit PointCloudConcatenateDataSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), approximate_sync_(false) {}
/** \brief Empty destructor. */
@ -88,7 +90,9 @@ private:
/** \brief The maximum number of messages that we can store in the queue. */
int maximum_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
/** \brief True if we use an approximate time synchronizer
* versus an exact one (false by default).
*/
bool approximate_sync_;
/** \brief A vector of message filters. */
@ -137,6 +141,6 @@ private:
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_
#define PCL_IO_CONCATENATE_FIELDS_H_
#ifndef PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
#define PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
// ROS includes
#include <nodelet_topic_tools/nodelet_lazy.h>
@ -47,6 +47,9 @@
#include <sensor_msgs/PointCloud2.h>
#include <map>
#include <vector>
namespace pcl_ros
{
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
@ -68,7 +71,7 @@ public:
/** \brief Empty constructor.
* \param queue_size the maximum queue size
*/
PointCloudConcatenateFieldsSynchronizer(int queue_size)
explicit PointCloudConcatenateFieldsSynchronizer(int queue_size)
: maximum_queue_size_(queue_size), maximum_seconds_(0) {}
/** \brief Empty destructor. */
@ -98,6 +101,6 @@ private:
/** \brief A queue for messages. */
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_
#define PCL_ROS_IO_PCD_IO_H_
#ifndef PCL_ROS__IO__PCD_IO_HPP_
#define PCL_ROS__IO__PCD_IO_HPP_
#include <pcl/io/pcd_io.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
@ -131,6 +132,6 @@ private:
public:
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_
#define PCL_NODELET_H_
#ifndef PCL_ROS__PCL_NODELET_HPP_
#define PCL_ROS__PCL_NODELET_HPP_
#include <sensor_msgs/PointCloud2.h>
// PCL includes
@ -51,7 +51,6 @@
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/point_cloud.hpp"
// ROS Nodelet includes
#include <nodelet_topic_tools/nodelet_lazy.h>
#include <message_filters/subscriber.h>
@ -62,6 +61,11 @@
// Include TF
#include <tf/transform_listener.h>
// STL
#include <string>
#include "pcl_ros/point_cloud.hpp"
using pcl_conversions::fromPCL;
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
{
public:
@ -128,7 +134,9 @@ protected:
/** \brief The maximum queue size (default: 3). */
int max_queue_size_;
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
/** \brief True if we use an approximate time synchronizer
* versus an exact one (false by default).
**/
bool approximate_sync_;
/** \brief TF listener object. */
@ -143,7 +151,8 @@ protected:
{
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
NODELET_WARN(
"[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!",
"[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) "
"with stamp %f, and frame %s on topic %s received!",
getName().c_str(),
cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -163,7 +172,8 @@ protected:
{
if (cloud->width * cloud->height != cloud->points.size()) {
NODELET_WARN(
"[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!",
"[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) "
"with stamp %f, and frame %s on topic %s received!",
getName().c_str(), cloud->points.size(), cloud->width, cloud->height,
fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
pnh_->resolveName(topic_name).c_str());
@ -182,7 +192,10 @@ protected:
{
/*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;
@ -197,13 +210,18 @@ protected:
{
/*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 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 unsubscribe() {}
@ -237,6 +255,6 @@ protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
} // namespace pcl_ros
#endif //#ifndef PCL_NODELET_H_
#endif // PCL_ROS__PCL_NODELET_HPP_

99
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 <pcl/point_cloud.h>
#include <pcl/point_traits.h>
#include <pcl/for_each_type.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 <sensor_msgs/PointCloud2.h>
#include <boost/mpl/size.hpp>
#include <boost/ref.hpp>
#include <string>
#include <utility>
#include <vector>
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
#include <type_traits>
#include <memory>
#endif
namespace pcl
{
@ -18,7 +74,7 @@ namespace detail
template<typename Stream, typename PointT>
struct FieldStreamer
{
FieldStreamer(Stream & stream)
explicit FieldStreamer(Stream & stream)
: stream_(stream) {}
template<typename U>
@ -59,8 +115,8 @@ struct FieldsLength
std::uint32_t length;
};
} // namespace pcl::detail
} // namespace pcl
} // namespace detail
} // namespace pcl
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;}
};
} // namespace ros::message_traits
} // namespace message_traits
namespace serialization
{
@ -297,30 +353,11 @@ struct Serializer<pcl::PointCloud<T>>
return length;
}
};
} // namespace ros::serialization
} // namespace serialization
/// @todo Printer specialization in message_operations
} // 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 ros
namespace pcl
{
@ -341,7 +378,7 @@ struct Holder
{
SharedPointer p;
Holder(const SharedPointer & p)
explicit Holder(const SharedPointer & p)
: p(p) {}
Holder(const Holder & other)
: p(other.p) {}
@ -373,7 +410,7 @@ inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
}
}
#endif
} // namespace pcl::detail
} // namespace detail
// add functions to convert to smart pointer used by ROS
template<class T>
@ -420,6 +457,6 @@ inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
return p;
}
#endif
} // namespace pcl
} // namespace pcl
#endif
#endif // PCL_ROS__POINT_CLOUD_HPP__

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

@ -42,8 +42,8 @@
**/
#ifndef PCL_ROS_PUBLISHER_H_
#define PCL_ROS_PUBLISHER_H_
#ifndef PCL_ROS__PUBLISHER_HPP_
#define PCL_ROS__PUBLISHER_HPP_
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
@ -51,6 +51,8 @@
#include <pcl_conversions/pcl_conversions.h>
#include <string>
namespace pcl_ros
{
class BasePublisher
@ -82,7 +84,7 @@ public:
operator void *() const
{
return (pub_) ? (void *)1 : (void *)0;
return (pub_) ? reinterpret_cast<void *>(1) : reinterpret_cast<void *>(0);
}
protected:
@ -135,15 +137,15 @@ public:
publish(const sensor_msgs::PointCloud2Ptr & point_cloud) const
{
pub_.publish(point_cloud);
//pub_.publish (*point_cloud);
// pub_.publish (*point_cloud);
}
void
publish(const sensor_msgs::PointCloud2 & point_cloud) const
{
pub_.publish(boost::make_shared<const sensor_msgs::PointCloud2>(point_cloud));
//pub_.publish (point_cloud);
// pub_.publish (point_cloud);
}
};
}
#endif //#ifndef PCL_ROS_PUBLISHER_H_
} // namespace pcl_ros
#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_
#define PCL_ROS_EXTRACT_CLUSTERS_H_
#ifndef PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_
#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 <pcl/segmentation/extract_clusters.h>
#include <limits>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/EuclideanClusterExtractionConfig.hpp"
namespace pcl_ros
@ -109,6 +108,6 @@ private:
public:
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_
#define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_
#ifndef PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_
#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/approximate_time.h>
#include <message_filters/pass_through.h>
// PCL includes
#include <pcl/segmentation/extract_polygonal_prism_data.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp"
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
{
@ -129,6 +125,6 @@ private:
public:
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_
#define PCL_ROS_SAC_SEGMENTATION_H_
#ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
#define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
#include "pcl_ros/pcl_nodelet.hpp"
#include <message_filters/pass_through.h>
// PCL includes
#include <pcl/segmentation/sac_segmentation.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include <string>
#include "pcl_ros/pcl_nodelet.hpp"
#include "pcl_ros/SACSegmentationConfig.hpp"
#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp"
@ -54,8 +51,9 @@ namespace pcl_ros
namespace sync_policies = message_filters::sync_policies;
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in
* the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus
* methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose
* SAC-based segmentation.
* \author Radu Bogdan Rusu
*/
class SACSegmentation : public PCLNodelet
@ -69,7 +67,8 @@ public:
SACSegmentation()
: min_inliers_(0) {}
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
/** \brief Set the input TF frame the data should be transformed into before processing,
* if input.header.frame_id is different.
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
*/
inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
@ -102,13 +101,17 @@ protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig>> srv_;
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
/** \brief The input TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_input_frame_;
/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
/** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_output_frame_;
/** \brief Null passthrough filter, used for pushing empty elements in the
@ -180,8 +183,8 @@ public:
};
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
* models that require the use of surface normals for estimation.
/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for
* Sample Consensus methods and models that require the use of surface normals for estimation.
*/
class SACSegmentationFromNormals : public SACSegmentation
{
@ -194,7 +197,8 @@ class SACSegmentationFromNormals : public SACSegmentation
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
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
*/
inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
@ -237,11 +241,15 @@ protected:
* synchronizer */
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_;
/** \brief The original data input TF frame. */
std::string tf_input_orig_frame_;
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
/** \brief The output TF frame the data should be transformed into,
* if input.header.frame_id is different.
*/
std::string tf_output_frame_;
/** \brief Nodelet initialization routine. */
@ -288,6 +296,6 @@ private:
public:
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_
#define PCL_ROS_SEGMENT_DIFFERENCES_H_
#ifndef PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
#define PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
#include <pcl/segmentation/segment_differences.h>
#include "pcl_ros/pcl_nodelet.hpp"
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/SegmentDifferencesConfig.hpp"
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
@ -108,6 +106,6 @@ private:
public:
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_
#define PCL_ROS_CONVEX_HULL_2D_H_
#ifndef PCL_ROS__SURFACE__CONVEX_HULL_HPP_
#define PCL_ROS__SURFACE__CONVEX_HULL_HPP_
#include "pcl_ros/pcl_nodelet.hpp"
// PCL includes
#include <pcl/surface/convex_hull.h>
// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include "pcl_ros/pcl_nodelet.hpp"
namespace pcl_ros
{
@ -94,6 +90,6 @@ private:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
} // namespace pcl_ros
#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_
#endif // PCL_ROS__SURFACE__CONVEX_HULL_HPP_

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

@ -35,25 +35,21 @@
*
*/
#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_
#define PCL_ROS_MOVING_LEAST_SQUARES_H_
#ifndef PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
#define PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
#include "pcl_ros/pcl_nodelet.hpp"
// PCL includes
#include <pcl/surface/mls.h>
#include <pcl/kdtree/kdtree.h> // for KdTree
// Dynamic reconfigure
#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 sync_policies = message_filters::sync_policies;
/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
* The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters.
* The type of the output is the same as the input, it only smooths the XYZ coordinates according
* to the parameters.
* Normals are estimated at each point as well and published on a separate topic.
* \author Radu Bogdan Rusu, Zoltan-Csaba Marton
*/
@ -71,7 +67,9 @@ class MovingLeastSquares : public PCLNodelet
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
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_;
/** \brief A pointer to the spatial search object. */
@ -81,7 +79,7 @@ protected:
double search_radius_;
/** \brief The number of K nearest neighbors to use for each point. */
//int k_;
// int k_;
/** \brief Whether to use a polynomial fit. */
bool use_polynomial_fit_;
@ -89,7 +87,9 @@ protected:
/** \brief The order of the polynomial to be fit. */
int polynomial_order_;
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */
/** \brief How 'flat' should the neighbor weighting gaussian be
* the smaller, the more local the fit).
*/
double gaussian_parameter_;
// ROS nodelet attributes
@ -147,6 +147,6 @@ private:
public:
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_
#define pcl_ROS_TRANSFORMS_H_
#ifndef PCL_ROS__TRANSFORMS_HPP_
#define PCL_ROS__TRANSFORMS_HPP_
#include <sensor_msgs/PointCloud2.h>
#include <pcl/common/transforms.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <string>
namespace pcl_ros
{
@ -176,6 +177,6 @@ transformPointCloud(
*/
void
transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat);
}
} // namespace pcl_ros
#endif // PCL_ROS_TRANSFORMS_H_
#endif // PCL_ROS__TRANSFORMS_HPP_

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

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

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

@ -78,4 +78,4 @@ pcl_ros::NormalEstimationTBB::computePublish(
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet)
#endif // HAVE_TBB
#endif // HAVE_TBB

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 <pluginlib/class_list_macros.h>
//////////////////////////////////////////////////////////////////////////////////////////////
bool
@ -102,7 +102,8 @@ pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t leve
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) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG(

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

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

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

@ -78,4 +78,4 @@ pcl_ros::NormalEstimationTBB::computePublish(
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet);
#endif // HAVE_TBB
#endif // HAVE_TBB

37
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/io/io.h>
#include <vector>
#include "pcl_ros/transforms.hpp"
/*//#include <pcl/filters/pixel_grid.h>
//#include <pcl/filters/filter_dimension.h>
@ -48,18 +49,18 @@
*/
// Include the implementations instead of compiling them separately to speed up compile time
//#include "extract_indices.cpp"
//#include "passthrough.cpp"
//#include "project_inliers.cpp"
//#include "radius_outlier_removal.cpp"
//#include "statistical_outlier_removal.cpp"
//#include "voxel_grid.cpp"
// #include "extract_indices.cpp"
// #include "passthrough.cpp"
// #include "project_inliers.cpp"
// #include "radius_outlier_removal.cpp"
// #include "statistical_outlier_removal.cpp"
// #include "voxel_grid.cpp"
/*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet);
//PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet);
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
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_);
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_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
pcl_msgs::PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2));
@ -182,7 +185,8 @@ pcl_ros::Filter::onInit()
void
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) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG(
@ -218,8 +222,10 @@ pcl_ros::Filter::input_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -228,7 +234,8 @@ pcl_ros::Filter::input_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.",
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on "
"topic %s received.",
getName().c_str(), cloud->width * cloud->height,
cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str());
}

13
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 <pluginlib/class_list_macros.h>
//////////////////////////////////////////////////////////////////////////////////////////////
bool
@ -65,7 +65,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
if (filter_min != config.filter_limit_min) {
filter_min = config.filter_limit_min;
NODELET_DEBUG(
"[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
"[%s::config_callback] Setting the minimum filtering value a point will be "
"considered from to: %f.",
getName().c_str(), filter_min);
// Set the filter min-max if different
impl_.setFilterLimits(filter_min, filter_max);
@ -74,14 +75,15 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
if (filter_max != config.filter_limit_max) {
filter_max = config.filter_limit_max;
NODELET_DEBUG(
"[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
"[%s::config_callback] Setting the maximum filtering value a point will be "
"considered from to: %f.",
getName().c_str(), filter_max);
// Set the filter min-max if different
impl_.setFilterLimits(filter_min, filter_max);
}
// Check the current value for the filter field
//std::string filter_field = impl_.getFilterFieldName ();
// std::string filter_field = impl_.getFilterFieldName ();
if (impl_.getFilterFieldName() != config.filter_field_name) {
// Set the filter field if different
impl_.setFilterFieldName(config.filter_field_name);
@ -108,7 +110,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
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) {
tf_input_frame_ = config.input_frame;
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 <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <vector>
//////////////////////////////////////////////////////////////////////////////////////////////
void
@ -99,16 +100,20 @@ pcl_ros::ProjectInliers::subscribe()
sub_model_.subscribe(*pnh_, "model", max_queue_size_);
if (approximate_sync_) {
sync_input_indices_model_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2,
PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_a_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
sync_input_indices_model_a_->registerCallback(
bind(
&ProjectInliers::input_indices_model_callback,
this, _1, _2, _3));
} else {
sync_input_indices_model_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2,
PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_e_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
sync_input_indices_model_e_->registerCallback(
bind(
@ -148,9 +153,12 @@ pcl_ros::ProjectInliers::input_indices_model_callback(
NODELET_DEBUG(
"[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n"
" - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.\n"
" - ModelCoefficients with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(),

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.",
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) {
impl_.setMeanK(config.mean_k);
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);
}

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) {
filter_min = config.filter_limit_min;
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);
}
if (filter_max != config.filter_limit_max) {
filter_max = config.filter_limit_max;
NODELET_DEBUG(
"[config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
"[config_callback] Setting the maximum filtering value a point will be considered "
"from to: %f.",
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());
}
// ---[ 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) {
tf_input_frame_ = config.input_frame;
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 <string>
#include "pcl_ros/io/bag_io.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////

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

@ -37,10 +37,11 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include "pcl_ros/transforms.hpp"
#include "pcl_ros/io/concatenate_data.hpp"
#include <pcl_conversions/pcl_conversions.h>
//////////////////////////////////////////////////////////////////////////////////////////////
void
@ -244,7 +245,7 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(
const PointCloud2 & in2,
PointCloud2 & out)
{
//ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
// ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
PointCloud2::Ptr in1_t(new PointCloud2());
PointCloud2::Ptr in2_t(new PointCloud2());

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

@ -39,11 +39,12 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/io/concatenate_fields.hpp"
#include <pcl_conversions/pcl_conversions.h>
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
{
@ -66,7 +67,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
onInitPostProcess();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
{
@ -75,19 +76,20 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
&PointCloudConcatenateFieldsSynchronizer::input_callback, this);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe()
{
sub_input_.shutdown();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud)
{
NODELET_DEBUG(
"[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
"[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on "
"topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(),
pnh_->resolveName("input").c_str());
@ -98,7 +100,8 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
queue_.size() > 0)
{
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(),
fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ));
queue_.erase(queue_.begin());
@ -107,7 +110,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
// Push back new data
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
std::vector<PointCloudConstPtr> & clouds = queue_[cloud->header.stamp];
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) {
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);
break;
}
@ -162,11 +166,10 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
// Clean the queue to avoid overflowing
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());
}
}
}
typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;

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

@ -38,20 +38,21 @@
#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
//#include <pcl_ros/subscriber.hpp>
// #include <pcl_ros/subscriber.hpp>
#include <nodelet_topic_tools/nodelet_mux.h>
#include <nodelet_topic_tools/nodelet_demux.h>
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2,
message_filters::Subscriber<sensor_msgs::PointCloud2>> NodeletMUX;
//typedef nodelet::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;
//#include "pcd_io.cpp"
//#include "bag_io.cpp"
//#include "concatenate_fields.cpp"
//#include "concatenate_data.cpp"
// #include "pcd_io.cpp"
// #include "bag_io.cpp"
// #include "concatenate_fields.cpp"
// #include "concatenate_data.cpp"
PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet);
//PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);
// PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);

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

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

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

@ -38,9 +38,10 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl/PointIndices.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/segmentation/extract_clusters.hpp"
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL;
using pcl_conversions::moveFromPCL;
@ -69,7 +70,7 @@ pcl_ros::EuclideanClusterExtraction::onInit()
return;
}
//private_nh.getParam ("use_indices", use_indices_);
// private_nh.getParam ("use_indices", use_indices_);
pnh_->getParam("publish_indices", publish_indices_);
if (publish_indices_) {
@ -110,16 +111,19 @@ pcl_ros::EuclideanClusterExtraction::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
bind(
&EuclideanClusterExtraction::
input_indices_callback, this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(
bind(
@ -206,8 +210,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
std_msgs::Header indices_header = indices->header;
NODELET_DEBUG(
"[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud_header.stamp.toSec(), cloud_header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
@ -215,7 +221,8 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on "
"topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
@ -235,10 +242,11 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
if (publish_indices_) {
for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) {
if (static_cast<int>(i) >= max_clusters_) {
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;
moveFromPCL(clusters[i], ros_pi);
ros_pi.header.stamp += ros::Duration(i * 0.001);
@ -250,15 +258,16 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
clusters.size(), pnh_->resolveName("output").c_str());
} else {
for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) {
if (static_cast<int>(i) >= max_clusters_) {
break;
}
PointCloud output;
copyPointCloud(*cloud, clusters[i].indices, output);
//PointCloud output_blob; // Convert from the templated output to the PointCloud blob
//pcl::toROSMsg (output, output_blob);
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
// PointCloud output_blob; // Convert from the templated output to the PointCloud blob
// pcl::toROSMsg (output, output_blob);
// 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);
header.stamp += ros::Duration(i * 0.001);
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 <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/transforms.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::moveToPCL;
@ -73,11 +73,13 @@ pcl_ros::ExtractPolygonalPrismData::subscribe()
// Create the objects here
if (approximate_sync_) {
sync_input_hull_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud, PointIndices>>>(max_queue_size_);
sync_input_hull_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices>>>(max_queue_size_);
} else {
sync_input_hull_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloud, PointIndices>>>(max_queue_size_);
sync_input_hull_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloud, PointIndices>>>(max_queue_size_);
}
if (use_indices_) {
@ -183,9 +185,12 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -198,8 +203,10 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
} else {
NODELET_DEBUG(
"[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -212,7 +219,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
if (cloud->header.frame_id != hull->header.frame_id) {
NODELET_DEBUG(
"[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.",
"[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input "
"point cloud (%s)! Using TF to transform.",
getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str());
PointCloud planar_hull;
if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) {
@ -233,7 +241,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
// 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()) {
pcl::PointIndices pcl_inliers;
moveToPCL(inliers, pcl_inliers);

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

@ -36,10 +36,10 @@
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/segmentation/sac_segmentation.hpp"
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/segmentation/sac_segmentation.hpp"
using pcl_conversions::fromPCL;
@ -61,7 +61,7 @@ pcl_ros::SACSegmentation::onInit()
NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!");
return;
}
double threshold; // unused - set via dynamic reconfigure in the callback
double threshold; // unused - set via dynamic reconfigure in the callback
if (!pnh_->getParam("distance_threshold", threshold)) {
NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
return;
@ -80,7 +80,8 @@ pcl_ros::SACSegmentation::onInit()
{
if (axis_param.size() != 3) {
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());
return;
}
@ -149,27 +150,28 @@ pcl_ros::SACSegmentation::subscribe()
// Synchronize the two topics. No need for an approximate synchronizer here, as we'll
// match the timestamps exactly
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
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_->registerCallback(
bind(
&SACSegmentation::input_indices_callback, this,
_1, _2));
}
// "latched_indices" not set, proceed with regular <input,indices> pairs
else {
} else { // "latched_indices" not set, proceed with regular <input,indices> pairs
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_ =
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_->registerCallback(
bind(
&SACSegmentation::input_indices_callback, this,
_1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
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_->registerCallback(
bind(
@ -205,7 +207,7 @@ pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32
boost::mutex::scoped_lock lock(mutex_);
if (impl_.getDistanceThreshold() != config.distance_threshold) {
//sac_->setDistanceThreshold (threshold_); - done in initSAC
// sac_->setDistanceThreshold (threshold_); - done in initSAC
impl_.setDistanceThreshold(config.distance_threshold);
NODELET_DEBUG(
"[%s::config_callback] Setting new distance to model threshold to: %f.",
@ -228,14 +230,14 @@ pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32
}
if (impl_.getMaxIterations() != config.max_iterations) {
//sac_->setMaxIterations (max_iterations_); - done in initSAC
// sac_->setMaxIterations (max_iterations_); - done in initSAC
impl_.setMaxIterations(config.max_iterations);
NODELET_DEBUG(
"[%s::config_callback] Setting new maximum number of iterations to: %d.",
getName().c_str(), config.max_iterations);
}
if (impl_.getProbability() != config.probability) {
//sac_->setProbability (probability_); - done in initSAC
// sac_->setProbability (probability_); - done in initSAC
impl_.setProbability(config.probability);
NODELET_DEBUG(
"[%s::config_callback] Setting new probability to: %f.",
@ -307,8 +309,10 @@ pcl_ros::SACSegmentation::input_indices_callback(
if (indices && !indices->header.frame_id.empty()) {
NODELET_DEBUG(
"[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -317,7 +321,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
@ -329,11 +334,13 @@ pcl_ros::SACSegmentation::input_indices_callback(
PointCloudConstPtr cloud_tf;
/* 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
// Convert the cloud into the different frame
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;
cloud_tf.reset (new PointCloud (cloud_transformed));
}
@ -348,7 +355,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
impl_.setInputCloud(pcl_ptr(cloud_tf));
impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
// 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()) {
pcl::PointIndices pcl_inliers;
pcl::ModelCoefficients pcl_model;
@ -362,7 +370,7 @@ pcl_ros::SACSegmentation::input_indices_callback(
// Probably need to transform the model of the plane here
// Check if we have enough inliers, clear inliers + model if not
if ((int)inliers.indices.size() <= min_inliers_) {
if (static_cast<int>(inliers.indices.size()) <= min_inliers_) {
inliers.indices.clear();
model.values.clear();
}
@ -371,7 +379,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
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(),
model.values.size(), pnh_->resolveName("model").c_str());
@ -405,7 +414,7 @@ pcl_ros::SACSegmentationFromNormals::onInit()
getName().c_str());
return;
}
double threshold; // unused - set via dynamic reconfigure in the callback
double threshold; // unused - set via dynamic reconfigure in the callback
if (!pnh_->getParam("distance_threshold", threshold)) {
NODELET_ERROR(
"[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!",
@ -426,7 +435,8 @@ pcl_ros::SACSegmentationFromNormals::onInit()
{
if (axis_param.size() != 3) {
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());
return;
}
@ -475,15 +485,18 @@ pcl_ros::SACSegmentationFromNormals::subscribe()
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
// Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked)
// 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);
if (approximate_sync_) {
sync_input_normals_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloudN, PointIndices>>>(max_queue_size_);
sync_input_normals_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices>>>(max_queue_size_);
} else {
sync_input_normals_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloudN, PointIndices>>>(max_queue_size_);
sync_input_normals_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices>>>(max_queue_size_);
}
// If we're supposed to look for PointIndices (indices)
@ -658,7 +671,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
return;
}
if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals"))
NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str());
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
@ -676,9 +689,12 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
if (indices && !indices->header.frame_id.empty()) {
NODELET_DEBUG(
"[%s::input_normals_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -692,8 +708,10 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
} else {
NODELET_DEBUG(
"[%s::input_normals_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@ -711,7 +729,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
if (cloud_nr_points != cloud_normals_nr_points) {
NODELET_ERROR(
"[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!",
"[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs "
"from the number of points in the normals (%d)!",
getName().c_str(), cloud_nr_points, cloud_normals_nr_points);
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
@ -728,7 +747,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
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()) {
pcl::PointIndices pcl_inliers;
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
if ((int)inliers.indices.size() <= min_inliers_) {
if (static_cast<int>(inliers.indices.size()) <= min_inliers_) {
inliers.indices.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_model_.publish(boost::make_shared<const ModelCoefficients>(model));
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(),
model.values.size(), pnh_->resolveName("model").c_str());
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 "pcl_ros/segmentation/segment_differences.hpp"
#include <pcl/io/io.h>
#include "pcl_ros/segmentation/segment_differences.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::onInit()
{
@ -57,7 +57,7 @@ pcl_ros::SegmentDifferences::onInit()
onInitPostProcess();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::subscribe()
{
@ -72,16 +72,18 @@ pcl_ros::SegmentDifferences::subscribe()
srv_->setCallback(f);
if (approximate_sync_) {
sync_input_target_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud>>>(max_queue_size_);
sync_input_target_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloud>>>(max_queue_size_);
sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_a_->registerCallback(
bind(
&SegmentDifferences::input_target_callback, this,
_1, _2));
} else {
sync_input_target_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloud>>>(max_queue_size_);
sync_input_target_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloud>>>(max_queue_size_);
sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_e_->registerCallback(
bind(
@ -90,7 +92,7 @@ pcl_ros::SegmentDifferences::subscribe()
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::unsubscribe()
{
@ -98,7 +100,7 @@ pcl_ros::SegmentDifferences::unsubscribe()
sub_target_filter_.unsubscribe();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level)
{
@ -131,8 +133,10 @@ pcl_ros::SegmentDifferences::input_target_callback(
NODELET_DEBUG(
"[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(

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

@ -36,7 +36,7 @@
*/
// Include the implementations here instead of compiling them separately to speed up compile time
//#include "extract_clusters.cpp"
//#include "extract_polygonal_prism_data.cpp"
//#include "sac_segmentation.cpp"
//#include "segment_differences.cpp"
// #include "extract_clusters.cpp"
// #include "extract_polygonal_prism_data.cpp"
// #include "sac_segmentation.cpp"
// #include "segment_differences.cpp"

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

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

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

@ -36,21 +36,24 @@
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/surface/moving_least_squares.hpp"
#include <pcl/io/io.h>
#include <vector>
#include "pcl_ros/surface/moving_least_squares.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::MovingLeastSquares::onInit()
{
PCLNodelet::onInit();
//ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
// ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
pub_output_ = advertise<PointCloudIn>(*pnh_, "output", max_queue_size_);
pub_normals_ = advertise<NormalCloudOut>(*pnh_, "normals", max_queue_size_);
//if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
// if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
if (!pnh_->getParam("search_radius", search_radius_)) {
//NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
// NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set "
// "at least one of these parameters before continuing.", getName ().c_str ());
NODELET_ERROR(
"[%s::onInit] Need a 'search_radius' parameter to be set before continuing!",
getName().c_str());
@ -93,8 +96,10 @@ pcl_ros::MovingLeastSquares::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloudIn, PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
@ -102,8 +107,10 @@ pcl_ros::MovingLeastSquares::subscribe()
&MovingLeastSquares::input_indices_callback,
this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(
@ -168,8 +175,10 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
@ -178,7 +187,8 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on "
"topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
getMTPrivateNodeHandle().resolveName("input").c_str());
@ -198,7 +208,7 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
// Initialize the spatial locator
// Do the reconstructon
//impl_.process (output);
// impl_.process (output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied

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

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

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

@ -48,7 +48,9 @@
#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
// other points than just XYZ
@ -78,7 +80,7 @@ void setStamp(ros::Time & stamp, std::uint64_t & pcl_stamp)
class Notification
{
public:
Notification(int expected_count)
explicit Notification(int expected_count)
: count_(0),
expected_count_(expected_count),
failure_count_(0)
@ -90,7 +92,7 @@ public:
++count_;
}
void failure(const PCDType::ConstPtr & message, FilterFailureReason reason)
void failure(const PCDType::ConstPtr & message, tf::FilterFailureReason reason)
{
++failure_count_;
}
@ -104,7 +106,7 @@ TEST(MessageFilter, noTransforms)
{
tf::TransformListener tf_client;
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));
PCDType::Ptr msg(new PCDType);
@ -120,7 +122,7 @@ TEST(MessageFilter, noTransformsSameFrame)
{
tf::TransformListener tf_client;
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));
PCDType::Ptr msg(new PCDType);
@ -136,7 +138,7 @@ TEST(MessageFilter, preexistingTransforms)
{
tf::TransformListener tf_client;
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));
PCDType::Ptr msg(new PCDType);
@ -161,7 +163,7 @@ TEST(MessageFilter, postTransforms)
{
tf::TransformListener tf_client;
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));
ros::Time stamp = ros::Time::now();
@ -190,7 +192,7 @@ TEST(MessageFilter, queueSize)
{
tf::TransformListener tf_client;
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.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
@ -226,7 +228,7 @@ TEST(MessageFilter, setTargetFrame)
{
tf::TransformListener tf_client;
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.setTargetFrame("frame1000");
@ -252,7 +254,7 @@ TEST(MessageFilter, multipleTargetFrames)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "", 1);
tf::MessageFilter<PCDType> filter(tf_client, "", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
std::vector<std::string> target_frames;
@ -277,9 +279,9 @@ TEST(MessageFilter, multipleTargetFrames)
ros::WallDuration(0.1).sleep();
ros::spinOnce();
EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
//ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
// ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
transform.child_frame_id_ = "frame2";
tf_client.setTransform(transform);
@ -295,7 +297,7 @@ TEST(MessageFilter, tolerance)
ros::Duration offset(0.2);
tf::TransformListener tf_client;
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.setTolerance(offset);
@ -314,9 +316,9 @@ TEST(MessageFilter, tolerance)
msg->header.stamp = pcl_stamp;
filter.add(msg);
EXPECT_EQ(0, n.count_); //No return due to lack of space for offset
EXPECT_EQ(0, n.count_); // No return due to lack of space for offset
//ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
// ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
transform.stamp_ += offset * 1.1;
tf_client.setTransform(transform);
@ -324,7 +326,7 @@ TEST(MessageFilter, tolerance)
ros::WallDuration(0.1).sleep();
ros::spinOnce();
EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
stamp += offset;
setStamp(stamp, pcl_stamp);
@ -332,14 +334,14 @@ TEST(MessageFilter, tolerance)
filter.add(msg);
EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
}
TEST(MessageFilter, outTheBackFailure)
{
tf::TransformListener tf_client;
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));
ros::Time stamp = ros::Time::now();
@ -366,7 +368,7 @@ TEST(MessageFilter, emptyFrameIDFailure)
{
tf::TransformListener tf_client;
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));
PCDType::Ptr msg(new PCDType);

24
pcl_ros/src/transforms.cpp
View File

@ -38,6 +38,8 @@
#include <pcl/common/io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <limits>
#include <string>
#include "pcl_ros/transforms.hpp"
#include "pcl_ros/impl/transforms.hpp"
@ -142,21 +144,24 @@ transformPointCloud(
in.fields[z_idx].offset, 0);
for (size_t i = 0; i < in.width * in.height; ++i) {
Eigen::Vector4f pt(*(float *)&in.data[xyz_offset[0]], *(float *)&in.data[xyz_offset[1]],
*(float *)&in.data[xyz_offset[2]], 1);
Eigen::Vector4f pt(*reinterpret_cast<float *>(&in.data[xyz_offset[0]]),
*reinterpret_cast<float *>(&in.data[xyz_offset[1]]),
*reinterpret_cast<float *>(&in.data[xyz_offset[2]], 1));
Eigen::Vector4f pt_out;
bool max_range_point = false;
int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset;
float * distance_ptr = (dist_idx < 0 ? NULL : (float *)(&in.data[distance_ptr_offset]));
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 (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
pt_out = pt;
} else { // max range point
} else { // max range point
pt[0] = *distance_ptr; // Replace x with the x value saved in distance
pt_out = transform * pt;
max_range_point = true;
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
// std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<",
// "<<pt_out[1]<<","<<pt_out[2]<<"\n";
}
} else {
pt_out = transform * pt;
@ -164,7 +169,7 @@ transformPointCloud(
if (max_range_point) {
// 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();
}
@ -181,7 +186,8 @@ transformPointCloud(
if (vp_idx != -1) {
// Transform the viewpoint info too
for (size_t i = 0; i < out.width * out.height; ++i) {
float * pstep = (float *)&out.data[i * out.point_step + out.fields[vp_idx].offset];
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
Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1);
Eigen::Vector4f vp_out = transform * vp_in;
@ -212,7 +218,7 @@ transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat)
out_mat(2, 3) = origin.z();
}
} // namespace pcl_ros
} // namespace pcl_ros
//////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(

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/view.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/transforms.hpp"
#include <tf/transform_listener.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 PointCloud::Ptr PointCloudPtr;

5
pcl_ros/tools/convert_pcd_to_image.cpp
View File

@ -53,6 +53,7 @@
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
/* ---[ */
int
@ -72,12 +73,12 @@ main(int argc, char ** argv)
pcl::io::loadPCDFile(std::string(argv[1]), cloud);
try {
pcl::toROSMsg(cloud, image); //convert the cloud
pcl::toROSMsg(cloud, image); // convert the cloud
} catch (std::runtime_error & e) {
ROS_ERROR_STREAM(
"Error in converting cloud to image message: " <<
e.what());
return 1; //fail!
return 1; // fail!
}
ros::Rate loop_rate(5);
while (nh.ok()) {

28
pcl_ros/tools/convert_pointcloud_to_image.cpp
View File

@ -40,14 +40,14 @@
**/
// ROS core
#include <ros/ros.h>
//Image message
// Image message
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
//pcl::toROSMsg
// pcl::toROSMsg
#include <pcl/io/pcd_io.h>
//conversions from PCL custom types
// conversions from PCL custom types
#include <pcl_conversions/pcl_conversions.h>
//stl stuff
// stl stuff
#include <string>
class PointCloudToImage
@ -61,9 +61,9 @@ public:
return;
}
try {
pcl::toROSMsg(*cloud, image_); //convert the cloud
pcl::toROSMsg(*cloud, image_); // convert the cloud
image_.header = cloud->header;
image_pub_.publish(image_); //publish our cloud image
image_pub_.publish(image_); // publish our cloud image
} catch (std::runtime_error & e) {
ROS_ERROR_STREAM(
"Error in converting cloud to image message: " <<
@ -78,7 +78,7 @@ public:
&PointCloudToImage::cloud_cb, this);
image_pub_ = nh_.advertise<sensor_msgs::Image>(image_topic_, 30);
//print some info about the node
// print some info about the node
std::string r_ct = nh_.resolveName(cloud_topic_);
std::string r_it = nh_.resolveName(image_topic_);
ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct);
@ -87,18 +87,18 @@ public:
private:
ros::NodeHandle nh_;
sensor_msgs::Image image_; //cache the image message
std::string cloud_topic_; //default input
std::string image_topic_; //default output
ros::Subscriber sub_; //cloud subscriber
ros::Publisher image_pub_; //image message publisher
sensor_msgs::Image image_; // cache the image message
std::string cloud_topic_; // default input
std::string image_topic_; // default output
ros::Subscriber sub_; // cloud subscriber
ros::Publisher image_pub_; // image message publisher
};
int
main(int argc, char ** argv)
{
ros::init(argc, argv, "convert_pointcloud_to_image");
PointCloudToImage pci; //this loads up the node
ros::spin(); //where she stops nobody knows
PointCloudToImage pci; // this loads up the node
ros::spin(); // where she stops nobody knows
return 0;
}

21
pcl_ros/tools/pcd_to_pointcloud.cpp
View File

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

6
pcl_ros/tools/pointcloud_to_pcd.cpp
View File

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