diff --git a/sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml deleted file mode 100644 index 75503a3caf159..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - blockage_ratio_threshold: 0.1 - blockage_count_threshold: 50 - blockage_buffering_frames: 2 - blockage_buffering_interval: 1 - enable_dust_diag: false - publish_debug_image: false - dust_ratio_threshold: 0.2 - dust_count_threshold: 10 - dust_kernel_size: 2 - dust_buffering_frames: 10 - dust_buffering_interval: 1 - max_distance_range: 200.0 - horizontal_resolution: 0.4 - blockage_kernel: 10 - angle_range: [0.0, 360.0] - vertical_bins: 40 - is_channel_order_top2down: true - horizontal_ring_id: 18 diff --git a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector.param.yaml deleted file mode 100644 index 7d055c5bee45f..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - ## Paramters in distortion_corrector.cpp - time_stamp_field_name: "time_stamp" - use_imu: true diff --git a/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md index f01833651d8c0..a7e9ec3114882 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md @@ -51,26 +51,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ## Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/blockage_diag.schema.json") }} - -| Name | Type | Description | -| ----------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- | -| `blockage_ratio_threshold` | float | The threshold of blockage area ratio.If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR. | -| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | -| `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | -| `angle_range` | vector | The effective range of LiDAR | -| `vertical_bins` | int | The LiDAR channel number | -| `is_channel_order_top2down` | bool | If the lidar channels are indexed from top to down | -| `blockage_buffering_frames` | int | The number of buffering about blockage detection [range:1-200] | -| `blockage_buffering_interval` | int | The interval of buffering about blockage detection | -| `dust_ratio_threshold` | float | The threshold of dusty area ratio | -| `dust_count_threshold` | int | The threshold of number continuous frames include dusty area | -| `blockage_kernel` | int | The kernel size of morphology processing the detected blockage area | -| `dust_kernel_size` | int | The kernel size of morphology processing in dusty area detection | -| `dust_buffering_frames` | int | The number of buffering about dusty area detection [range:1-200] | -| `dust_buffering_interval` | int | The interval of buffering about dusty area detection | -| `max_distance_range` | double | Maximum view range for the LiDAR | -| `horizontal_resolution` | double | The horizontal resolution of depth map image [deg/pixel] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/blockage_diag.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/crop-box-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/crop-box-filter.md index 6b6fa0fe8b219..22778d79839bb 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/crop-box-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/crop-box-filter.md @@ -20,16 +20,7 @@ This implementation inherit `autoware::pointcloud_preprocessor::Filter` class, p ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/crop_box_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| ------- | ------ | ------------- | ----------------------------------------- | -| `min_x` | double | -1.0 | x-coordinate minimum value for crop range | -| `max_x` | double | 1.0 | x-coordinate maximum value for crop range | -| `min_y` | double | -1.0 | y-coordinate minimum value for crop range | -| `max_y` | double | 1.0 | y-coordinate maximum value for crop range | -| `min_z` | double | -1.0 | z-coordinate minimum value for crop range | -| `max_z` | double | 1.0 | z-coordinate maximum value for crop range | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/crop_box_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md index 3ec48b61940a3..ab5a07b5279bc 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md @@ -36,20 +36,12 @@ Please note that the processing time difference between the two distortion metho ### Core Parameters -<<<<<<< HEAD -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json") }} -======= {{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json") }} ->>>>>>> original/main ## Launch ```bash -<<<<<<< HEAD -ros2 launch pointcloud_preprocessor distortion_corrector.launch.xml -======= ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml ->>>>>>> original/main ``` ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index 2b80039cf87d4..6c1204f013987 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -36,41 +36,19 @@ These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, #### Approximate Downsample Filter -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/approximate_downsample_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ---------------- | -| `voxel_size_x` | double | 0.3 | voxel size x [m] | -| `voxel_size_y` | double | 0.3 | voxel size y [m] | -| `voxel_size_z` | double | 0.1 | voxel size z [m] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter.schema.json") }} ### Random Downsample Filter -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/random_downsample_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| ------------ | ---- | ------------- | ------------------------------- | -| `sample_num` | int | 1500 | number of indices to be sampled | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/random_downsample_filter.schema.json") }} ### Voxel Grid Downsample Filter -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/voxel_grid_downsample_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ---------------- | -| `voxel_size_x` | double | 0.3 | voxel size x [m] | -| `voxel_size_y` | double | 0.3 | voxel size y [m] | -| `voxel_size_z` | double | 0.1 | voxel size z [m] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_downsample_filter.schema.json") }} ### Pickup Based Voxel Grid Downsample Filter -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/pickup_based_voxel_grid_downsample_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ---------------- | -| `voxel_size_x` | double | 1.0 | voxel size x [m] | -| `voxel_size_y` | double | 1.0 | voxel size y [m] | -| `voxel_size_z` | double | 1.0 | voxel size z [m] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/pickup_based_voxel_grid_downsample_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 59661d71220ca..2a7d28ade0559 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -46,35 +46,11 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Node Parameters -<<<<<<< HEAD -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/dual_return_outlier_filter.schema.json") }} - -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). -======= This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ->>>>>>> original/main ### Core Parameters -| Name | Type | Description | -| ---------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `vertical_bins` | int | The number of vertical bin for visibility histogram | -| `max_azimuth_diff` | float | Threshold for ring_outlier_filter | -| `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter | -| `general_distance_ratio` | double | Threshold for ring_outlier_filter | -| `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | -| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | -| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | -| `roi_mode` | string | The name of ROI mode for switching | -| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode | -| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode | -| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode | -| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode | -| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode | -| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode | -| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md index 6b9e129021e75..1b54ecad8a1be 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md @@ -25,15 +25,7 @@ The `passthrough_filter` is a node that removes points on the outside of a range ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/passthrough_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| ----------------------- | ------ | ------------- | ------------------------------------------------------ | -| `filter_limit_min` | int | 0 | minimum allowed field value | -| `filter_limit_max` | int | 127 | maximum allowed field value | -| `filter_field_name` | string | "ring" | filtering field name | -| `keep_organized` | bool | false | flag to keep indices structure | -| `filter_limit_negative` | bool | false | flag to return whether the data is inside limit or not | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/passthrough_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-accumulator.md b/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-accumulator.md index a76df48803cbb..7925cecba75d0 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-accumulator.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-accumulator.md @@ -24,12 +24,7 @@ The `pointcloud_accumulator` is a node that accumulates pointclouds for a given ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/pointcloud_accumulator.schema.json") }} - -| Name | Type | Default Value | Description | -| ------------------------ | ------ | ------------- | ----------------------- | -| `accumulation_time_sec` | double | 2.0 | accumulation period [s] | -| `pointcloud_buffer_size` | int | 50 | buffer size | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/pointcloud_accumulator.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md index 7ecbae3c2fe8e..17696017a1903 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md @@ -24,12 +24,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/radius_search_2d_outlier_filter.schema.json") }} - -| Name | Type | Description | -| --------------- | ------ | ------------------------------------------------------------------------------------------------------------------------ | -| `min_neighbors` | int | If points in the circle centered on reference point is less than `min_neighbors`, a reference point is judged as outlier | -| `search_radius` | double | Searching number of points included in `search_radius` | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/radius_search_2d_outlier_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md index a805a0bf397fe..2955cb47a0f81 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -56,22 +56,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/ring_outlier_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | -| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | -| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation | -| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation | -| `max_distance` | float | 12.0 | The limit distance for visibility score calculation | -| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram | -| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram | -| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md index 52623e42c7f06..61be9921bbb5c 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md @@ -23,14 +23,9 @@ The `vector_map_filter` is a node that removes points on the outside of lane by ## Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/lanelet2_map_filter.schema.json") }} - ### Core Parameters -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ----------- | -| `voxel_size_x` | double | 0.04 | voxel size | -| `voxel_size_y` | double | 0.04 | voxel size | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-inside-area-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-inside-area-filter.md index 8fe0bcfe0a902..fae421d44a29e 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-inside-area-filter.md @@ -33,12 +33,6 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/vector_map_inside_area_filter.schema.json") }} - -| Name | Type | Description | -| -------------- | ------ | --------------------------- | -| `polygon_type` | string | polygon type to be filtered | -| `use_z` | bool | use z value for filtering | -| `z_threshold` | float | z threshold for filtering | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/vector_map_inside_area_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md index d9c3f4f0679da..8d8cb0cde26df 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md @@ -23,14 +23,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/voxel_grid_outlier_filter.schema.json") }} - -| Name | Type | Default Value | Description | -| ------------------------ | ------ | ------------- | ------------------------------------------ | -| `voxel_size_x` | double | 0.3 | the voxel size along x-axis [m] | -| `voxel_size_y` | double | 0.3 | the voxel size along y-axis [m] | -| `voxel_size_z` | double | 0.1 | the voxel size along z-axis [m] | -| `voxel_points_threshold` | int | 2 | the minimum number of points in each voxel | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp deleted file mode 100644 index 18f6c3851866d..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include - -#if __has_include() -#include -#else -#include -#endif - -#include - -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using diagnostic_updater::DiagnosticStatusWrapper; -using diagnostic_updater::Updater; - -class BlockageDiagComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - image_transport::Publisher lidar_depth_map_pub_; - image_transport::Publisher blockage_mask_pub_; - image_transport::Publisher single_frame_dust_mask_pub; - image_transport::Publisher multi_frame_dust_mask_pub; - image_transport::Publisher blockage_dust_merged_pub; - rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; - rclcpp::Publisher::SharedPtr blockage_type_pub_; - -private: - void onBlockageChecker(DiagnosticStatusWrapper & stat); - void dustChecker(DiagnosticStatusWrapper & stat); - Updater updater_{this}; - int vertical_bins_; - std::vector angle_range_deg_; - int horizontal_ring_id_; - float blockage_ratio_threshold_; - float dust_ratio_threshold_; - float ground_blockage_ratio_ = -1.0f; - float sky_blockage_ratio_ = -1.0f; - float ground_dust_ratio_ = -1.0f; - std::vector ground_blockage_range_deg_ = {0.0f, 0.0f}; - std::vector sky_blockage_range_deg_ = {0.0f, 0.0f}; - int blockage_kernel_ = 10; - int blockage_frame_count_ = 0; - int ground_blockage_count_ = 0; - int sky_blockage_count_ = 0; - int blockage_count_threshold_; - bool is_channel_order_top2down_; - int blockage_buffering_frames_; - int blockage_buffering_interval_; - bool enable_dust_diag_; - bool publish_debug_image_; - int dust_kernel_size_; - int dust_buffering_frames_; - int dust_buffering_interval_; - int dust_buffering_frame_counter_ = 0; - int dust_count_threshold_; - int dust_frame_count_ = 0; - double max_distance_range_{200.0}; - double horizontal_resolution_{0.4}; - boost::circular_buffer no_return_mask_buffer{1}; - boost::circular_buffer dust_mask_buffer{1}; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit BlockageDiagComponent(const rclcpp::NodeOptions & options); -}; - -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp deleted file mode 100644 index 2443e45fe4c63..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * 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. - * - * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -// ROS includes -#include "autoware_point_types/types.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using autoware_point_types::PointXYZIRC; -using point_cloud_msg_wrapper::PointCloud2Modifier; - -/** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data - * synchronizer: it listens for a set of input PointCloud messages on the same topic, - * checks their timestamps, and concatenates their fields together into a single - * PointCloud output message. - * \author Radu Bogdan Rusu - */ -class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node -{ -public: - typedef sensor_msgs::msg::PointCloud2 PointCloud2; - - /** \brief constructor. */ - explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); - - /** \brief constructor. - * \param queue_size the maximum queue size - */ - PointCloudConcatenateDataSynchronizerComponent( - const rclcpp::NodeOptions & node_options, int queue_size); - - /** \brief Empty destructor. */ - virtual ~PointCloudConcatenateDataSynchronizerComponent() {} - -private: - /** \brief The output PointCloud publisher. */ - rclcpp::Publisher::SharedPtr pub_output_; - /** \brief Delay Compensated PointCloud publisher*/ - std::map::SharedPtr> - transformed_raw_pc_publisher_map_; - - /** \brief The maximum number of messages that we can store in the queue. */ - int maximum_queue_size_ = 3; - - double timeout_sec_ = 0.1; - - bool publish_synchronized_pointcloud_; - bool keep_input_frame_in_synchronized_pointcloud_; - std::string synchronized_pointcloud_postfix_; - - std::set not_subscribed_topic_names_; - - /** \brief A vector of subscriber. */ - std::vector::SharedPtr> filters_; - - rclcpp::Subscription::SharedPtr sub_twist_; - rclcpp::Subscription::SharedPtr sub_odom_; - - rclcpp::TimerBase::SharedPtr timer_; - diagnostic_updater::Updater updater_{this}; - - const std::string input_twist_topic_type_; - - /** \brief Output TF frame the concatenated points should be transformed to. */ - std::string output_frame_; - - /** \brief Input point cloud topics. */ - // XmlRpc::XmlRpcValue input_topics_; - std::vector input_topics_; - - /** \brief TF listener object. */ - std::shared_ptr tf2_buffer_; - std::shared_ptr tf2_listener_; - - std::deque twist_ptr_queue_; - - std::map cloud_stdmap_; - std::map cloud_stdmap_tmp_; - std::mutex mutex_; - - std::vector input_offset_; - std::map offset_map_; - - void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( - const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); - std::map combineClouds( - sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); - void publish(); - - void convertToXYZIRCCloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, - sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); - void setPeriod(const int64_t new_period); - void cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, - const std::string & topic_name); - void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); - void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); - void timer_callback(); - - void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); - std::string replaceSyncTopicNamePostfix( - const std::string & original_topic_name, const std::string & postfix); - - /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; -}; - -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp deleted file mode 100644 index 36b1910a4d798..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * 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. - * - * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -// ROS includes -#include "autoware_point_types/types.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using autoware_point_types::PointXYZIRC; -using point_cloud_msg_wrapper::PointCloud2Modifier; -/** \brief @b PointCloudConcatenationComponent is a special form of data - * synchronizer: it listens for a set of input PointCloud messages on the same topic, - * checks their timestamps, and concatenates their fields together into a single - * PointCloud output message. - * \author Radu Bogdan Rusu - */ -class PointCloudConcatenationComponent : public rclcpp::Node -{ -public: - typedef sensor_msgs::msg::PointCloud2 PointCloud2; - - /** \brief constructor. */ - explicit PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options); - - /** \brief constructor. - * \param queue_size the maximum queue size - */ - PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options, int queue_size); - - /** \brief Empty destructor. */ - virtual ~PointCloudConcatenationComponent() {} - -private: - /** \brief The output PointCloud publisher. */ - rclcpp::Publisher::SharedPtr pub_output_; - /** \brief Delay Compensated PointCloud publisher*/ - std::map::SharedPtr> - transformed_raw_pc_publisher_map_; - - /** \brief The maximum number of messages that we can store in the queue. */ - int maximum_queue_size_ = 3; - - double timeout_sec_ = 0.1; - - std::set not_subscribed_topic_names_; - - /** \brief A vector of subscriber. */ - std::vector::SharedPtr> filters_; - - rclcpp::Subscription::SharedPtr sub_twist_; - - rclcpp::TimerBase::SharedPtr timer_; - diagnostic_updater::Updater updater_{this}; - - /** \brief Output TF frame the concatenated points should be transformed to. */ - std::string output_frame_; - - /** \brief Input point cloud topics. */ - // XmlRpc::XmlRpcValue input_topics_; - std::vector input_topics_; - - /** \brief TF listener object. */ - std::shared_ptr tf2_buffer_; - std::shared_ptr tf2_listener_; - - std::deque twist_ptr_queue_; - - std::map cloud_stdmap_; - std::map cloud_stdmap_tmp_; - std::mutex mutex_; - - std::vector input_offset_; - std::map offset_map_; - - void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - void transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, - const std::string & target_frame); - void checkSyncStatus(); - void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); - void publish(); - - void convertToXYZIRCCloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, - sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); - void setPeriod(const int64_t new_period); - void cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, - const std::string & topic_name); - void timer_callback(); - - void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); - - /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; -}; - -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp deleted file mode 100644 index 09b5c7fd5f5fa..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2010, 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. - * - * $Id: cropbox.cpp - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ - -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/transform_info.hpp" - -#include - -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class CropBoxFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - - // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform - // to new API - virtual void faster_filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, - const TransformInfo & transform_info); - - void publishCropBoxPolygon(); - -private: - struct CropBoxParam - { - float min_x; - float max_x; - float min_y; - float max_y; - float min_z; - float max_z; - bool negative{false}; - } param_; - - rclcpp::Publisher::SharedPtr crop_box_polygon_pub_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit CropBoxFilterComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp deleted file mode 100644 index e37329a9a4cc3..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ /dev/null @@ -1,175 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ -#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include - -#include -#include -#include - -namespace pointcloud_preprocessor -{ - -class DistortionCorrectorBase -{ -public: - virtual bool pointcloud_transform_exists() = 0; - virtual bool pointcloud_transform_needed() = 0; - virtual std::deque get_twist_queue() = 0; - virtual std::deque get_angular_velocity_queue() = 0; - - virtual void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; - virtual void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; - virtual void setPointCloudTransform( - const std::string & base_frame, const std::string & lidar_frame) = 0; - virtual void initialize() = 0; - virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; -}; - -template -class DistortionCorrector : public DistortionCorrectorBase -{ -protected: - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_; - bool pointcloud_transform_needed_{false}; - bool pointcloud_transform_exists_{false}; - bool imu_transform_exists_{false}; - rclcpp::Node * node_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - std::deque twist_queue_; - std::deque angular_velocity_queue_; - - void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame); - void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); - void getTwistAndIMUIterator( - bool use_imu, double first_point_time_stamp_sec, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu); - void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); - void undistortPoint( - sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, - sensor_msgs::PointCloud2Iterator & it_z, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu, float const & time_offset, - const bool & is_twist_valid, const bool & is_imu_valid) - { - static_cast(this)->undistortPointImplementation( - it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - }; - -public: - explicit DistortionCorrector(rclcpp::Node * node) - : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) - { - } - bool pointcloud_transform_exists(); - bool pointcloud_transform_needed(); - std::deque get_twist_queue(); - std::deque get_angular_velocity_queue(); - void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; - - void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; - void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; - bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); -}; - -class DistortionCorrector2D : public DistortionCorrector -{ -private: - // defined outside of for loop for performance reasons. - tf2::Quaternion baselink_quat_; - tf2::Transform baselink_tf_odom_; - tf2::Vector3 point_tf_; - tf2::Vector3 undistorted_point_tf_; - float theta_; - float x_; - float y_; - - // TF - tf2::Transform tf2_lidar_to_base_link_; - tf2::Transform tf2_base_link_to_lidar_; - -public: - explicit DistortionCorrector2D(rclcpp::Node * node) : DistortionCorrector(node) {} - void initialize() override; - void undistortPointImplementation( - sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, - sensor_msgs::PointCloud2Iterator & it_z, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu, const float & time_offset, - const bool & is_twist_valid, const bool & is_imu_valid); - - void setPointCloudTransform( - const std::string & base_frame, const std::string & lidar_frame) override; -}; - -class DistortionCorrector3D : public DistortionCorrector -{ -private: - // defined outside of for loop for performance reasons. - Eigen::Vector4f point_eigen_; - Eigen::Vector4f undistorted_point_eigen_; - Eigen::Matrix4f transformation_matrix_; - Eigen::Matrix4f prev_transformation_matrix_; - - // TF - Eigen::Matrix4f eigen_lidar_to_base_link_; - Eigen::Matrix4f eigen_base_link_to_lidar_; - -public: - explicit DistortionCorrector3D(rclcpp::Node * node) : DistortionCorrector(node) {} - void initialize() override; - void undistortPointImplementation( - sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, - sensor_msgs::PointCloud2Iterator & it_z, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu, const float & time_offset, - const bool & is_twist_valid, const bool & is_imu_valid); - void setPointCloudTransform( - const std::string & base_frame, const std::string & lidar_frame) override; -}; - -} // namespace pointcloud_preprocessor - -#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp deleted file mode 100644 index 78cdb80838678..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp -#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ -#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ - -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -======== -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ - -#include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using rcl_interfaces::msg::SetParametersResult; -using sensor_msgs::msg::PointCloud2; - -class DistortionCorrectorComponent : public rclcpp::Node -{ -public: - explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); - -private: - rclcpp::Subscription::SharedPtr twist_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr pointcloud_sub_; - - rclcpp::Publisher::SharedPtr undistorted_pointcloud_pub_; - - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - - std::string base_frame_; - bool use_imu_; - bool use_3d_distortion_correction_; - - std::unique_ptr distortion_corrector_; - - void onPointCloud(PointCloud2::UniquePtr points_msg); - void onTwist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); - void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); -}; - -} // namespace autoware::pointcloud_preprocessor - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp -#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ -======== -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp deleted file mode 100644 index d5d2750442a2f..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/* - * 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. - * - * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ - * - */ -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include - -#include -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit ApproximateDownsampleFilterComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp deleted file mode 100644 index 89d6ea51a7283..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include "autoware/pointcloud_preprocessor/transform_info.hpp" - -#include -#include -#include - -#include -#include - -namespace autoware::pointcloud_preprocessor -{ - -class FasterVoxelGridDownsampleFilter -{ - using PointCloud2 = sensor_msgs::msg::PointCloud2; - using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; - -public: - FasterVoxelGridDownsampleFilter(); - void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); - void set_field_offsets(const PointCloud2ConstPtr & input, const rclcpp::Logger & logger); - void filter( - const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, - const rclcpp::Logger & logger); - -private: - struct Centroid - { - float x; - float y; - float z; - float intensity; - uint32_t point_count_; - - Centroid() : x(0), y(0), z(0), intensity(0) {} - Centroid(float _x, float _y, float _z, float _intensity) - : x(_x), y(_y), z(_z), intensity(_intensity) - { - this->point_count_ = 1; - } - - void add_point(float _x, float _y, float _z, float _intensity) - { - this->x += _x; - this->y += _y; - this->z += _z; - this->intensity += _intensity; - this->point_count_++; - } - - Eigen::Vector4f calc_centroid() const - { - Eigen::Vector4f centroid( - (this->x / this->point_count_), (this->y / this->point_count_), - (this->z / this->point_count_), (this->intensity / this->point_count_)); - return centroid; - } - }; - - Eigen::Vector3f inverse_voxel_size_; - int x_offset_; - int y_offset_; - int z_offset_; - int intensity_index_; - int intensity_offset_; - bool offset_initialized_; - - Eigen::Vector4f get_point_from_global_offset( - const PointCloud2ConstPtr & input, size_t global_offset); - - bool get_min_max_voxel( - const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel); - - std::unordered_map calc_centroids_each_voxel( - const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, - const Eigen::Vector3i & min_voxel); - - void copy_centroids_to_output( - std::unordered_map & voxel_centroid_map, PointCloud2 & output, - const TransformInfo & transform_info); -}; - -} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp deleted file mode 100644 index 9a1f2780be11b..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ // NOLINT - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include - -#include -#include - -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -/** - * @class PickupBasedVoxelGridDownsampleFilterComponent - * @brief A filter component for downsampling point clouds using a voxel grid approach. - * - * This component reduces the number of points in a point cloud by grouping them into voxels - * and picking a representative point for each voxel. It's useful for reducing computational - * load when processing large point clouds. - */ -class PickupBasedVoxelGridDownsampleFilterComponent -: public autoware::pointcloud_preprocessor::Filter -{ -protected: - void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - -private: - float voxel_size_x_; ///< The size of the voxel in the x dimension. - float voxel_size_y_; ///< The size of the voxel in the y dimension. - float voxel_size_z_; ///< The size of the voxel in the z dimension. - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit PickupBasedVoxelGridDownsampleFilterComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp deleted file mode 100644 index f820793fb86f8..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * 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. - * - * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class RandomDownsampleFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - -private: - size_t sample_num_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit RandomDownsampleFilterComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp deleted file mode 100644 index 1af2eb5b04552..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * 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. - * - * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT - -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/transform_info.hpp" - -#include -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class VoxelGridDownsampleFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - - // TODO(atsushi421): Temporary Implementation: Remove this interface when all the filter nodes - // conform to new API - virtual void faster_filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, - const TransformInfo & transform_info); - -private: - float voxel_size_x_; - float voxel_size_y_; - float voxel_size_z_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit VoxelGridDownsampleFilterComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp deleted file mode 100644 index a6e113412231a..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ /dev/null @@ -1,307 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * 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. - * - * $Id: filter.h 35876 2011-02-09 01:04:36Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__FILTER_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__FILTER_HPP_ - -#include "autoware/pointcloud_preprocessor/transform_info.hpp" - -#include -#include -#include - -// PCL includes -#include - -#include -#include -// PCL includes -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Include TF -#include -#include -#include - -// Include tier4 autoware utils -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -namespace sync_policies = message_filters::sync_policies; - -/** \brief For parameter service callback */ -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/** \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 rclcpp::Node -{ -public: - using PointCloud2 = sensor_msgs::msg::PointCloud2; - using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; - - using PointCloud = pcl::PointCloud; - using PointCloudPtr = PointCloud::Ptr; - using PointCloudConstPtr = PointCloud::ConstPtr; - - using PointIndices = pcl_msgs::msg::PointIndices; - using PointIndicesPtr = PointIndices::SharedPtr; - using PointIndicesConstPtr = PointIndices::ConstSharedPtr; - - using ModelCoefficients = pcl_msgs::msg::ModelCoefficients; - using ModelCoefficientsPtr = ModelCoefficients::SharedPtr; - using ModelCoefficientsConstPtr = ModelCoefficients::ConstSharedPtr; - - using IndicesPtr = pcl::IndicesPtr; - using IndicesConstPtr = pcl::IndicesConstPtr; - - using ExactTimeSyncPolicy = - message_filters::Synchronizer>; - 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()); - -protected: - /** \brief The input PointCloud2 subscriber. */ - rclcpp::Subscription::SharedPtr sub_input_; - - /** \brief The output PointCloud2 publisher. */ - rclcpp::Publisher::SharedPtr pub_output_; - - /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_input_filter_; - - /** \brief The message filter subscriber for PointIndices. */ - message_filters::Subscriber sub_indices_filter_; - - /** \brief The desired user filter field name. */ - std::string filter_field_name_; - - /** \brief The minimum allowed filter value a point will be considered from. */ - double filter_limit_min_; - - /** \brief The maximum allowed filter value a point will be considered from. */ - double filter_limit_max_; - - /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a - * filter_limit_max_). Default: false. */ - bool filter_limit_negative_; - - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. */ - std::string tf_input_frame_; - - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. */ - std::string tf_output_frame_; - - /** \brief Internal mutex. */ - std::mutex mutex_; - - /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; - - /** \brief Virtual abstract filter method. To be implemented by every child. - * \param input the input point cloud dataset. - * \param indices a pointer to the vector of point indices to use. - * \param output the resultant filtered PointCloud2 - */ - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) = 0; - - // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform - // to new API. It's not pure virtual function so that a child class does not have to implement - // this function. - virtual void faster_filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, - const TransformInfo & transform_info); // != 0 - - /** \brief Lazy transport subscribe routine. */ - virtual void subscribe(const std::string & filter_name); - virtual void subscribe(); - - /** \brief Lazy transport unsubscribe routine. */ - virtual void unsubscribe(); - - /** \brief Call the child filter () method, optionally transform the result, and publish it. - * \param input the input point cloud dataset. - * \param indices a pointer to the vector of point indices to use. - */ - void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices); - - ////////////////////// - // from PCLNodelet // - ////////////////////// - /** \brief Set to true if point indices are used. - * - * When receiving a point cloud, if use_indices_ is false, the entire - * point cloud is processed for the given operation. If use_indices_ is - * true, then the ~indices topic is read to get the vector of point - * indices specifying the subset of the point cloud that will be used for - * the operation. In the case where use_indices_ is true, the ~input and - * ~indices topics must be synchronised in time, either exact or within a - * specified jitter. See also @ref latched_indices_ and approximate_sync. - **/ - bool use_indices_ = false; - /** \brief Set to true if the indices topic is latched. - * - * If use_indices_ is true, the ~input and ~indices topics generally must - * be synchronised in time. By setting this flag to true, the most recent - * value from ~indices can be used instead of requiring a synchronised - * message. - **/ - bool latched_indices_ = false; - - /** \brief The maximum queue size (default: 3). */ - size_t max_queue_size_ = 3; - - /** \brief True if we use an approximate time synchronizer - * versus an exact one (false by default). */ - bool approximate_sync_ = false; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - inline bool isValid( - const PointCloud2ConstPtr & cloud, const std::string & /*topic_name*/ = "input") - { - if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { - RCLCPP_WARN( - this->get_logger(), - "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, " - "and frame %s received!", - cloud->data.size(), cloud->width, cloud->height, cloud->point_step, - rclcpp::Time(cloud->header.stamp).seconds(), cloud->header.frame_id.c_str()); - return false; - } - return true; - } - - inline bool isValid( - const PointIndicesConstPtr & /*indices*/, const std::string & /*topic_name*/ = "indices") - { - return true; - } - - inline bool isValid( - const ModelCoefficientsConstPtr & /*model*/, const std::string & /*topic_name*/ = "model") - { - return true; - } - -private: - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_filter_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult filterParamCallback( - const std::vector & p); - - /** \brief Synchronized input, and indices.*/ - std::shared_ptr sync_input_indices_e_; - std::shared_ptr sync_input_indices_a_; - - /** \brief PointCloud2 + Indices data callback. */ - void input_indices_callback(const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices); - - /** \brief Get a matrix for conversion from the original frame to the target frame */ - bool calculate_transform_matrix( - const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, - TransformInfo & transform_info /*output*/); - - bool _calculate_transform_matrix( - const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, - const tf2_ros::Buffer & tf_buffer, Eigen::Matrix4f & eigen_transform /*output*/); - - bool convert_output_costly(std::unique_ptr & output); - - // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform - // to new API. - void faster_input_indices_callback( - const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices); - - void setupTF(); -}; -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__FILTER_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp deleted file mode 100644 index b8aba769b17a5..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include -#include -#include -#include - -#include -#include -#include - -#if __has_include() -#include -#else -#include -#endif -#include -#include - -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using diagnostic_updater::DiagnosticStatusWrapper; -using diagnostic_updater::Updater; - -class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - image_transport::Publisher image_pub_; - rclcpp::Publisher::SharedPtr visibility_pub_; - rclcpp::Publisher::SharedPtr noise_cloud_pub_; - -private: - void onVisibilityChecker(DiagnosticStatusWrapper & stat); - Updater updater_{this}; - double visibility_ = -1.0f; - double weak_first_distance_ratio_; - double general_distance_ratio_; - int weak_first_local_noise_threshold_; - double visibility_error_threshold_; - double visibility_warn_threshold_; - int vertical_bins_; - float max_azimuth_diff_; - std::string roi_mode_; - float x_max_; - float x_min_; - float y_max_; - float y_min_; - float z_max_; - float z_min_; - - float min_azimuth_deg_; - float max_azimuth_deg_; - float max_distance_; - - std::unordered_map roi_mode_map_ = { - {"No_ROI", 0}, - {"Fixed_xyz_ROI", 1}, - {"Fixed_azimuth_ROI", 2}, - }; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit DualReturnOutlierFilterComponent(const rclcpp::NodeOptions & options); -}; - -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp deleted file mode 100644 index ffb737b0b9b06..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ // NOLINT - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include - -#include -#include -#include -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class RadiusSearch2DOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - -private: - double search_radius_; - size_t min_neighbors_; - - // pcl::RadiusOutlierRemoval radius_outlier_removal_; - pcl::search::Search::Ptr kd_tree_; - // pcl::ExtractIndices extract_indices_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit RadiusSearch2DOutlierFilterComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp deleted file mode 100644 index caedeac62b88a..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ - -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/transform_info.hpp" -#include "autoware_point_types/types.hpp" - -#include -#include - -#if __has_include() -#include -#else -#include -#endif - -#include -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using point_cloud_msg_wrapper::PointCloud2Modifier; - -class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - using InputPointIndex = autoware_point_types::PointXYZIRCAEDTIndex; - using InputPointType = autoware_point_types::PointXYZIRCAEDT; - using OutputPointType = autoware_point_types::PointXYZIRC; - - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - - // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform - // to new API - virtual void faster_filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, - const TransformInfo & transform_info); - - rclcpp::Publisher::SharedPtr visibility_pub_; - -private: - /** \brief publisher of excluded pointcloud for debug reason. **/ - rclcpp::Publisher::SharedPtr outlier_pointcloud_publisher_; - - double distance_ratio_; - double object_length_threshold_; - int num_points_threshold_; - uint16_t max_rings_num_; - size_t max_points_num_per_ring_; - bool publish_outlier_pointcloud_; - - // for visibility score - int noise_threshold_; - int vertical_bins_; - int horizontal_bins_; - - float min_azimuth_deg_; - float max_azimuth_deg_; - float max_distance_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - - bool isCluster( - const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) - { - if (walk_size > num_points_threshold_) return true; - - auto first_point = - reinterpret_cast(&input->data[data_idx_both_ends.first]); - auto last_point = - reinterpret_cast(&input->data[data_idx_both_ends.second]); - - const auto x = first_point->x - last_point->x; - const auto y = first_point->y - last_point->y; - const auto z = first_point->z - last_point->z; - - return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; - } - - void setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size); - float calculateVisibilityScore(const PointCloud2 & input); - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); -}; - -} // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp deleted file mode 100644 index 4ec50c53c2393..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class VoxelGridOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; - int voxel_points_threshold_; - - pcl::VoxelGrid voxel_filter; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit VoxelGridOutlierFilterComponent(const rclcpp::NodeOptions & option); -}; -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp deleted file mode 100644 index bbb2c0b113658..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010, 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. - * - * $Id: passthrough.cpp 36194 2011-02-23 07:49:21Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class PassThroughFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit PassThroughFilterComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp deleted file mode 100644 index 0ffb50dc65092..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ // NOLINT - -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp" - -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class PassThroughFilterUInt16Component : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -private: - pcl::PassThroughUInt16 impl_; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit PassThroughFilterUInt16Component(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp deleted file mode 100644 index 7da891f20955b..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp +++ /dev/null @@ -1,474 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2012, 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 the copyright holder(s) 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. - * - * $Id$ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ - -#include -#include - -#include -#include -#include - -namespace pcl -{ -// cspell: ignore ptfilter -/** \brief @b PassThroughUInt16 passes points in a cloud based on constraints for one particular - * field of the point type. \details Iterates through the entire input once, automatically filtering - * non-finite points and the points outside the interval specified by setFilterLimits(), which - * applies only to the field specified by setFilterFieldName().

Usage example: \code - * pcl::PassThroughUInt16 ptfilter (true); // Initializing with true will allow us to - * extract the removed indices ptfilter.setInputCloud (cloud_in); ptfilter.setFilterFieldName ("x"); - * ptfilter.setFilterLimits (0.0, 1000.0); - * ptfilter.filter (*indices_x); - * // The indices_x array indexes all points of cloud_in that have x between 0.0 and 1000.0 - * indices_rem = ptfilter.getRemovedIndices (); - * // The indices_rem array indexes all points of cloud_in that have x smaller than 0.0 or larger - * than 1000.0 - * // and also indexes all non-finite points of cloud_in - * ptfilter.setIndices (indices_x); - * ptfilter.setFilterFieldName ("z"); - * ptfilter.setFilterLimits (-10.0, 10.0); - * ptfilter.setNegative (true); - * ptfilter.filter (*indices_xz); - * // The indices_xz array indexes all points of cloud_in that have x between 0.0 and 1000.0 and z - * larger than 10.0 or smaller than -10.0 ptfilter.setIndices (indices_xz); - * ptfilter.setFilterFieldName ("intensity"); - * ptfilter.setFilterLimits (FLT_MIN, 0.5); - * ptfilter.setNegative (false); - * ptfilter.filter (*cloud_out); - * // The resulting cloud_out contains all points of cloud_in that are finite and have: - * // x between 0.0 and 1000.0, z larger than 10.0 or smaller than -10.0 and intensity smaller than - * 0.5. \endcode \author Radu Bogdan Rusu \ingroup filters - */ -template -class PassThroughUInt16 : public FilterIndices -{ -protected: - typedef typename FilterIndices::PointCloud PointCloud; - typedef typename PointCloud::Ptr PointCloudPtr; - typedef typename PointCloud::ConstPtr PointCloudConstPtr; - typedef typename pcl::traits::fieldList::type FieldList; - -public: - 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 - * points being removed (default = false). - */ - explicit PassThroughUInt16(bool extract_removed_indices = false) - : FilterIndices::FilterIndices(extract_removed_indices), - filter_field_name_(""), - filter_limit_min_(0), - filter_limit_max_(UINT16_MAX) - { - filter_name_ = "PassThroughUInt16"; - } - - /** \brief Provide the name of the field to be used for filtering data. - * \details In conjunction with setFilterLimits(), points having values outside this interval for - * this field will be discarded. \param[in] field_name The name of the field that will be used for - * filtering. - */ - inline void setFilterFieldName(const std::string & field_name) - { - filter_field_name_ = field_name; - } - - /** \brief Retrieve the name of the field to be used for filtering data. - * \return The name of the field that will be used for filtering. - */ - inline std::string const getFilterFieldName() { return filter_field_name_; } - - /** \brief Set the numerical limits for the field for filtering data. - * \details In conjunction with setFilterFieldName(), points having values outside this interval - * for this field will be discarded. \param[in] limit_min The minimum allowed field value (default - * = FLT_MIN). \param[in] limit_max The maximum allowed field value (default = FLT_MAX). - */ - inline void setFilterLimits(const std::uint16_t & limit_min, const std::uint16_t & limit_max) - { - filter_limit_min_ = limit_min; - filter_limit_max_ = limit_max; - } - - /** \brief Get the numerical limits for the field for filtering data. - * \param[out] limit_min The minimum allowed field value (default = FLT_MIN). - * \param[out] limit_max The maximum allowed field value (default = FLT_MAX). - */ - inline void getFilterLimits(std::uint16_t & limit_min, std::uint16_t & limit_max) - { - limit_min = filter_limit_min_; - limit_max = filter_limit_max_; - } - - /** \brief Set to true if we want to return the data outside the interval specified by - * setFilterLimits (min, max) Default: false. \warning This method will be removed in the future. - * Use setNegative() instead. \param[in] limit_negative return data inside the interval (false) or - * outside (true) - */ - inline void setFilterLimitsNegative(const bool limit_negative) { negative_ = limit_negative; } - - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside - * (false). \warning This method will be removed in the future. Use getNegative() instead. - * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, - * false otherwise - */ - inline void getFilterLimitsNegative(bool & limit_negative) { limit_negative = negative_; } - - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside - * (false). \warning This method will be removed in the future. Use getNegative() instead. \return - * true if data \b outside the interval [min; max] is to be returned, false otherwise - */ - inline bool getFilterLimitsNegative() { return negative_; } - -protected: - using PCLBase::input_; - using PCLBase::indices_; - using Filter::filter_name_; - using Filter::getClassName; - using FilterIndices::negative_; - using FilterIndices::keep_organized_; - using FilterIndices::user_filter_value_; - using FilterIndices::extract_removed_indices_; - using FilterIndices::removed_indices_; - - /** \brief Filtered results are stored in a separate point cloud. - * \param[out] output The resultant point cloud. - */ - void applyFilter(PointCloud & output); - - /** \brief Filtered results are indexed by an indices array. - * \param[out] indices The resultant indices. - */ - void applyFilter(std::vector & indices) { applyFilterIndices(indices); } - - /** \brief Filtered results are indexed by an indices array. - * \param[out] indices The resultant indices. - */ - void applyFilterIndices(std::vector & indices); - -private: - /** \brief The name of the field that will be used for filtering. */ - std::string filter_field_name_; - - /** \brief The minimum allowed field value (default = FLT_MIN). */ - std::uint16_t filter_limit_min_; - - /** \brief The maximum allowed field value (default = FLT_MIN). */ - std::uint16_t filter_limit_max_; -}; - -//////////////////////////////////////////////////////////////////////////////////////////// -/** \brief PassThroughUInt16 uses the base Filter class methods to pass through all data that - * satisfies the user given constraints. \author Radu B. Rusu \ingroup filters - */ -template <> -class PCL_EXPORTS PassThroughUInt16 : public Filter -{ - typedef pcl::PCLPointCloud2 PCLPointCloud2; - typedef PCLPointCloud2::Ptr PCLPointCloud2Ptr; - typedef PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr; - - using Filter::removed_indices_; - using Filter::extract_removed_indices_; - -public: - /** \brief Constructor. */ - explicit PassThroughUInt16(bool extract_removed_indices = false) - : Filter::Filter(extract_removed_indices), - keep_organized_(false), - user_filter_value_(std::numeric_limits::quiet_NaN()), - filter_field_name_(""), - filter_limit_min_(0), - filter_limit_max_(UINT16_MAX), - filter_limit_negative_(false) - { - filter_name_ = "PassThroughUInt16"; - } - - /** \brief Set whether the filtered points should be kept and set to the - * value given through \a setUserFilterValue (default: NaN), or removed - * from the PointCloud, thus potentially breaking its organized - * structure. By default, points are removed. - * - * \param[in] val set to true whether the filtered points should be kept and - * set to a user given value (default: NaN) - */ - inline void setKeepOrganized(bool val) { keep_organized_ = val; } - - /** \brief Obtain the value of the internal \a keep_organized_ parameter. */ - inline bool getKeepOrganized() { return keep_organized_; } - - /** \brief Provide a value that the filtered points should be set to - * instead of removing them. Used in conjunction with \a - * setKeepOrganized (). - * \param[in] val the user given value that the filtered point dimensions should be set to - */ - inline void setUserFilterValue(float val) { user_filter_value_ = val; } - - /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a - * setFilterLimits, points having values outside this interval will be discarded. \param[in] - * field_name the name of the field that contains values used for filtering - */ - inline void setFilterFieldName(const std::string & field_name) - { - filter_field_name_ = field_name; - } - - /** \brief Get the name of the field used for filtering. */ - inline std::string const getFilterFieldName() { return filter_field_name_; } - - /** \brief Set the field filter limits. All points having field values outside this interval will - * be discarded. \param[in] limit_min the minimum allowed field value \param[in] limit_max the - * maximum allowed field value - */ - inline void setFilterLimits(const std::uint16_t & limit_min, const std::uint16_t & limit_max) - { - filter_limit_min_ = limit_min; - filter_limit_max_ = limit_max; - } - - /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, - * FLT_MAX. \param[out] limit_min the minimum allowed field value \param[out] limit_max the - * maximum allowed field value - */ - inline void getFilterLimits(std::uint16_t & limit_min, std::uint16_t & limit_max) - { - limit_min = filter_limit_min_; - limit_max = filter_limit_max_; - } - - /** \brief Set to true if we want to return the data outside the interval specified by - * setFilterLimits (min, max). Default: false. \param[in] limit_negative return data inside the - * interval (false) or outside (true) - */ - inline void setFilterLimitsNegative(const bool limit_negative) - { - filter_limit_negative_ = limit_negative; - } - - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside - * (false). \param[out] limit_negative true if data \b outside the interval [min; max] is to be - * returned, false otherwise - */ - inline void getFilterLimitsNegative(bool & limit_negative) - { - limit_negative = filter_limit_negative_; - } - - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside - * (false). \return true if data \b outside the interval [min; max] is to be returned, false - * otherwise - */ - inline bool getFilterLimitsNegative() { return filter_limit_negative_; } - -protected: - void applyFilter(PCLPointCloud2 & output); - -private: - /** \brief Keep the structure of the data organized, by setting the - * filtered points to a user given value (NaN by default). - */ - bool keep_organized_; - - /** \brief User given value to be set to any filtered point. Casted to - * the correct field type. - */ - float user_filter_value_; - - /** \brief The desired user filter field name. */ - std::string filter_field_name_; - - /** \brief The minimum allowed filter value a point will be considered from. */ - std::uint16_t filter_limit_min_; - - /** \brief The maximum allowed filter value a point will be considered from. */ - std::uint16_t filter_limit_max_; - - /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a - * filter_limit_max_). Default: false. */ - bool filter_limit_negative_; -}; -} // namespace pcl - -template -void pcl::PassThroughUInt16::applyFilter(PointCloud & output) -{ - std::vector indices; - if (keep_organized_) { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilterIndices(indices); - extract_removed_indices_ = temp; - - output = *input_; - for (int rii = 0; rii < static_cast(removed_indices_->size()); - ++rii) // rii = removed indices iterator - { - output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = - output.points[(*removed_indices_)[rii]].z = user_filter_value_; - } - if (!std::isfinite(user_filter_value_)) { - output.is_dense = false; - } - } else { - output.is_dense = true; - applyFilterIndices(indices); - copyPointCloud(*input_, indices, output); - } -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template -void pcl::PassThroughUInt16::applyFilterIndices(std::vector & indices) -{ - // The arrays to be used - indices.resize(indices_->size()); - removed_indices_->resize(indices_->size()); - int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator - - // Has a field name been specified? - if (filter_field_name_.empty()) { - // Only filter for non-finite entries then - for (int iii = 0; iii < static_cast(indices_->size()); - ++iii) // iii = input indices iterator - { - // Non-finite entries are always passed to removed indices - if ( - !std::isfinite(input_->points[(*indices_)[iii]].x) || - !std::isfinite(input_->points[(*indices_)[iii]].y) || - !std::isfinite(input_->points[(*indices_)[iii]].z)) { - if (extract_removed_indices_) { - (*removed_indices_)[rii++] = (*indices_)[iii]; - } - continue; - } - indices[oii++] = (*indices_)[iii]; - } - } else { - // Attempt to get the field name's index - std::vector fields; - int distance_idx = pcl::getFieldIndex(filter_field_name_, fields); - if (distance_idx == -1) { - PCL_WARN( - "[pcl::%s::applyFilter] Unable to find field name in point type.\n", - getClassName().c_str()); - indices.clear(); - removed_indices_->clear(); - return; - } - - // Filter for non-finite entries and the specified field limits - for (int iii = 0; iii < static_cast(indices_->size()); - ++iii) // iii = input indices iterator - { - // Non-finite entries are always passed to removed indices - if ( - !std::isfinite(input_->points[(*indices_)[iii]].x) || - !std::isfinite(input_->points[(*indices_)[iii]].y) || - !std::isfinite(input_->points[(*indices_)[iii]].z)) { - if (extract_removed_indices_) { - (*removed_indices_)[rii++] = (*indices_)[iii]; - } - continue; - } - - // Get the field's value - const std::uint8_t * pt_data = - reinterpret_cast(&input_->points[(*indices_)[iii]]); - std::uint16_t field_value = 0; - memcpy(&field_value, pt_data + fields[distance_idx].offset, sizeof(std::uint16_t)); - - // Remove NAN/INF/-INF values. We expect passthrough to output clean valid data. - if (!std::isfinite(field_value)) { - if (extract_removed_indices_) { - (*removed_indices_)[rii++] = (*indices_)[iii]; - } - continue; - } - - // Outside of the field limits are passed to removed indices - if (!negative_ && (field_value < filter_limit_min_ || field_value > filter_limit_max_)) { - if (extract_removed_indices_) { - (*removed_indices_)[rii++] = (*indices_)[iii]; - } - continue; - } - - // Inside of the field limits are passed to removed indices if negative was set - if (negative_ && field_value >= filter_limit_min_ && field_value <= filter_limit_max_) { - if (extract_removed_indices_) { - (*removed_indices_)[rii++] = (*indices_)[iii]; - } - continue; - } - - // Otherwise it was a normal point for output (inlier) - indices[oii++] = (*indices_)[iii]; - } - } - - // Resize the output arrays - indices.resize(oii); - removed_indices_->resize(rii); -} - -#define PCL_INSTANTIATE_PassThroughUInt16(T) template class PCL_EXPORTS pcl::PassThroughUInt16; - -#ifdef PCL_NO_PRECOMPILE -#include -#endif - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp deleted file mode 100644 index 6646426a29b99..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ // NOLINT - -#include "autoware/pointcloud_preprocessor/filter.hpp" - -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class PointcloudAccumulatorComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - -private: - double accumulation_time_sec_; - boost::circular_buffer pointcloud_buffer_; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit PointcloudAccumulatorComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp deleted file mode 100644 index f52f2f306c69b..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/geometry.hpp" -======== -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/utility/geometry.hpp" ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp - -#include -#include -#include - -#include -#include - -#include - -namespace autoware::pointcloud_preprocessor -{ -class PolygonRemoverComponent : public autoware::pointcloud_preprocessor::Filter -{ -protected: - virtual void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - - void publishRemovedPolygon(); - - void update_polygon(const geometry_msgs::msg::Polygon::ConstSharedPtr & polygon_in); - static PolygonCgal polygon_geometry_to_cgal( - const geometry_msgs::msg::Polygon::ConstSharedPtr & polygon_in); - PointCloud2 remove_updated_polygon_from_cloud(const PointCloud2ConstPtr & cloud_in); - PointCloud2 remove_polygon_cgal_from_cloud( - const PointCloud2::ConstSharedPtr & cloud_in_ptr, const PolygonCgal & polyline_polygon); - -private: - rclcpp::Parameter param; - std::vector polygon_vertices_; - geometry_msgs::msg::Polygon::SharedPtr polygon_; - - bool polygon_is_initialized_; - bool will_visualize_; - PolygonCgal polygon_cgal_; - visualization_msgs::msg::Marker marker_; - - rclcpp::Publisher::SharedPtr pub_marker_ptr_; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit PolygonRemoverComponent(const rclcpp::NodeOptions & options); -}; -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp deleted file mode 100644 index 00704b20b85ef..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ /dev/null @@ -1,191 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * 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. - * - * $Id: TIME_SYNCHRONIZER.cpp 35231 2011-01-14 05:33:20Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -// ROS includes -#include "autoware_point_types/types.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using autoware_point_types::PointXYZIRC; -using point_cloud_msg_wrapper::PointCloud2Modifier; -// cspell:ignore Yoshi -/** \brief @b PointCloudDataSynchronizerComponent is a special form of data - * synchronizer: it listens for a set of input PointCloud messages on the same topic, - * checks their timestamps, and concatenates their fields together into a single - * PointCloud output message. - * \author Radu Bogdan Rusu - * \edited by Yoshi Ri - */ -class PointCloudDataSynchronizerComponent : public rclcpp::Node -{ -public: - typedef sensor_msgs::msg::PointCloud2 PointCloud2; - - /** \brief constructor. */ - explicit PointCloudDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); - - /** \brief constructor. - * \param queue_size the maximum queue size - */ - PointCloudDataSynchronizerComponent(const rclcpp::NodeOptions & node_options, int queue_size); - - /** \brief Empty destructor. */ - virtual ~PointCloudDataSynchronizerComponent() {} - -private: - /** \brief Delay Compensated PointCloud publisher*/ - std::map::SharedPtr> - transformed_raw_pc_publisher_map_; - - /** \brief The maximum number of messages that we can store in the queue. */ - int maximum_queue_size_ = 3; - - double timeout_sec_ = 0.1; - - std::set not_subscribed_topic_names_; - - /** \brief A vector of subscriber. */ - std::vector::SharedPtr> filters_; - - rclcpp::Subscription::SharedPtr sub_twist_; - rclcpp::Subscription::SharedPtr sub_odom_; - - rclcpp::TimerBase::SharedPtr timer_; - diagnostic_updater::Updater updater_{this}; - - const std::string input_twist_topic_type_; - - /** \brief Output TF frame the concatenated points should be transformed to. */ - std::string output_frame_; - bool keep_input_frame_in_synchronized_pointcloud_; - - /** \brief Input point cloud topics. */ - // XmlRpc::XmlRpcValue input_topics_; - std::vector input_topics_; - - /** \brief TF listener object. */ - std::shared_ptr tf2_buffer_; - std::shared_ptr tf2_listener_; - - std::deque twist_ptr_queue_; - - std::map cloud_stdmap_; - std::map cloud_stdmap_tmp_; - std::mutex mutex_; - - std::vector input_offset_; - std::map offset_map_; - - void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - void transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, - const std::string & target_frame); - Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( - const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); - std::map synchronizeClouds(); - void publish(); - - void convertToXYZIRCCloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, - sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); - void setPeriod(const int64_t new_period); - void cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, - const std::string & topic_name); - void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); - void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); - void timer_callback(); - - void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); - std::string replaceSyncTopicNamePostfix( - const std::string & original_topic_name, const std::string & postfix); - - /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; -}; - -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp deleted file mode 100644 index 9c4ba7c5ea7ea..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include - -namespace autoware::pointcloud_preprocessor -{ - -/** - * This holds the coordinate transformation information of the point cloud. - * Usage example: - * \code - * if (transform_info.need_transform) { - * point = transform_info.eigen_transform * point; - * } - * \endcode - */ -struct TransformInfo -{ - TransformInfo() - { - eigen_transform = Eigen::Matrix4f::Identity(4, 4); - need_transform = false; - } - - Eigen::Matrix4f eigen_transform; - bool need_transform; -}; - -} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp deleted file mode 100644 index c019fd04f6c87..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp -#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ -#define POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ -======== -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/geometry.hpp - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -using K = CGAL::Exact_predicates_inexact_constructions_kernel; -using PointCgal = K::Point_2; -using PolygonCgal = std::vector; - -namespace autoware::pointcloud_preprocessor::utils -{ -/** - * @brief convert ROS polygon to CGAL polygon - */ -void to_cgal_polygon(const geometry_msgs::msg::Polygon & polygon_in, PolygonCgal & polygon_out); - -/** - * @brief convert lanelet polygon to CGAL polygon - */ -void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & polygon_out); - -/** - * @brief remove points in the given polygon - */ -void remove_polygon_cgal_from_cloud( - const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon, - sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional & max_z = std::nullopt); - -/** - * @brief remove points in the given polygon - */ -void remove_polygon_cgal_from_cloud( - const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out, const std::optional & max_z = std::nullopt); - -/** - * @brief remove points in the given polygons - */ -void remove_polygon_cgal_from_cloud( - const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, - const std::optional & max_z = std::nullopt); - -/** - * @brief remove points in the given polygons - */ -void remove_polygon_cgal_from_cloud( - const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, - const std::optional & max_z = std::nullopt); - -/** - * @brief return true if the given point is inside the at least one of the polygons - */ -bool point_within_cgal_polys( - const pcl::PointXYZ & point, const std::vector & polyline_polygons); - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp -} // namespace pointcloud_preprocessor::utils - -#endif // POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ -======== -} // namespace autoware::pointcloud_preprocessor::utils - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/geometry.hpp diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp deleted file mode 100644 index ef87a4f31457b..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ -#define POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ - -#include - -namespace pointcloud_preprocessor::utils -{ -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to - * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ -bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input); - -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is - * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRC */ -bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input); - -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That - * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ -bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input); - -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. That - * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */ -bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input); - -} // namespace pointcloud_preprocessor::utils - -#endif // POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp deleted file mode 100644 index 4ba773ed618ac..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include - -#include -#include -#include -#include - -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::Point2d; - -namespace autoware::pointcloud_preprocessor -{ -class Lanelet2MapFilterComponent : public rclcpp::Node -{ - using PointCloud2 = sensor_msgs::msg::PointCloud2; - using PointCloud2Ptr = sensor_msgs::msg::PointCloud2::SharedPtr; - using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; - -public: - explicit Lanelet2MapFilterComponent(const rclcpp::NodeOptions & options); - -private: - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - rclcpp::Subscription::SharedPtr map_sub_; - rclcpp::Subscription::SharedPtr pointcloud_sub_; - rclcpp::Publisher::SharedPtr filtered_pointcloud_pub_; - - lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::ConstLanelets road_lanelets_; - - float voxel_size_x_; - float voxel_size_y_; - - void pointcloudCallback(const PointCloud2ConstPtr msg); - - void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); - - bool transformPointCloud( - const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr, - PointCloud2 * out_cloud_ptr); - - LinearRing2d getConvexHull(const pcl::PointCloud::Ptr & input_cloud); - - lanelet::ConstLanelets getIntersectedLanelets( - const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets_); - - pcl::PointCloud getLaneFilteredPointCloud( - const lanelet::ConstLanelets & joint_lanelets, - const pcl::PointCloud::Ptr & cloud); - - bool pointWithinLanelets(const Point2d & point, const lanelet::ConstLanelets & joint_lanelets); - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); -}; - -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp deleted file mode 100644 index 834de2c1782db..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/geometry.hpp" -======== -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/utility/geometry.hpp" ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp - -#include -#include -#include - -#include - -#include - -using autoware::universe_utils::MultiPoint2d; - -namespace autoware::pointcloud_preprocessor -{ -class VectorMapInsideAreaFilterComponent : public autoware::pointcloud_preprocessor::Filter -{ -private: - void filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - - rclcpp::Subscription::SharedPtr map_sub_; - lanelet::ConstPolygons3d polygon_lanelets_; - - void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); - - // parameter - std::string polygon_type_; - bool use_z_filter_ = false; - float z_threshold_; - -public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - explicit VectorMapInsideAreaFilterComponent(const rclcpp::NodeOptions & options); -}; - -} // namespace autoware::pointcloud_preprocessor - -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/blockage_diagnostics_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/blockage_diagnostics_node.launch.xml deleted file mode 100644 index 6d1aa1f906828..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/blockage_diagnostics_node.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml deleted file mode 100644 index 4afc4906f0b3a..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml - - - -======== - - - - - ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/pickup_based_voxel_grid_downsample_filter.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/pointcloud_accumulator.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/radius_search_2d_outlier_filter.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml deleted file mode 100644 index aa7c96156ae4c..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter_node.launch.xml - - - -======== - - ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter.launch.xml - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter_node.launch.xml deleted file mode 100644 index 5bffc6f3e0463..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter_node.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - -<<<<<<<< HEAD:sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter_node.launch.xml - - - - - -======== - - ->>>>>>>> original/main:sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_downsample_filter.launch.xml diff --git a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter.launch.xml similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter.launch.xml