Merge pull request #1815 from shmpwk/fix/pointpainting_pointcloud_filter

fix(autoware_image_projection_based_fusion): modify incorrect index access in pointcloud filtering for out-of-range points (autowarefoundation#10087)
tier4 · Feb 17, 2025 · 7fe5954 · 7fe5954
2 parents c316a1c + 892cbb2
commit 7fe5954
Showing 1 changed file with 4 additions and 2 deletions.
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
@@ -234,8 +234,7 @@ void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_
   sensor_msgs::PointCloud2Iterator<float> iter_painted_z(painted_pointcloud_msg, "z");
   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_x.end(); ++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 +244,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;
     }
   }