fix intensity of PointXYZIRC from float to uint8

autowarefoundation · May 7, 2024 · ab5777b · ab5777b
1 parent 501a124
commit ab5777b
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 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
@@ -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<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
-           float_eq<float>(p1.z, p2.z) && float_eq<float>(p1.intensity, p2.intensity) &&
+           float_eq<float>(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
@@ -512,7 +512,7 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud(
   sensor_msgs::PointCloud2Iterator<float> it_z(*input_ptr, "z");
 
   if (has_intensity) {
-    sensor_msgs::PointCloud2Iterator<float> it_i(*input_ptr, "intensity");
+    sensor_msgs::PointCloud2Iterator<uint8_t> 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
@@ -161,7 +161,7 @@ void RingOutlierFilterComponent::faster_filter(
           }
           const float & intensity =
             *reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
-          output_ptr->intensity = intensity;
+          output_ptr->intensity = static_cast<uint8_t>(intensity);
           const uint8_t & return_type =
             *reinterpret_cast<const uint8_t *>(&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<const float *>(
             &input->data[indices[walk_first_idx] + intensity_offset]);
-          outlier_ptr->intensity = intensity;
+          outlier_ptr->intensity = static_cast<uint8_t>(intensity);
           const uint8_t & return_type = *reinterpret_cast<const uint8_t *>(
             &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<const float *>(&input->data[indices[i] + intensity_offset]);
-        output_ptr->intensity = intensity;
+        output_ptr->intensity = static_cast<uint8_t>(intensity);
         const uint8_t & return_type =
           *reinterpret_cast<const uint8_t *>(&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<const float *>(&input->data[indices[i] + intensity_offset]);
-        outlier_ptr->intensity = intensity;
+        outlier_ptr->intensity = static_cast<uint8_t>(intensity);
         const uint8_t & return_type =
           *reinterpret_cast<const uint8_t *>(&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