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 ca7b0c598a37e..63aad08fe7611 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 42ac59f4e62b8..613cfd2b177e1 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 67c7b9cd28ea2..53edda0852cf6 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 e272fc6d4c9d8..6f2b2f90ab1e1 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 a86a07cd73958..e16f1230a9608 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 a3f216fef5952..ab51a71627567 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 9ab4d0f898468..90e2c1170053b 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 @@ -84,7 +84,7 @@ void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback( stop_watch_ptr_->toc("processing_time", true); 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 502b4a878965b..447e39d015b1e 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 @@ -250,7 +250,7 @@ void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud stop_watch_ptr_->toc("processing_time", true); 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 b9d001b5dc439..d7997e03f03b8 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 3baa4f07af928..37478b5aa8d6b 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 c0289a4ffde6f..76001ff1a2d81 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 299829a41dd52..4a1728ff4a26d 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 cbf4630e201e4..99cc683cc3815 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 682fc97ef6c51..5326fd95b82b4 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 6e46f2a376bf5..382a8155ab5a1 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 cbef2564920ac..1eb1d5cff1968 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 8ec423cd9be98..5befbd2b11455 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 a59d8b8b8bbb5..45aeedabe01cb 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 a452512e36bdf..51410771edb9f 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 c471f0ab71510..df79596b9212f 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 50650254028e5..afeec5881b503 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 b3888ace507e7..2d2dc632fcf79 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 1222f138c5b62..6e7a9b3454e74 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 fac15a18e2962..77cfbe4610bf1 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 @@ -161,7 +161,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 66e2c089a934f..942d632a01447 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 b8513d5b040b7..3176edec034ae 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 a665f82747d2c..70dcdcab1c2e6 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 bdb7a0bc705dc..c5a30fba37156 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 98f4333de3a44..7ebf80c2da958 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 d0d79907eb1dd..93e3cbff7a808 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -128,6 +128,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()); @@ -272,9 +273,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 0c8abb5f7dba9..dbbf60c4442f9 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 53f2bf45e1046..19c36a1bdb1ce 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 b0697ca106533..f4e688d8b298f 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 05f7514c1e408..f312b6c0b8505 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 e34cdba64b929..5fafa7383b54e 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 35e8dfdec7d3d..678da0fbe6b86 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 266aa13b06bbb..5336efb7d81e9 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 94cdc5e2259bf..ca4f0d6ccd2be 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 59ce02d8439a0..1b39215bc175f 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 e0f92c159903d..08709c4126d51 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -145,7 +145,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]); @@ -371,11 +371,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 3e31e16309b7c..3a512eb8f83ad 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(