From 0328e3467f7200316da709ce8474a8561c7b9012 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 23 Apr 2024 16:45:13 +0900 Subject: [PATCH 1/4] add PointXYZIRC Signed-off-by: a-maumau --- .../include/autoware_point_types/types.hpp | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) 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..d122ba4059551 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,29 @@ enum ReturnType : uint8_t { DUAL_ONLY, }; +struct PointXYZIRC +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + float intensity{0.0F}; + union { // for memory alignment + uint16_t _data; + struct + { + uint8_t padding{0U}; + uint8_t return_type{0U}; + }; + }; + 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) && float_eq(p1.intensity, p2.intensity) && + p1.return_type == p2.return_type && p1.channel == p2.channel; + } +}; + struct PointXYZIRADRT { float x{0.0F}; @@ -78,10 +101,16 @@ struct PointXYZIRADRT enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp }; LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); 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); +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, @@ -90,6 +119,12 @@ using PointXYZIRADRTGenerator = std::tuple< } // namespace autoware_point_types +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware_point_types::PointXYZIRC, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( + std::uint8_t, padding, padding)(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)( From 33341bb0900723844ba4b652a21fc0d2cf7f72bd Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 23 Apr 2024 16:48:58 +0900 Subject: [PATCH 2/4] fix RingOutlierFilter and ConcatenateDataSynchronizer to use PointXYZIRC Signed-off-by: a-maumau --- .../concatenate_and_time_sync_nodelet.hpp | 1 + .../ring_outlier_filter_nodelet.hpp | 7 +- .../concatenate_and_time_sync_nodelet.cpp | 15 ++-- .../ring_outlier_filter_nodelet.cpp | 70 ++++++++++++++----- 4 files changed, 67 insertions(+), 26 deletions(-) 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 84b056bec37b7..73e27bef1a1ab 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 @@ -88,6 +88,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 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 c871f1acbe5b9..2c70cc49bef16 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 @@ -27,7 +27,8 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRADRT; using point_cloud_msg_wrapper::PointCloud2Modifier; class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter @@ -64,8 +65,8 @@ 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; 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 402ceb65f6594..d73ea68816148 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 @@ -420,7 +420,14 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( not_subscribed_topic_names_.insert(e.first); } } + + // after concatenation, channels have no consistent meaning + size_t channel_offset = offsetof(PointXYZIRC, channel); + for (size_t data_idx = 0; data_idx < concat_cloud_ptr->data.size(); data_idx += concat_cloud_ptr->point_step) { + concat_cloud_ptr->data[data_idx + channel_offset] = 0; + } concat_cloud_ptr->header.stamp = oldest_stamp; + return transformed_clouds; } @@ -545,14 +552,10 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_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); } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); @@ -561,7 +564,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] = input; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -574,7 +577,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = input; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), 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 762c62dc03b39..45bbdeb8bec01 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 @@ -69,7 +69,7 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); + output.point_step = sizeof(PointXYZIRC); output.data.resize(output.point_step * input->width); size_t output_size = 0; @@ -77,7 +77,7 @@ void RingOutlierFilterComponent::faster_filter( PointCloud2 outlier_points; size_t outlier_points_size = 0; if (publish_outlier_pointcloud_) { - outlier_points.point_step = sizeof(PointXYZI); + outlier_points.point_step = sizeof(PointXYZIRC); outlier_points.data.resize(outlier_points.point_step * input->width); } @@ -89,6 +89,8 @@ void RingOutlierFilterComponent::faster_filter( 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; std::vector> ring2indices; ring2indices.reserve(max_rings_num_); @@ -143,8 +145,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); @@ -153,20 +155,28 @@ 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]); output_ptr->intensity = intensity; + const uint8_t & return_type = + *reinterpret_cast(&input->data[indices[i] + return_type_offset]); + output_ptr->return_type = return_type; + const uint16_t & channel = + *reinterpret_cast(&input->data[indices[i] + ring_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++) { auto outlier_ptr = - reinterpret_cast(&outlier_points.data[outlier_points_size]); + reinterpret_cast(&outlier_points.data[outlier_points_size]); auto input_ptr = - reinterpret_cast(&input->data[indices[walk_first_idx]]); + reinterpret_cast(&input->data[indices[walk_first_idx]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; @@ -174,11 +184,19 @@ void RingOutlierFilterComponent::faster_filter( outlier_ptr->y = p[1]; outlier_ptr->z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_ptr->x = input_ptr->x; + outlier_ptr->y = input_ptr->y; + outlier_ptr->z = input_ptr->z; } const float & intensity = *reinterpret_cast( &input->data[indices[walk_first_idx] + intensity_offset]); outlier_ptr->intensity = intensity; + const uint8_t & return_type = *reinterpret_cast( + &input->data[indices[walk_first_idx] + return_type_offset]); + outlier_ptr->return_type = return_type; + const uint16_t & channel = *reinterpret_cast( + &input->data[indices[walk_first_idx] + ring_offset]); + outlier_ptr->channel = channel; outlier_points_size += outlier_points.point_step; } @@ -193,8 +211,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); @@ -203,18 +221,26 @@ 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]); output_ptr->intensity = intensity; + const uint8_t & return_type = + *reinterpret_cast(&input->data[indices[i] + return_type_offset]); + output_ptr->return_type = return_type; + const uint16_t & channel = + *reinterpret_cast(&input->data[indices[i] + ring_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++) { - auto outlier_ptr = reinterpret_cast(&outlier_points.data[outlier_points_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto outlier_ptr = reinterpret_cast(&outlier_points.data[outlier_points_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); p = transform_info.eigen_transform * p; @@ -222,20 +248,28 @@ void RingOutlierFilterComponent::faster_filter( outlier_ptr->y = p[1]; outlier_ptr->z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_ptr->x = input_ptr->x; + outlier_ptr->y = input_ptr->y; + outlier_ptr->z = input_ptr->z; } const float & intensity = *reinterpret_cast(&input->data[indices[i] + intensity_offset]); outlier_ptr->intensity = intensity; + const uint8_t & return_type = + *reinterpret_cast(&input->data[indices[i] + return_type_offset]); + outlier_ptr->return_type = return_type; + const uint16_t & channel = + *reinterpret_cast(&input->data[indices[i] + ring_offset]); + outlier_ptr->channel = channel; outlier_points_size += outlier_points.point_step; } } } - setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); + setUpPointCloudFormat(input, output, output_size, /*num_fields=*/7); if (publish_outlier_pointcloud_) { - setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4); + setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/7); outlier_pointcloud_publisher_->publish(outlier_points); } @@ -316,7 +350,9 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( 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); + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, "padding", 1, + sensor_msgs::msg::PointField::UINT8, "return_type", 1, sensor_msgs::msg::PointField::UINT8, + "channel", 1, sensor_msgs::msg::PointField::UINT16); } } // namespace pointcloud_preprocessor From 501a1242bbe3cb733184ca62d371346725686b01 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 23 Apr 2024 08:29:24 +0000 Subject: [PATCH 3/4] style(pre-commit): autofix --- .../include/autoware_point_types/types.hpp | 8 ++++---- .../outlier_filter/ring_outlier_filter_nodelet.hpp | 8 +++++--- .../concatenate_and_time_sync_nodelet.cpp | 3 ++- .../src/outlier_filter/ring_outlier_filter_nodelet.cpp | 3 ++- 4 files changed, 13 insertions(+), 9 deletions(-) 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 d122ba4059551..215db7d96825f 100644 --- a/common/autoware_point_types/include/autoware_point_types/types.hpp +++ b/common/autoware_point_types/include/autoware_point_types/types.hpp @@ -60,7 +60,7 @@ struct PointXYZIRC float y{0.0F}; float z{0.0F}; float intensity{0.0F}; - union { // for memory alignment + union { // for memory alignment uint16_t _data; struct { @@ -121,9 +121,9 @@ using PointXYZIRADRTGenerator = std::tuple< POINT_CLOUD_REGISTER_POINT_STRUCT( autoware_point_types::PointXYZIRC, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( - std::uint8_t, padding, padding)(std::uint8_t, return_type, return_type)( - std::uint16_t, channel, channel)) + (float, x, + x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint8_t, padding, padding)( + std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) POINT_CLOUD_REGISTER_POINT_STRUCT( autoware_point_types::PointXYZIRADRT, 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 2c70cc49bef16..184d88853f36e 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 @@ -27,8 +27,8 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZIRC; using autoware_point_types::PointXYZIRADRT; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter @@ -65,8 +65,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; 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 d73ea68816148..88674e4da53f7 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 @@ -423,7 +423,8 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( // after concatenation, channels have no consistent meaning size_t channel_offset = offsetof(PointXYZIRC, channel); - for (size_t data_idx = 0; data_idx < concat_cloud_ptr->data.size(); data_idx += concat_cloud_ptr->point_step) { + for (size_t data_idx = 0; data_idx < concat_cloud_ptr->data.size(); + data_idx += concat_cloud_ptr->point_step) { concat_cloud_ptr->data[data_idx + channel_offset] = 0; } concat_cloud_ptr->header.stamp = oldest_stamp; 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 45bbdeb8bec01..8e2d048607823 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 @@ -239,7 +239,8 @@ void RingOutlierFilterComponent::faster_filter( } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i < walk_last_idx; i++) { - auto outlier_ptr = reinterpret_cast(&outlier_points.data[outlier_points_size]); + auto outlier_ptr = + reinterpret_cast(&outlier_points.data[outlier_points_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); From 83e6d64535b318f5993bfbcab69880cf776fa7cb Mon Sep 17 00:00:00 2001 From: a-maumau Date: Tue, 7 May 2024 17:02:45 +0900 Subject: [PATCH 4/4] fix intensity of PointXYZIRC from float to uint8 Signed-off-by: a-maumau --- .../include/autoware_point_types/types.hpp | 16 ++++------------ .../concatenate_and_time_sync_nodelet.cpp | 4 ++-- .../ring_outlier_filter_nodelet.cpp | 17 ++++++++--------- 3 files changed, 14 insertions(+), 23 deletions(-) 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 215db7d96825f..067ebe04e1339 100644 --- a/common/autoware_point_types/include/autoware_point_types/types.hpp +++ b/common/autoware_point_types/include/autoware_point_types/types.hpp @@ -59,20 +59,13 @@ struct PointXYZIRC float x{0.0F}; float y{0.0F}; float z{0.0F}; - float intensity{0.0F}; - union { // for memory alignment - uint16_t _data; - struct - { - uint8_t padding{0U}; - uint8_t return_type{0U}; - }; - }; + uint8_t intensity{0.0F}; + uint8_t return_type{0U}; 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) && float_eq(p1.intensity, p2.intensity) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && p1.return_type == p2.return_type && p1.channel == p2.channel; } }; @@ -121,8 +114,7 @@ using PointXYZIRADRTGenerator = std::tuple< POINT_CLOUD_REGISTER_POINT_STRUCT( autoware_point_types::PointXYZIRC, - (float, x, - x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint8_t, padding, padding)( + (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( 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 88674e4da53f7..cb386ca357921 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 @@ -512,7 +512,7 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "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; point.x = *it_x; @@ -527,7 +527,7 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; + point.intensity = 0U; output_modifier.push_back(std::move(point)); } } 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 8e2d048607823..7a04f361b000c 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 @@ -161,7 +161,7 @@ void RingOutlierFilterComponent::faster_filter( } const float & intensity = *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; + output_ptr->intensity = static_cast(intensity); const uint8_t & return_type = *reinterpret_cast(&input->data[indices[i] + return_type_offset]); output_ptr->return_type = return_type; @@ -190,7 +190,7 @@ void RingOutlierFilterComponent::faster_filter( } const float & intensity = *reinterpret_cast( &input->data[indices[walk_first_idx] + intensity_offset]); - outlier_ptr->intensity = intensity; + outlier_ptr->intensity = static_cast(intensity); const uint8_t & return_type = *reinterpret_cast( &input->data[indices[walk_first_idx] + return_type_offset]); outlier_ptr->return_type = return_type; @@ -227,7 +227,7 @@ void RingOutlierFilterComponent::faster_filter( } const float & intensity = *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; + output_ptr->intensity = static_cast(intensity); const uint8_t & return_type = *reinterpret_cast(&input->data[indices[i] + return_type_offset]); output_ptr->return_type = return_type; @@ -255,7 +255,7 @@ void RingOutlierFilterComponent::faster_filter( } const float & intensity = *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - outlier_ptr->intensity = intensity; + outlier_ptr->intensity = static_cast(intensity); const uint8_t & return_type = *reinterpret_cast(&input->data[indices[i] + return_type_offset]); outlier_ptr->return_type = return_type; @@ -267,10 +267,10 @@ void RingOutlierFilterComponent::faster_filter( } } - setUpPointCloudFormat(input, output, output_size, /*num_fields=*/7); + setUpPointCloudFormat(input, output, output_size, /*num_fields=*/6); if (publish_outlier_pointcloud_) { - setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/7); + setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/6); outlier_pointcloud_publisher_->publish(outlier_points); } @@ -351,9 +351,8 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( 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, "padding", 1, - sensor_msgs::msg::PointField::UINT8, "return_type", 1, sensor_msgs::msg::PointField::UINT8, - "channel", 1, sensor_msgs::msg::PointField::UINT16); + "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1, + sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16); } } // namespace pointcloud_preprocessor