Use point_cloud_msg_wrapper::PointCloud2View

Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp>
autowarefoundation · Jul 6, 2022 · 04a6229 · 04a6229
1 parent 502d3ca
commit 04a6229
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 12 deletions.
diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/common/types.hpp
@@ -109,6 +109,24 @@ 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());
+  }
+};
+
 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
@@ -80,23 +80,16 @@ void convertObjectMsg2SensorMsg(
   point_cloud_msg_wrapper::PointCloud2Modifier<autoware::common::types::PointXYZI> modifier{output,
     "euclidean_cluster_cloud"};
 
-  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");
-
   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) {
+    point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{feature_object.
+      feature
+      .cluster};
+    for (const auto & point : view) {
       modifier.push_back(
-        {*iter_in_x, *iter_in_y, *iter_in_z,
+        {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))});