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