Fix filterPointCloudByTrajectory

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
autowarefoundation · Apr 19, 2022 · 9cc9fbd · 9cc9fbd
1 parent d125685
commit 9cc9fbd
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/...acle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/...acle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp
@@ -54,12 +54,13 @@ pcl::PointCloud<pcl::PointXYZ> filterPointCloudByTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius)
 {
   pcl::PointCloud<pcl::PointXYZ> filtered_pointcloud;
-  for (const auto & trajectory_point : trajectory.points) {
-    for (const auto & point : pointcloud.points) {
+  for (const auto & point : pointcloud.points) {
+    for (const auto & trajectory_point : trajectory.points) {
       const double dx = trajectory_point.pose.position.x - point.x;
       const double dy = trajectory_point.pose.position.y - point.y;
       if (std::hypot(dx, dy) < radius) {
         filtered_pointcloud.points.push_back(point);
+        break;
       }
     }
   }