fix(pointpainting): modify pointcloud index

Signed-off-by: keita1523 <keita.miura@tier4.jp>
autowarefoundation · Feb 10, 2025 · 602941d · 602941d
1 parent c08e6b3
commit 602941d
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
@@ -235,7 +235,7 @@ void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_
   for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(tmp, "x"), iter_y(tmp, "y"),
        iter_z(tmp, "z");
        iter_x != iter_x.end();
-       ++iter_x, ++iter_y, ++iter_z, ++iter_painted_x, ++iter_painted_y, ++iter_painted_z) {
+       ++iter_x, ++iter_y, ++iter_z) {
     if (
       *iter_x <= pointcloud_range.at(0) || *iter_x >= pointcloud_range.at(3) ||
       *iter_y <= pointcloud_range.at(1) || *iter_y >= pointcloud_range.at(4)) {
@@ -245,6 +245,9 @@ void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_
       *iter_painted_y = *iter_y;
       *iter_painted_z = *iter_z;
       j += painted_point_step;
+      ++iter_painted_x;
+      ++iter_painted_y;
+      ++iter_painted_z;
     }
   }