From 93490979c97c91ce2aeda17201926c46844d38ca Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 9 Jul 2024 22:17:40 +0900 Subject: [PATCH] feat: migrating pointcloud types (#6996) * feat: changed most of sensing to the new type Signed-off-by: Kenzo Lobos-Tsunekawa * chore: started applying changes to the perception stack Signed-off-by: Kenzo Lobos-Tsunekawa * feat: confirmed operation until centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa * feat: reverted to the original implementation of pointpainting Signed-off-by: Kenzo Lobos-Tsunekawa * chore: forgot to push a header Signed-off-by: Kenzo Lobos-Tsunekawa * feat: also implemented the changes for the subsample filters that were out of scope before Signed-off-by: Kenzo Lobos-Tsunekawa * fix: some point type changes were missing from the latest merge from main Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed unused code, added comments, and brought back a removed publish Signed-off-by: Kenzo Lobos-Tsunekawa * chore: replaced pointcloud_raw for pointcloud_raw_ex to avoid extra processing time in the drivers Signed-off-by: Kenzo Lobos-Tsunekawa * feat: added memory layout checks Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated documentation regarding the point types Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added hyperlinks to the point definitions. will be valid only once the PR is merged Signed-off-by: Kenzo Lobos-Tsunekawa * fix: fixed compilation due to moving the utilities files to the base library Signed-off-by: Kenzo Lobos-Tsunekawa * chore: separated the utilities functions due to a dependency issue * chore: forgot that perception also uses the filter class Signed-off-by: Kenzo Lobos-Tsunekawa * feature: adapted the undistortion tests to the new point type Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> --- .../include/autoware_point_types/types.hpp | 91 +++++++- .../test/test_point_types.cpp | 9 + .../traffic_light.launch.xml | 2 +- .../scan_ground_filter_nodelet.hpp | 1 + .../src/scan_ground_filter_nodelet.cpp | 4 +- .../src/pointpainting_fusion/node.cpp | 6 +- .../src/detector.cpp | 24 +- .../src/lidar_apollo_segmentation_tvm.cpp | 26 ++- .../src/low_intensity_cluster_filter.cpp | 6 +- .../pointcloud_preprocessor/CMakeLists.txt | 14 +- .../docs/dual-return-outlier-filter.md | 4 +- .../docs/ring-outlier-filter.md | 11 +- .../concatenate_and_time_sync_nodelet.hpp | 4 +- .../concatenate_pointclouds.hpp | 4 +- .../faster_voxel_grid_downsample_filter.hpp | 4 +- .../ring_outlier_filter_nodelet.hpp | 14 +- .../polygon_remover/polygon_remover.hpp | 2 +- .../time_synchronizer_nodelet.hpp | 4 +- .../utility/{utilities.hpp => geometry.hpp} | 16 +- .../utility/memory.hpp | 40 ++++ .../vector_map_inside_area_filter.hpp | 4 +- .../blockage_diag/blockage_diag_nodelet.cpp | 10 +- .../concatenate_and_time_sync_nodelet.cpp | 63 ++++-- .../concatenate_pointclouds.cpp | 62 +++-- .../distortion_corrector.cpp | 43 +++- .../faster_voxel_grid_downsample_filter.cpp | 27 ++- .../voxel_grid_downsample_filter_nodelet.cpp | 2 +- .../pointcloud_preprocessor/src/filter.cpp | 26 +++ .../dual_return_outlier_filter_nodelet.cpp | 34 +-- .../ring_outlier_filter_nodelet.cpp | 149 ++++++------- .../time_synchronizer_nodelet.cpp | 47 ++-- .../utility/{utilities.cpp => geometry.cpp} | 93 +------- .../src/utility/memory.cpp | 211 ++++++++++++++++++ .../test/test_distortion_corrector_node.cpp | 35 +-- .../test/test_utilities.cpp | 4 +- 35 files changed, 767 insertions(+), 329 deletions(-) rename sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/{utilities.hpp => geometry.hpp} (78%) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp rename sensing/pointcloud_preprocessor/src/utility/{utilities.cpp => geometry.cpp} (52%) create mode 100644 sensing/pointcloud_preprocessor/src/utility/memory.cpp diff --git a/common/autoware_point_types/include/autoware_point_types/types.hpp b/common/autoware_point_types/include/autoware_point_types/types.hpp index 71ea80c5a9733..a3b0670280530 100644 --- a/common/autoware_point_types/include/autoware_point_types/types.hpp +++ b/common/autoware_point_types/include/autoware_point_types/types.hpp @@ -54,6 +54,23 @@ enum ReturnType : uint8_t { DUAL_ONLY, }; +struct PointXYZIRC +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + + friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel; + } +}; + struct PointXYZIRADRT { float x{0.0F}; @@ -75,25 +92,97 @@ struct PointXYZIRADRT } }; -enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp }; +struct PointXYZIRCAEDT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + float azimuth{0.0F}; + float elevation{0.0F}; + float distance{0.0F}; + std::uint32_t time_stamp{0U}; + + friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel && + float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && + p1.time_stamp == p2.time_stamp; + } +}; + +enum class PointXYZIIndex { X, Y, Z, Intensity }; +enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; +enum class PointXYZIRADRTIndex { + X, + Y, + Z, + Intensity, + Ring, + Azimuth, + Distance, + ReturnType, + TimeStamp +}; +enum class PointXYZIRCAEDTIndex { + X, + Y, + Z, + Intensity, + ReturnType, + Channel, + Azimuth, + Elevation, + Distance, + TimeStamp +}; LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); + +using PointXYZIRCGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator>; + using PointXYZIRADRTGenerator = std::tuple< point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, field_return_type_generator, field_time_stamp_generator>; +using PointXYZIRCAEDTGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator, field_azimuth_generator, + field_elevation_generator, field_distance_generator, field_time_stamp_generator>; + } // namespace autoware_point_types +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware_point_types::PointXYZIRC, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) + POINT_CLOUD_REGISTER_POINT_STRUCT( autoware_point_types::PointXYZIRADRT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( double, time_stamp, time_stamp)) +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware_point_types::PointXYZIRCAEDT, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, + return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( + float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) #endif // AUTOWARE_POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp index 27ab1ef993ab3..16887ac2aa498 100644 --- a/common/autoware_point_types/test/test_point_types.cpp +++ b/common/autoware_point_types/test/test_point_types.cpp @@ -36,6 +36,15 @@ TEST(PointEquality, PointXYZIRADRT) EXPECT_EQ(pt0, pt1); } +TEST(PointEquality, PointXYZIRCAEDT) +{ + using autoware_point_types::PointXYZIRCAEDT; + + PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + EXPECT_EQ(pt0, pt1); +} + TEST(PointEquality, FloatEq) { // test template diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index ed7ff059a6e53..6cc47de76ea96 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 3ae41173a7a7d..6bfdb271b14e9 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -162,6 +162,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter int y_offset_; int z_offset_; int intensity_offset_; + int intensity_type_; bool offset_initialized_; void set_field_offsets(const PointCloud2ConstPtr & input); diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 7853aa400b6a5..2a2e96b47771d 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -96,8 +96,9 @@ inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstP int intensity_index = pcl::getFieldIndex(*input, "intensity"); if (intensity_index != -1) { intensity_offset_ = input->fields[intensity_index].offset; + intensity_type_ = input->fields[intensity_index].datatype; } else { - intensity_offset_ = z_offset_ + sizeof(float); + intensity_offset_ = -1; } offset_initialized_ = true; } @@ -569,6 +570,7 @@ void ScanGroundFilterComponent::extractObjectPoints( PointCloud2 & out_object_cloud) { size_t output_data_size = 0; + for (const auto & i : in_indices.indices) { std::memcpy( &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 7973c22cc1d78..3fb2aca2171f7 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -296,13 +296,13 @@ void PointPaintingFusionNode::fuseOnSingleImage( // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); const auto x_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::X)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::X)) .offset; const auto y_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Y)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::Y)) .offset; const auto z_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Z)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::Z)) .offset; const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; const auto p_step = painted_pointcloud_msg.point_step; diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index b704bab39338a..ba4d7f788b0f5 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -16,6 +16,8 @@ #include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include + #include #include #include @@ -126,7 +128,27 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( // convert from ros to pcl pcl::PointCloud::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud); - pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + // pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + + auto pcl_pointcloud_raw = *pcl_pointcloud_raw_ptr; + pcl_pointcloud_raw.width = transformed_cloud.width; + pcl_pointcloud_raw.height = transformed_cloud.height; + pcl_pointcloud_raw.is_dense = transformed_cloud.is_dense == 1; + pcl_pointcloud_raw.resize(transformed_cloud.width * transformed_cloud.height); + + sensor_msgs::PointCloud2ConstIterator it_x(transformed_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(transformed_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(transformed_cloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_intensity(transformed_cloud, "intensity"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { + pcl::PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = static_cast(*it_intensity); + pcl_pointcloud_raw.emplace_back(std::move(point)); + } // generate feature map std::shared_ptr feature_map_ptr = diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index e78cd57b3a4a1..088dab6377ac6 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include #include @@ -173,7 +175,29 @@ std::shared_ptr ApolloLidarSegmentation::detec sensor_msgs::msg::PointCloud2 transformed_cloud; ApolloLidarSegmentation::transformCloud(input, transformed_cloud, z_offset_); // convert from ros to pcl - pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_ptr_); + // pcl::fromROSMsg( + // transformed_cloud, *pcl_pointcloud_ptr_); // Manual conversion is needed since intensity + // comes as an uint8_t + + auto pcl_pointcloud = *pcl_pointcloud_ptr_; + pcl_pointcloud.width = input.width; + pcl_pointcloud.height = input.height; + pcl_pointcloud.is_dense = input.is_dense == 1; + pcl_pointcloud.resize(input.width * input.height); + + sensor_msgs::PointCloud2ConstIterator it_x(input, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(input, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(input, "z"); + sensor_msgs::PointCloud2ConstIterator it_intensity(input, "intensity"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { + pcl::PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = static_cast(*it_intensity); + pcl_pointcloud.emplace_back(std::move(point)); + } // inference pipeline auto output = pipeline->schedule(pcl_pointcloud_ptr_); diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp index 4772c355ec0b6..a5fd2b9a1c285 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp @@ -123,9 +123,9 @@ bool LowIntensityClusterFilter::isValidatedCluster(const sensor_msgs::msg::Point RCLCPP_WARN(get_logger(), "Invalid point cloud data. point_step is less than 16."); return true; } - for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); iter != iter.end(); - ++iter) { - mean_intensity += *iter; + for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); + iter != iter.end(); ++iter) { + mean_intensity += static_cast(*iter); } const size_t num_points = cluster.width * cluster.height; mean_intensity /= static_cast(num_points); diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 2d0649fe43954..76b9cc91ea586 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -26,11 +26,13 @@ include_directories( add_library(pointcloud_preprocessor_filter_base SHARED src/filter.cpp + src/utility/memory.cpp ) target_include_directories(pointcloud_preprocessor_filter_base PUBLIC "$" "$" + ${autoware_point_types_INCLUDE_DIRS} ) ament_target_dependencies(pointcloud_preprocessor_filter_base @@ -41,6 +43,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base tf2_ros autoware_universe_utils pcl_ros + autoware_point_types ) add_library(faster_voxel_grid_downsample_filter SHARED @@ -59,7 +62,6 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter ) ament_auto_add_library(pointcloud_preprocessor_filter SHARED - src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp src/concatenate_data/concatenate_pointclouds.cpp src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -82,6 +84,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/blockage_diag/blockage_diag_nodelet.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp + src/utility/geometry.cpp ) target_link_libraries(pointcloud_preprocessor_filter @@ -227,16 +230,19 @@ ament_auto_package(INSTALL_TO_SHARE if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_utilities test/test_utilities.cpp ) - ament_add_gtest(distortion_corrector_node_tests - test/test_distortion_corrector_node.cpp + + ament_add_gtest(test_distortion_corrector_node + test/test_distortion_corrector_node.cpp ) target_link_libraries(test_utilities pointcloud_preprocessor_filter) - target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter) + target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter) endif() diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md index c5a0b75e9b33b..d5993a803fa87 100644 --- a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -10,7 +10,7 @@ This node can remove rain and fog by considering the light reflected from the ob ![outlier_filter-return_type](./image/outlier_filter-return_type.drawio.svg) -Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRADRT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L57-L76) data structure. +Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) data structure. Another feature of this node is that it publishes visibility as a diagnostic topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle. @@ -73,7 +73,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ## Assumptions / Known limits Not recommended for use as it is under development. -Input data must be `PointXYZIRADRT` type data including `return_type`. +Input data must be [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) type data including `return_type`. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index e87023ef00149..0a179ddf6a8ef 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -73,16 +73,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ## Assumptions / Known limits -It is a prerequisite to input a scan point cloud in chronological order. In this repository it is defined as blow structure (please refer to [PointXYZIRADT](https://github.com/tier4/AutowareArchitectureProposal.iv/blob/5d8dff0db51634f0c42d2a3e87ca423fbee84348/sensing/preprocessor/pointcloud/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp#L53-L62)). - -- X: x -- Y: y -- z: z -- I: intensity -- R: ring -- A :azimuth -- D: distance -- T: time_stamp +This nodes requires that the points of the input point cloud are in chronological order and that individual points follow the memory layout specified by [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116). ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 86c4ed5943540..619a0b8520b97 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -87,7 +87,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data @@ -169,7 +169,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index ead66d98b8be7..77717c51e6981 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -86,7 +86,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +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, @@ -160,7 +160,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp index d80e62d08330d..379c4c79e555a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp @@ -34,7 +34,7 @@ class FasterVoxelGridDownsampleFilter 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); + 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); @@ -48,7 +48,7 @@ class FasterVoxelGridDownsampleFilter float intensity; uint32_t point_count_; - Centroid() : x(0), y(0), z(0), intensity(-1.0) {} + 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) { diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index 55ba79aac4593..08fbfe73f2744 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -35,12 +35,15 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; class RingOutlierFilterComponent : public 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); @@ -83,8 +86,10 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter { 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]); + 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; @@ -94,8 +99,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter } void setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields); + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size); float calculateVisibilityScore(const PointCloud2 & input); public: diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp index 49baed4ed7ed5..4d2e66eea700e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp @@ -16,7 +16,7 @@ #define POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 4153cee7695f8..2dc98a571ff2c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -87,7 +87,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; // cspell:ignore Yoshi /** \brief @b PointCloudDataSynchronizerComponent is a special form of data @@ -166,7 +166,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node std::map synchronizeClouds(); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp similarity index 78% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp rename to sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp index 654057e7bb8d7..0175f88ceb89c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ -#define POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ +#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ +#define POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ #include #include @@ -81,14 +81,6 @@ void remove_polygon_cgal_from_cloud( bool point_within_cgal_polys( const pcl::PointXYZ & point, const std::vector & polyline_polygons); -/** \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_PointXYZI(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_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input); - } // namespace pointcloud_preprocessor::utils -#endif // POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ +#endif // POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp new file mode 100644 index 0000000000000..ef87a4f31457b --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp @@ -0,0 +1,40 @@ +// 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/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index d376a47ae8ab0..e2ba7bcd883aa 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// 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. @@ -16,7 +16,7 @@ #define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include #include diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 773e6db056990..aae50a18f2024 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -21,7 +21,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZIRADRT; +using autoware_point_types::PointXYZIRCAEDT; using diagnostic_msgs::msg::DiagnosticStatus; BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options) @@ -175,7 +175,7 @@ void BlockageDiagComponent::filter( } ideal_horizontal_bins = static_cast( (angle_range_deg_[1] + compensate_angle - angle_range_deg_[0]) / horizontal_resolution_); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); @@ -196,7 +196,7 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { for (const auto p : pcl_input->points) { - double azimuth_deg = p.azimuth / 100.; + double azimuth_deg = p.azimuth * (180.0 / M_PI); if ( ((azimuth_deg > angle_range_deg_[0]) && (azimuth_deg <= angle_range_deg_[1] + compensate_angle)) || @@ -208,9 +208,9 @@ void BlockageDiagComponent::filter( uint16_t depth_intensity = UINT16_MAX * (1.0 - std::min(p.distance / max_distance_range_, 1.0)); if (is_channel_order_top2down_) { - full_size_depth_map.at(p.ring, horizontal_bin_index) = depth_intensity; + full_size_depth_map.at(p.channel, horizontal_bin_index) = depth_intensity; } else { - full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin_index) = + full_size_depth_map.at(vertical_bins - p.channel - 1, horizontal_bin_index) = depth_intensity; } } diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 17f650fcb8e54..d10ba9a3fbd29 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -51,6 +51,8 @@ #include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -491,40 +493,56 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( +void PointCloudConcatenateDataSynchronizerComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -549,15 +567,28 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); auto input = std::make_shared(*input_ptr); if (input->data.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); } else { - // convert to XYZI pointcloud if pointcloud is not empty - convertToXYZICloud(input, xyzi_input_ptr); + // convert to XYZIRC pointcloud if pointcloud is not empty + convertToXYZIRCCloud(input, xyzirc_input_ptr); } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); @@ -566,7 +597,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -579,7 +610,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 0ecaa5588bf65..2ff73c7913f0a 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -14,6 +14,8 @@ #include "pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -325,39 +327,56 @@ void PointCloudConcatenationComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenationComponent::convertToXYZICloud( +void PointCloudConcatenationComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -382,10 +401,23 @@ void PointCloudConcatenationComponent::setPeriod(const int64_t new_period) void PointCloudConcatenationComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); - convertToXYZICloud(input, xyzi_input_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZIRCCloud(input, xyzirc_input_ptr); const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( @@ -393,7 +425,7 @@ void PointCloudConcatenationComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -406,7 +438,7 @@ void PointCloudConcatenationComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index f933d4c6f75d9..4a7152a3cd38a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,8 +14,9 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "autoware/universe_utils/math/trigonometry.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" +#include #include namespace pointcloud_preprocessor @@ -181,6 +182,22 @@ bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointc "Required field time stamp doesn't exist in the point cloud."); return false; } + + if (!utils::is_data_layout_compatible_with_point_xyzircaedt(pointcloud)) { + RCLCPP_ERROR( + node_->get_logger(), + "The pointcloud layout is not compatible with PointXYZIRCAEDT. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyziradrt(pointcloud)) { + RCLCPP_ERROR( + node_->get_logger(), + "The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy " + "code/data"); + } + + return false; + } + return true; } @@ -193,10 +210,12 @@ void DistortionCorrector::undistortPointCloud( sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); - sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); - double prev_time_stamp_sec{*it_time_stamp}; - const double first_point_time_stamp_sec{*it_time_stamp}; + double prev_time_stamp_sec{ + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)}; + const double first_point_time_stamp_sec{ + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)}; std::deque::iterator it_twist; std::deque::iterator it_imu; @@ -214,29 +233,33 @@ void DistortionCorrector::undistortPointCloud( bool is_imu_time_stamp_too_late = false; bool is_twist_valid = true; bool is_imu_valid = true; + double global_point_stamp; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { is_twist_valid = true; is_imu_valid = true; + global_point_stamp = + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp); + // Get closest twist information - while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { + while (it_twist != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) { ++it_twist; twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { + if (std::abs(global_point_stamp - twist_stamp) > 0.1) { is_twist_time_stamp_too_late = true; is_twist_valid = false; } // Get closest IMU information if (use_imu && !angular_velocity_queue_.empty()) { - while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { + while (it_imu != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) { ++it_imu; imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { + if (std::abs(global_point_stamp - imu_stamp) > 0.1) { is_imu_time_stamp_too_late = true; is_imu_valid = false; } @@ -244,12 +267,12 @@ void DistortionCorrector::undistortPointCloud( is_imu_valid = false; } - float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); + float time_offset = static_cast(global_point_stamp - prev_time_stamp_sec); // Undistort a single point based on the strategy undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - prev_time_stamp_sec = *it_time_stamp; + prev_time_stamp_sec = global_point_stamp; } warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 9c07a4ec47cba..88a79edc9ce69 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -29,16 +29,27 @@ void FasterVoxelGridDownsampleFilter::set_voxel_size( Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z); } -void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input) +void FasterVoxelGridDownsampleFilter::set_field_offsets( + const PointCloud2ConstPtr & input, const rclcpp::Logger & logger) { x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; intensity_index_ = pcl::getFieldIndex(*input, "intensity"); + + if ( + intensity_index_ < 0 || input->fields[pcl::getFieldIndex(*input, "intensity")].datatype != + sensor_msgs::msg::PointField::UINT8) { + RCLCPP_ERROR( + logger, + "There is no intensity field in the input point cloud or the intensity field is not of type " + "UINT8."); + } + if (intensity_index_ != -1) { intensity_offset_ = input->fields[intensity_index_].offset; } else { - intensity_offset_ = z_offset_ + sizeof(float); + intensity_offset_ = -1; } offset_initialized_ = true; } @@ -49,7 +60,7 @@ void FasterVoxelGridDownsampleFilter::filter( { // Check if the field offset has been set if (!offset_initialized_) { - set_field_offsets(input); + set_field_offsets(input, logger); } // Compute the minimum and maximum voxel coordinates @@ -85,10 +96,9 @@ Eigen::Vector4f FasterVoxelGridDownsampleFilter::get_point_from_global_offset( const PointCloud2ConstPtr & input, size_t global_offset) { float intensity = 0.0; - if (intensity_index_ == -1) { - intensity = -1.0; - } else { - intensity = *reinterpret_cast(&input->data[global_offset + intensity_offset_]); + if (intensity_index_ >= 0) { + intensity = static_cast( + *reinterpret_cast(&input->data[global_offset + intensity_offset_])); } Eigen::Vector4f point( *reinterpret_cast(&input->data[global_offset + x_offset_]), @@ -179,7 +189,8 @@ void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( *reinterpret_cast(&output.data[output_data_size + x_offset_]) = centroid[0]; *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1]; *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2]; - *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3]; + *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = + static_cast(centroid[3]); output_data_size += output.point_step; } } diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index f11f37397a142..6e7469ff05f67 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -108,7 +108,7 @@ void VoxelGridDownsampleFilterComponent::faster_filter( std::scoped_lock lock(mutex_); FasterVoxelGridDownsampleFilter faster_voxel_filter; faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); - faster_voxel_filter.set_field_offsets(input); + faster_voxel_filter.set_field_offsets(input, this->get_logger()); faster_voxel_filter.filter(input, output, transform_info, this->get_logger()); } diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index 035be38f4c342..a0b1814995d8b 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -51,6 +51,8 @@ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -411,6 +413,30 @@ bool pointcloud_preprocessor::Filter::convert_output_costly(std::unique_ptrget_logger(), "[input_indices_callback] Invalid input!"); return; diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index d8f5c78ab1e73..1fa067ce75db6 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -28,7 +28,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZIRADRT; +using autoware_point_types::PointXYZIRCAEDT; using autoware_point_types::ReturnType; using diagnostic_msgs::msg::DiagnosticStatus; @@ -121,22 +121,22 @@ void DualReturnOutlierFilterComponent::filter( if (indices) { RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); } - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); uint32_t vertical_bins = vertical_bins_; uint32_t horizontal_bins = 36; - float max_azimuth = 36000.0f; + float max_azimuth = 2 * M_PI; float min_azimuth = 0.0f; switch (roi_mode_map_[roi_mode_]) { case 2: { - max_azimuth = max_azimuth_deg_ * 100.0; - min_azimuth = min_azimuth_deg_ * 100.0; + max_azimuth = max_azimuth_deg_ * (M_PI / 180.0); + min_azimuth = min_azimuth_deg_ * (M_PI / 180.0); break; } default: { - max_azimuth = 36000.0f; + max_azimuth = 2 * M_PI; min_azimuth = 0.0f; break; } @@ -145,13 +145,13 @@ void DualReturnOutlierFilterComponent::filter( uint32_t horizontal_resolution = static_cast((max_azimuth - min_azimuth) / horizontal_bins); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl_output->points.reserve(pcl_input->points.size()); - std::vector> pcl_input_ring_array; - std::vector> weak_first_pcl_input_ring_array; + std::vector> pcl_input_ring_array; + std::vector> weak_first_pcl_input_ring_array; - pcl::PointCloud::Ptr noise_output(new pcl::PointCloud); + pcl::PointCloud::Ptr noise_output(new pcl::PointCloud); noise_output->points.reserve(pcl_input->points.size()); pcl_input_ring_array.resize( vertical_bins); // TODO(davidw): this is for Pandar 40 only, make dynamic @@ -160,9 +160,9 @@ void DualReturnOutlierFilterComponent::filter( // Split into 36 x 10 degree bins x 40 lines (TODO: change to dynamic) for (const auto & p : pcl_input->points) { if (p.return_type == ReturnType::DUAL_WEAK_FIRST) { - weak_first_pcl_input_ring_array.at(p.ring).push_back(p); + weak_first_pcl_input_ring_array.at(p.channel).push_back(p); } else { - pcl_input_ring_array.at(p.ring).push_back(p); + pcl_input_ring_array.at(p.channel).push_back(p); } } @@ -175,16 +175,16 @@ void DualReturnOutlierFilterComponent::filter( } std::vector deleted_azimuths; std::vector deleted_distances; - pcl::PointCloud temp_segment; + pcl::PointCloud temp_segment; bool keep_next = false; - uint ring_id = weak_first_single_ring.points.front().ring; + uint ring_id = weak_first_single_ring.points.front().channel; for (auto iter = std::begin(weak_first_single_ring) + 1; iter != std::end(weak_first_single_ring) - 1; ++iter) { const float min_dist = std::min(iter->distance, (iter + 1)->distance); const float max_dist = std::max(iter->distance, (iter + 1)->distance); float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; if (max_dist < min_dist * weak_first_distance_ratio_ && azimuth_diff < max_azimuth_diff) { temp_segment.points.push_back(*iter); @@ -294,14 +294,14 @@ void DualReturnOutlierFilterComponent::filter( if (input_ring.size() < 2) { continue; } - pcl::PointCloud temp_segment; + pcl::PointCloud temp_segment; bool keep_next = false; // uint ring_id = input_ring.points.front().ring; for (auto iter = std::begin(input_ring) + 1; iter != std::end(input_ring) - 1; ++iter) { const float min_dist = std::min(iter->distance, (iter + 1)->distance); const float max_dist = std::max(iter->distance, (iter + 1)->distance); float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; if (max_dist < min_dist * general_distance_ratio_ && azimuth_diff < max_azimuth_diff) { temp_segment.points.push_back(*iter); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index a9722d041c42c..aeb5dfc1acdf7 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -82,24 +82,22 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); + output.point_step = sizeof(OutputPointType); output.data.resize(output.point_step * input->width); size_t output_size = 0; - pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); - - const auto ring_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - const auto azimuth_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; - const auto distance_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - const auto intensity_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; - const auto return_type_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).offset; - const auto time_stamp_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).offset; + pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); + + const auto input_channel_offset = + input->fields.at(static_cast(InputPointIndex::Channel)).offset; + const auto input_azimuth_offset = + input->fields.at(static_cast(InputPointIndex::Azimuth)).offset; + const auto input_distance_offset = + input->fields.at(static_cast(InputPointIndex::Distance)).offset; + const auto input_intensity_offset = + input->fields.at(static_cast(InputPointIndex::Intensity)).offset; + const auto input_return_type_offset = + input->fields.at(static_cast(InputPointIndex::ReturnType)).offset; std::vector> ring2indices; ring2indices.reserve(max_rings_num_); @@ -110,7 +108,8 @@ void RingOutlierFilterComponent::faster_filter( } for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { - const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); + const uint16_t ring = + *reinterpret_cast(&input->data[data_idx + input_channel_offset]); ring2indices[ring].push_back(data_idx); } @@ -132,30 +131,30 @@ void RingOutlierFilterComponent::faster_filter( // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) const float & current_azimuth = - *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); + *reinterpret_cast(&input->data[current_data_idx + input_azimuth_offset]); const float & next_azimuth = - *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); + *reinterpret_cast(&input->data[next_data_idx + input_azimuth_offset]); float azimuth_diff = next_azimuth - current_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; const float & current_distance = - *reinterpret_cast(&input->data[current_data_idx + distance_offset]); + *reinterpret_cast(&input->data[current_data_idx + input_distance_offset]); const float & next_distance = - *reinterpret_cast(&input->data[next_data_idx + distance_offset]); + *reinterpret_cast(&input->data[next_data_idx + input_distance_offset]); if ( std::max(current_distance, next_distance) < std::min(current_distance, next_distance) * distance_ratio_ && - azimuth_diff < 100.f) { - continue; // Determined to be included in the same walk + azimuth_diff < 1.0 * (180.0 / M_PI)) { // one degree + continue; // Determined to be included in the same walk } if (isCluster( input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), walk_last_idx - walk_first_idx + 1)) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); @@ -164,41 +163,38 @@ void RingOutlierFilterComponent::faster_filter( output_ptr->y = p[1]; output_ptr->z = p[2]; } else { - *output_ptr = *input_ptr; + output_ptr->x = input_ptr->x; + output_ptr->y = input_ptr->y; + output_ptr->z = input_ptr->z; } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + const std::uint8_t & intensity = *reinterpret_cast( + &input->data[indices[i] + input_intensity_offset]); output_ptr->intensity = intensity; + const std::uint8_t & return_type = *reinterpret_cast( + &input->data[indices[i] + input_return_type_offset]); + output_ptr->return_type = return_type; + + const std::uint8_t & channel = *reinterpret_cast( + &input->data[indices[i] + input_channel_offset]); + output_ptr->channel = channel; + output_size += output.point_step; } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - PointXYZIRADRT outlier_point; auto input_ptr = - reinterpret_cast(&input->data[indices[walk_first_idx]]); + reinterpret_cast(&input->data[indices[walk_first_idx]]); + InputPointType outlier_point = *input_ptr; + if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; outlier_point.x = p[0]; outlier_point.y = p[1]; outlier_point.z = p[2]; - } else { - outlier_point = *input_ptr; } - outlier_point.intensity = *reinterpret_cast( - &input->data[indices[walk_first_idx] + intensity_offset]); - outlier_point.ring = *reinterpret_cast( - &input->data[indices[walk_first_idx] + ring_offset]); - outlier_point.azimuth = *reinterpret_cast( - &input->data[indices[walk_first_idx] + azimuth_offset]); - outlier_point.distance = *reinterpret_cast( - &input->data[indices[walk_first_idx] + distance_offset]); - outlier_point.return_type = *reinterpret_cast( - &input->data[indices[walk_first_idx] + return_type_offset]); - outlier_point.time_stamp = *reinterpret_cast( - &input->data[indices[walk_first_idx] + time_stamp_offset]); outlier_pcl->push_back(outlier_point); } } @@ -212,8 +208,8 @@ void RingOutlierFilterComponent::faster_filter( input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), walk_last_idx - walk_first_idx + 1)) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); @@ -222,46 +218,42 @@ void RingOutlierFilterComponent::faster_filter( output_ptr->y = p[1]; output_ptr->z = p[2]; } else { - *output_ptr = *input_ptr; + output_ptr->x = input_ptr->x; + output_ptr->y = input_ptr->y; + output_ptr->z = input_ptr->z; } const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + *reinterpret_cast(&input->data[indices[i] + input_intensity_offset]); output_ptr->intensity = intensity; + const std::uint8_t & return_type = *reinterpret_cast( + &input->data[indices[i] + input_return_type_offset]); + output_ptr->return_type = return_type; + + const std::uint8_t & channel = + *reinterpret_cast(&input->data[indices[i] + input_channel_offset]); + output_ptr->channel = channel; + output_size += output.point_step; } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i < walk_last_idx; i++) { - PointXYZIRADRT outlier_point; - - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + InputPointType outlier_point = *input_ptr; if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; outlier_point.x = p[0]; outlier_point.y = p[1]; outlier_point.z = p[2]; - } else { - outlier_point = *input_ptr; } - outlier_point.intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - outlier_point.ring = - *reinterpret_cast(&input->data[indices[i] + ring_offset]); - outlier_point.azimuth = - *reinterpret_cast(&input->data[indices[i] + azimuth_offset]); - outlier_point.distance = - *reinterpret_cast(&input->data[indices[i] + distance_offset]); - outlier_point.return_type = - *reinterpret_cast(&input->data[indices[i] + return_type_offset]); - outlier_point.time_stamp = - *reinterpret_cast(&input->data[indices[i] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } } - setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); + setUpPointCloudFormat(input, output, output_size); if (publish_outlier_pointcloud_) { PointCloud2 outlier; @@ -351,8 +343,7 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba } void RingOutlierFilterComponent::setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields) + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size) { formatted_points.data.resize(points_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -365,40 +356,40 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( formatted_points.is_bigendian = input->is_bigendian; formatted_points.is_dense = input->is_dense; - sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points); - pcd_modifier.setPointCloud2Fields( - num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, - sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + // This is a hack to get the correct fields in the output point cloud without creating the fields + // manually + sensor_msgs::msg::PointCloud2 msg_aux; + pcl::toROSMsg(pcl::PointCloud(), msg_aux); + formatted_points.fields = msg_aux.fields; } float RingOutlierFilterComponent::calculateVisibilityScore( const sensor_msgs::msg::PointCloud2 & input) { - pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); pcl::fromROSMsg(input, *input_cloud); const uint32_t vertical_bins = vertical_bins_; const uint32_t horizontal_bins = horizontal_bins_; - const float max_azimuth = max_azimuth_deg_ * 100.0; - const float min_azimuth = min_azimuth_deg_ * 100.0; + const float max_azimuth = max_azimuth_deg_ * (M_PI / 180.f); + const float min_azimuth = min_azimuth_deg_ * (M_PI / 180.f); const uint32_t horizontal_resolution = static_cast((max_azimuth - min_azimuth) / horizontal_bins); - std::vector> ring_point_clouds(vertical_bins); + std::vector> ring_point_clouds(vertical_bins); cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); // Split points into rings for (const auto & point : input_cloud->points) { - ring_point_clouds.at(point.ring).push_back(point); + ring_point_clouds.at(point.channel).push_back(point); } // Calculate frequency for each bin in each ring for (const auto & ring_points : ring_point_clouds) { if (ring_points.empty()) continue; - const uint ring_id = ring_points.front().ring; + const uint ring_id = ring_points.front().channel; std::vector frequency_in_ring(horizontal_bins, 0); for (const auto & point : ring_points.points) { diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 35c35471bf30e..0c9aa202f2afa 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -430,39 +430,56 @@ void PointCloudDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudDataSynchronizerComponent::convertToXYZICloud( +void PointCloudDataSynchronizerComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -489,9 +506,9 @@ void PointCloudDataSynchronizerComponent::cloud_callback( { std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); if (input->data.size() > 0) { - convertToXYZICloud(input, xyzi_input_ptr); + convertToXYZIRCCloud(input, xyzirc_input_ptr); } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); @@ -500,7 +517,7 @@ void PointCloudDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -513,7 +530,7 @@ void PointCloudDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/geometry.cpp similarity index 52% rename from sensing/pointcloud_preprocessor/src/utility/utilities.cpp rename to sensing/pointcloud_preprocessor/src/utility/geometry.cpp index ec84a2467db78..e3013d05d925f 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/geometry.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// 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. @@ -12,9 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/utilities.hpp" - -#include +#include "pointcloud_preprocessor/utility/geometry.hpp" namespace pointcloud_preprocessor::utils { @@ -157,91 +155,4 @@ bool point_within_cgal_polys( return false; } -bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud2 & input) -{ - using autoware_point_types::PointIndex; - using autoware_point_types::PointXYZI; - if (input.fields.size() < 4) { - return false; - } - bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); - same_layout &= field_x.name == "x"; - same_layout &= field_x.offset == offsetof(PointXYZI, x); - same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); - same_layout &= field_y.name == "y"; - same_layout &= field_y.offset == offsetof(PointXYZI, y); - same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); - same_layout &= field_z.name == "z"; - same_layout &= field_z.offset == offsetof(PointXYZI, z); - same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); - same_layout &= field_intensity.name == "intensity"; - same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity); - same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_intensity.count == 1; - return same_layout; -} - -bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input) -{ - using autoware_point_types::PointIndex; - using autoware_point_types::PointXYZIRADRT; - if (input.fields.size() < 9) { - return false; - } - bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); - same_layout &= field_x.name == "x"; - same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x); - same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); - same_layout &= field_y.name == "y"; - same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y); - same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); - same_layout &= field_z.name == "z"; - same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z); - same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); - same_layout &= field_intensity.name == "intensity"; - same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity); - same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_intensity.count == 1; - const auto & field_ring = input.fields.at(static_cast(PointIndex::Ring)); - same_layout &= field_ring.name == "ring"; - same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring); - same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; - same_layout &= field_ring.count == 1; - const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); - same_layout &= field_azimuth.name == "azimuth"; - same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth); - same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_azimuth.count == 1; - const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); - same_layout &= field_distance.name == "distance"; - same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance); - same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_distance.count == 1; - const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); - same_layout &= field_return_type.name == "return_type"; - same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type); - same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; - same_layout &= field_return_type.count == 1; - const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); - same_layout &= field_time_stamp.name == "time_stamp"; - same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp); - same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; - same_layout &= field_time_stamp.count == 1; - return same_layout; -} - } // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/src/utility/memory.cpp b/sensing/pointcloud_preprocessor/src/utility/memory.cpp new file mode 100644 index 0000000000000..138573a2014ff --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/utility/memory.cpp @@ -0,0 +1,211 @@ +// 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. + +#include "pointcloud_preprocessor/utility/memory.hpp" + +#include + +namespace pointcloud_preprocessor::utils +{ +bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIIndex; + using autoware_point_types::PointXYZI; + if (input.fields.size() < 4) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZI, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZI, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZI, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRCIndex; + using autoware_point_types::PointXYZIRC; + if (input.fields.size() < 6) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRC, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRC, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRC, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRC, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_intensity.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRC, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + same_layout &= field_ring.name == "channel"; + same_layout &= field_ring.offset == offsetof(PointXYZIRC, channel); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRADRTIndex; + using autoware_point_types::PointXYZIRADRT; + if (input.fields.size() < 9) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Ring)); + same_layout &= field_ring.name == "ring"; + same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + same_layout &= field_azimuth.name == "azimuth"; + same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth); + same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_azimuth.count == 1; + const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + same_layout &= field_distance.name == "distance"; + same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance); + same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_distance.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + same_layout &= field_time_stamp.name == "time_stamp"; + same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp); + same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; + same_layout &= field_time_stamp.count == 1; + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRCAEDTIndex; + using autoware_point_types::PointXYZIRCAEDT; + if (input.fields.size() != 10) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRCAEDT, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRCAEDT, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRCAEDT, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRCAEDT, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_intensity.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRCAEDT, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + same_layout &= field_ring.name == "channel"; + same_layout &= field_ring.offset == offsetof(PointXYZIRCAEDT, channel); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + same_layout &= field_azimuth.name == "azimuth"; + same_layout &= field_azimuth.offset == offsetof(PointXYZIRCAEDT, azimuth); + same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_azimuth.count == 1; + const auto & field_elevation = input.fields.at(static_cast(PointIndex::Elevation)); + same_layout &= field_elevation.name == "elevation"; + same_layout &= field_elevation.offset == offsetof(PointXYZIRCAEDT, elevation); + same_layout &= field_elevation.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_elevation.count == 1; + const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + same_layout &= field_distance.name == "distance"; + same_layout &= field_distance.offset == offsetof(PointXYZIRCAEDT, distance); + same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_distance.count == 1; + const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + same_layout &= field_time_stamp.name == "time_stamp"; + same_layout &= field_time_stamp.offset == offsetof(PointXYZIRCAEDT, time_stamp); + same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::UINT32; + same_layout &= field_time_stamp.count == 1; + return same_layout; +} + +} // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index 7997aaf43c202..3b500bfb3e982 100644 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -58,7 +58,7 @@ class DistortionCorrectorTest : public ::testing::Test // Spin the node for a while to ensure transforms are published auto start = std::chrono::steady_clock::now(); - auto timeout = std::chrono::seconds(1); + auto timeout = std::chrono::milliseconds(100); while (std::chrono::steady_clock::now() - start < timeout) { rclcpp::spin_some(node_); std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -194,19 +194,24 @@ class DistortionCorrectorTest : public ::testing::Test }}; // Generate timestamps for the points - std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); + std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); modifier.setPointCloud2Fields( - 4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + 10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "time_stamp", 1, sensor_msgs::msg::PointField::FLOAT64); + "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1, + sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16, + "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1, + sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, + "time_stamp", 1, sensor_msgs::msg::PointField::UINT32); + modifier.resize(number_of_points_); sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); for (size_t i = 0; i < number_of_points_; ++i) { *iter_x = points[i].x(); @@ -226,15 +231,15 @@ class DistortionCorrectorTest : public ::testing::Test return pointcloud_msg; } - std::vector generatePointTimestamps( + std::vector generatePointTimestamps( rclcpp::Time pointcloud_timestamp, size_t number_of_points) { - std::vector timestamps; - rclcpp::Time point_stamp = pointcloud_timestamp; + std::vector timestamps; + rclcpp::Time global_point_stamp = pointcloud_timestamp; for (size_t i = 0; i < number_of_points; ++i) { - double timestamp = point_stamp.seconds(); - timestamps.push_back(timestamp); - point_stamp = addMilliseconds(point_stamp, points_interval_ms_); + std::uint32_t relative_timestamp = (global_point_stamp - pointcloud_timestamp).nanoseconds(); + timestamps.push_back(relative_timestamp); + global_point_stamp = addMilliseconds(global_point_stamp, points_interval_ms_); } return timestamps; @@ -735,11 +740,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion) sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); std::vector expected_points; for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { - double time_offset = *iter_t - timestamp.seconds(); + double time_offset = static_cast(*iter_t) / 1e9; expected_points.emplace_back( *iter_x + static_cast(velocity * time_offset), *iter_y, *iter_z); } @@ -822,11 +827,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); std::vector expected_pointcloud; for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { - double time_offset = *iter_t - timestamp.seconds(); + double time_offset = static_cast(*iter_t) / 1e9; float angle = angular_velocity * time_offset; // Set the quaternion for the current angle diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/pointcloud_preprocessor/test/test_utilities.cpp index 68c86dfbd0505..e3075b4b11e79 100644 --- a/sensing/pointcloud_preprocessor/test/test_utilities.cpp +++ b/sensing/pointcloud_preprocessor/test/test_utilities.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include