feat(euclidean_cluster): use point_cloud_msg_wrapper

common/autoware_auto_common/include/common/types.hpp

@@ -109,6 +109,54 @@ struct COMMON_PUBLIC PointXYZI
   }
 };
 
+struct COMMON_PUBLIC PointXYZ
+{
+  float32_t x{0.0F};
+  float32_t y{0.0F};
+  float32_t z{0.0F};
+  friend bool operator==(const PointXYZ & p1, const PointXYZ & 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());
+  }
+};
+
+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

perception/euclidean_cluster/lib/utils.cpp

@@ -13,6 +13,9 @@
 // limitations under the License.
 #include "euclidean_cluster/utils.hpp"
 
+#include <common/types.hpp>
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
 #include <sensor_msgs/msg/point_field.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
@@ -27,12 +30,11 @@ geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & poin
   centroid.x = 0.0f;
   centroid.y = 0.0f;
   centroid.z = 0.0f;
-  for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(pointcloud, "x"),
-       iter_y(pointcloud, "y"), iter_z(pointcloud, "z");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
-    centroid.x += *iter_x;
-    centroid.y += *iter_y;
-    centroid.z += *iter_z;
+  point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{pointcloud};
+  for (const auto & point : view) {
+    centroid.x += point.x;
+    centroid.y += point.y;
+    centroid.z += point.z;
   }
   const size_t size = pointcloud.width * pointcloud.height;
   centroid.x = centroid.x / static_cast<float>(size);
@@ -73,40 +75,20 @@ void convertObjectMsg2SensorMsg(
     pointcloud_size += feature_object.feature.cluster.width * feature_object.feature.cluster.height;
   }
 
-  sensor_msgs::PointCloud2Modifier modifier(output);
-  modifier.setPointCloud2Fields(
-    4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32,
-    "z", 1, sensor_msgs::msg::PointField::FLOAT32, "rgb", 1, sensor_msgs::msg::PointField::FLOAT32);
-  modifier.resize(pointcloud_size);
-
-  sensor_msgs::PointCloud2Iterator<float> iter_out_x(output, "x");
-  sensor_msgs::PointCloud2Iterator<float> iter_out_y(output, "y");
-  sensor_msgs::PointCloud2Iterator<float> iter_out_z(output, "z");
-  sensor_msgs::PointCloud2Iterator<uint8_t> iter_out_r(output, "r");
-  sensor_msgs::PointCloud2Iterator<uint8_t> iter_out_g(output, "g");
-  sensor_msgs::PointCloud2Iterator<uint8_t> iter_out_b(output, "b");
+  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,
                                     200, 200, 0, 200, 0,   200, 0, 200, 200};  // 6 pattern
   for (size_t i = 0; i < input.feature_objects.size(); ++i) {
     const auto & feature_object = input.feature_objects.at(i);
-    sensor_msgs::PointCloud2ConstIterator<float> iter_in_x(feature_object.feature.cluster, "x");
-    sensor_msgs::PointCloud2ConstIterator<float> iter_in_y(feature_object.feature.cluster, "y");
-    sensor_msgs::PointCloud2ConstIterator<float> iter_in_z(feature_object.feature.cluster, "z");
-    for (; iter_in_x != iter_in_x.end(); ++iter_in_x, ++iter_in_y, ++iter_in_z, ++iter_out_x,
-                                         ++iter_out_y, ++iter_out_z, ++iter_out_r, ++iter_out_g,
-                                         ++iter_out_b) {
-      *iter_out_x = *iter_in_x;
-      *iter_out_y = *iter_in_y;
-      *iter_out_z = *iter_in_z;
-      *iter_out_r = color_data[3 * (i % 6) + 0];
-      *iter_out_g = color_data[3 * (i % 6) + 1];
-      *iter_out_b = color_data[3 * (i % 6) + 2];
+    point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{
+      feature_object.feature.cluster};
+    for (const auto & point : view) {
+      modifier.push_back(
+        {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});
     }
   }
-
-  output.width = pointcloud_size;
-  output.height = 1;
-  output.is_dense = false;
 }
 }  // namespace euclidean_cluster

perception/euclidean_cluster/package.xml

@@ -11,6 +11,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_auto_common</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>compare_map_segmentation</depend>
   <depend>geometry_msgs</depend>