From 974cee710d822e050011dced79b6d48401cd75ab Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 28 Apr 2022 03:08:33 +0900 Subject: [PATCH] fix: modify build error of point cloud filters in rolling (#804) * fix(pointcloud_preprocessor): modify build error in rolling Signed-off-by: wep21 * fix(ground_segmentation): modify build error in rolling Signed-off-by: wep21 * fix(compare_map_segmentation): modify build error in rolling Signed-off-by: wep21 * fix(occupancy_grid_map_outlier_filter): modify build error in rolling Signed-off-by: wep21 * fix(euclidean_cluster): modify build error in rolling Signed-off-by: wep21 * fix(detection_by_tracker): modify build error in rolling Signed-off-by: wep21 * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../compare_elevation_map_filter_node.hpp | 2 +- .../distance_based_compare_map_filter_nodelet.hpp | 2 +- ...l_based_approximate_compare_map_filter_nodelet.hpp | 2 +- .../voxel_based_compare_map_filter_nodelet.hpp | 2 +- ...oxel_distance_based_compare_map_filter_nodelet.hpp | 2 +- .../src/distance_based_compare_map_filter_nodelet.cpp | 2 +- ...l_based_approximate_compare_map_filter_nodelet.cpp | 2 +- .../src/voxel_based_compare_map_filter_nodelet.cpp | 2 +- ...oxel_distance_based_compare_map_filter_nodelet.cpp | 2 +- perception/detection_by_tracker/CMakeLists.txt | 2 +- .../include/detection_by_tracker/debugger.hpp | 6 ++++++ .../detection_by_tracker_core.hpp | 6 ++++++ perception/euclidean_cluster/CMakeLists.txt | 2 +- perception/ground_segmentation/CMakeLists.txt | 11 ++++++----- .../ransac_ground_filter_nodelet.hpp | 6 ++++++ .../ground_segmentation/ray_ground_filter_nodelet.hpp | 6 ++++++ .../scan_ground_filter_nodelet.hpp | 6 ++++++ .../src/ransac_ground_filter_nodelet.cpp | 3 +-- .../src/ray_ground_filter_nodelet.cpp | 2 +- .../occupancy_grid_map_outlier_filter_nodelet.hpp | 1 + .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 7 ++++++- sensing/pointcloud_preprocessor/CMakeLists.txt | 7 +++++++ .../blockage_diag/blockage_diag_nodelet.hpp | 2 +- .../concatenate_data/concatenate_data_nodelet.hpp | 3 ++- .../crop_box_filter/crop_box_filter_nodelet.hpp | 2 +- .../distortion_corrector/distortion_corrector.hpp | 6 ++++++ .../approximate_downsample_filter_nodelet.hpp | 2 +- .../random_downsample_filter_nodelet.hpp | 2 +- .../voxel_grid_downsample_filter_nodelet.hpp | 2 +- .../include/pointcloud_preprocessor/filter.hpp | 4 +--- .../dual_return_outlier_filter_nodelet.hpp | 2 +- .../radius_search_2d_outlier_filter_nodelet.hpp | 2 +- .../outlier_filter/ring_outlier_filter_nodelet.hpp | 1 + .../voxel_grid_outlier_filter_nodelet.hpp | 2 +- .../passthrough_filter/passthrough_filter_nodelet.hpp | 2 +- .../passthrough_filter_uint16_nodelet.hpp | 2 +- .../passthrough_filter/passthrough_uint16.hpp | 4 ++-- .../pointcloud_accumulator_nodelet.hpp | 2 +- .../vector_map_filter/lanelet2_map_filter_nodelet.hpp | 6 ++++++ .../src/concatenate_data/concatenate_data_nodelet.cpp | 7 ++++--- .../radius_search_2d_outlier_filter_nodelet.cpp | 2 +- 41 files changed, 97 insertions(+), 41 deletions(-) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp index ca7b0c598a37..63aad08fe761 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp @@ -53,7 +53,7 @@ class CompareElevationMapFilterComponent : public pointcloud_preprocessor::Filte void elevationMapCallback(const grid_map_msgs::msg::GridMap::ConstSharedPtr elevation_map); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit CompareElevationMapFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace compare_map_segmentation diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 42ac59f4e62b..613cfd2b177e 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -45,7 +45,7 @@ class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::F rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit DistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace compare_map_segmentation diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp index 67c7b9cd28ea..53edda0852cf 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp @@ -47,7 +47,7 @@ class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preproc rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelBasedApproximateCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace compare_map_segmentation diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp index e272fc6d4c9d..6f2b2f90ab1e 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp @@ -51,7 +51,7 @@ class VoxelBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filt rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace compare_map_segmentation diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index a86a07cd7395..e16f1230a960 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -49,7 +49,7 @@ class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocess rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelDistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace compare_map_segmentation diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index a3f216fef595..ab51a7162756 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -63,7 +63,7 @@ void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCl { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); map_ptr_ = map_pcl_ptr; diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 78bdf191f3d8..70f90bed924a 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -72,7 +72,7 @@ void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback( { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); set_map_in_voxel_grid_ = true; diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 7afce3d35bc6..3c206f7cec7b 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -239,7 +239,7 @@ void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); set_map_in_voxel_grid_ = true; diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index b9d001b5dc43..d7997e03f03b 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -74,7 +74,7 @@ void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback( { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); tf_input_frame_ = map_pcl_ptr->header.frame_id; diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt index 3baa4f07af92..37478b5aa8d6 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -20,7 +20,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ### Find PCL Dependencies -find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation) +find_package(PCL REQUIRED) ### Find Eigen Dependencies find_package(eigen3_cmake_module REQUIRED) diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index c0289a4ffde6..76001ff1a2d8 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -31,7 +31,13 @@ #include #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif + #include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 299829a41dd5..4a1728ff4a26 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -33,7 +33,13 @@ #include #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif + #include #include diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index cbf4630e201e..99cc683cc381 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -9,7 +9,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_auto REQUIRED) -find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation) +find_package(PCL REQUIRED) ament_auto_find_build_dependencies() diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index 682fc97ef6c5..5326fd95b82b 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -32,11 +32,12 @@ ament_auto_find_build_dependencies() include_directories( include - ${Boost_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${GRID_MAP_INCLUDE_DIR} + SYSTEM + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${GRID_MAP_INCLUDE_DIR} ) ament_auto_add_library(ground_segmentation SHARED diff --git a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp index 6e46f2a376bf..382a8155ab5a 100644 --- a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp @@ -25,7 +25,13 @@ #include #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif + #include #include diff --git a/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp index cbef2564920a..1eb1d5cff196 100644 --- a/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp @@ -51,7 +51,13 @@ #include #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif + #include #include diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 8ec423cd9be9..5befbd2b1145 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -25,7 +25,13 @@ #include #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif + #include #include diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index a59d8b8b8bbb..45aeedabe01c 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -187,7 +186,7 @@ void RANSACGroundFilterComponent::extractPointsIndices( { pcl::ExtractIndices extract_ground; extract_ground.setInputCloud(in_cloud_ptr); - extract_ground.setIndices(boost::make_shared(in_indices)); + extract_ground.setIndices(pcl::make_shared(in_indices)); extract_ground.setNegative(false); // true removes the indices, false leaves only the indices extract_ground.filter(*out_only_indices_cloud_ptr); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index a452512e36bd..51410771edb9 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -297,7 +297,7 @@ void RayGroundFilterComponent::ExtractPointsIndices( { pcl::ExtractIndices extract_ground; extract_ground.setInputCloud(in_cloud_ptr); - extract_ground.setIndices(boost::make_shared(in_indices)); + extract_ground.setIndices(pcl::make_shared(in_indices)); extract_ground.setNegative(false); // true removes the indices, false leaves only the indices extract_ground.filter(*out_only_indices_cloud_ptr); diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 4ae0fde705e1..a79d5bb35a57 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 8b47cee94313..2c9ea8cfc8a5 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -21,7 +21,12 @@ #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif #include #include @@ -103,7 +108,7 @@ RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node) node.declare_parameter("radius_search_2d_filter.min_points_and_distance_ratio", 400.0f); min_points_ = node.declare_parameter("radius_search_2d_filter.min_points", 4); max_points_ = node.declare_parameter("radius_search_2d_filter.max_points", 70); - kd_tree_ = boost::make_shared>(false); + kd_tree_ = pcl::make_shared>(false); } void RadiusSearch2dfilter::filter( diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index b3888ace507e..2d2dc632fcf7 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -73,6 +73,13 @@ if(OPENMP_FOUND) ) endif() +# workaround to allow deprecated header to build on both galactic and rolling +if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) + target_compile_definitions(pointcloud_preprocessor_filter PRIVATE + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) +endif() + # ========== Concatenate data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 1222f138c5b6..6e7a9b3454e7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -71,7 +71,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter std::string lidar_model_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit BlockageDiagComponent(const rclcpp::NodeOptions & options); }; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp index 7083ab14e99d..01f125d003f4 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -159,7 +159,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); void cloud_callback( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); void timer_callback(); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp index 66e2c089a934..942d632a0144 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -92,7 +92,7 @@ class CropBoxFilterComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit CropBoxFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index d121fdbf269f..5b597bbbc380 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -23,7 +23,13 @@ #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif + #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp index a665f82747d2..70dcdcab1c2e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp @@ -77,7 +77,7 @@ class ApproximateDownsampleFilterComponent : public pointcloud_preprocessor::Fil double voxel_size_z_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit ApproximateDownsampleFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp index bdb7a0bc705d..c5a30fba3715 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp @@ -75,7 +75,7 @@ class RandomDownsampleFilterComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RandomDownsampleFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp index 98f4333de3a4..7ebf80c2da95 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp @@ -78,7 +78,7 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelGridDownsampleFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index 194f58a6fe7e..1b53b4dfc0ae 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -124,6 +124,7 @@ class Filter : public rclcpp::Node using ApproximateTimeSyncPolicy = message_filters::Synchronizer>; + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit Filter( const std::string & filter_name = "pointcloud_preprocessor_filter", const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); @@ -264,9 +265,6 @@ class Filter : public rclcpp::Node void input_indices_callback(const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices); void setupTF(); - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp index 0c8abb5f7dba..dbbf60c4442f 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp @@ -84,7 +84,7 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter float max_distance_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit DualReturnOutlierFilterComponent(const rclcpp::NodeOptions & options); }; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp index 53f2bf45e104..19c36a1bdb1c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp @@ -49,7 +49,7 @@ class RadiusSearch2DOutlierFilterComponent : public pointcloud_preprocessor::Fil rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RadiusSearch2DOutlierFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index b0697ca10653..f4e688d8b298 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -61,6 +61,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter } public: + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); }; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp index 05f7514c1e40..f312b6c0b850 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp @@ -45,7 +45,7 @@ class VoxelGridOutlierFilterComponent : public pointcloud_preprocessor::Filter pcl::VoxelGrid voxel_filter; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelGridOutlierFilterComponent(const rclcpp::NodeOptions & option); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp index e34cdba64b92..5fafa7383b54 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp @@ -72,7 +72,7 @@ class PassThroughFilterComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PassThroughFilterComponent(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp index 35e8dfdec7d3..678da0fbe6b8 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp @@ -40,7 +40,7 @@ class PassThroughFilterUInt16Component : public pointcloud_preprocessor::Filter pcl::PassThroughUInt16 impl_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PassThroughFilterUInt16Component(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp index 266aa13b06bb..5336efb7d81e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp @@ -100,8 +100,8 @@ class PassThroughUInt16 : public FilterIndices typedef typename pcl::traits::fieldList::type FieldList; public: - typedef boost::shared_ptr> Ptr; - typedef boost::shared_ptr> ConstPtr; + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> ConstPtr; /** \brief Constructor. * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp index 94cdc5e2259b..ca4f0d6ccd2b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp @@ -40,7 +40,7 @@ class PointcloudAccumulatorComponent : public pointcloud_preprocessor::Filter boost::circular_buffer pointcloud_buffer_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PointcloudAccumulatorComponent(const rclcpp::NodeOptions & options); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index 59ce02d8439a..1b39215bc175 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -25,7 +25,13 @@ #include #include + +#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER #include +#else +#include +#endif + #include #include diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index 8808cf3db0d8..3ef1be0a66f1 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -135,7 +135,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro cloud_stdmap_tmp_ = cloud_stdmap_; // CAN'T use auto type here. - std::function cb = std::bind( + std::function cb = std::bind( &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this, std::placeholders::_1, input_topics_[d]); @@ -351,11 +351,12 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new } void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name) + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { std::lock_guard lock(mutex_); + auto input = std::make_shared(*input_ptr); sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); - convertToXYZICloud(input_ptr, xyzi_input_ptr); + convertToXYZICloud(input, xyzi_input_ptr); const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp index 3e31e16309b7..3a512eb8f83a 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp @@ -32,7 +32,7 @@ RadiusSearch2DOutlierFilterComponent::RadiusSearch2DOutlierFilterComponent( search_radius_ = static_cast(declare_parameter("search_radius", 0.2)); } - kd_tree_ = boost::make_shared>(false); + kd_tree_ = pcl::make_shared>(false); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback(