Use four uint8_t for representing RGB(A) values instead of a float32_t

Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp>
autowarefoundation · Jul 13, 2022 · 256e891 · 256e891
1 parent eabbad8
commit 256e891
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 5 deletions.
diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/common/types.hpp
@@ -127,6 +127,36 @@ struct COMMON_PUBLIC PointXYZ
   }
 };
 
+struct COMMON_PUBLIC PointXYZRGBA
+{
+  float32_t x{0.0F};
+  float32_t y{0.0F};
+  float32_t z{0.0F};
+  uint8_t r{0};
+  uint8_t g{0};
+  uint8_t b{0};
+  uint8_t a{0};
+  friend bool operator==(const PointXYZRGBA & p1, const PointXYZRGBA & p2) noexcept
+  {
+    return helper_functions::comparisons::rel_eq(
+             p1.x, p2.x, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.y, p2.y, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.z, p2.z, std::numeric_limits<float32_t>::epsilon()) &&
+
+           (p1.r == p2.r) &&
+
+           (p1.g == p2.g) &&
+
+           (p1.b == p2.b) &&
+
+           (p1.a == p2.a);
+  }
+};
+
 using PointBlock = std::vector<PointXYZIF>;
 using PointPtrBlock = std::vector<const PointXYZIF *>;
 /// \brief Stores basic configuration information, does some simple validity checking

diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp
@@ -75,7 +75,7 @@ void convertObjectMsg2SensorMsg(
     pointcloud_size += feature_object.feature.cluster.width * feature_object.feature.cluster.height;
   }
 
-  point_cloud_msg_wrapper::PointCloud2Modifier<autoware::common::types::PointXYZI> modifier{
+  point_cloud_msg_wrapper::PointCloud2Modifier<autoware::common::types::PointXYZRGBA> modifier{
     output, "euclidean_cluster_cloud"};
 
   constexpr uint8_t color_data[] = {200, 0,   0, 0,   200, 0,   0, 0,   200,
@@ -86,10 +86,8 @@ void convertObjectMsg2SensorMsg(
       feature_object.feature.cluster};
     for (const auto & point : view) {
       modifier.push_back(
-        {point.x, point.y, point.z,
-         static_cast<float>(
-           (color_data[3 * (i % 6) + 0] << 3) + (color_data[3 * (i % 6) + 1] << 2) +
-           (color_data[3 * (i % 6) + 2] << 1))});
+        {point.x, point.y, point.z, (color_data[3 * (i % 6) + 0]), (color_data[3 * (i % 6) + 1]),
+         (color_data[3 * (i % 6) + 2]), 0});
     }
   }
 }