fix(obstacle_collision_checker): fix bug with filterPointCloudByTraje…

…ctory (tier4#729)

* Add unit test showcasing the issue

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix filterPointCloudByTrajectory

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add NOLINT for direct include of .cpp file

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

control/obstacle_collision_checker/CMakeLists.txt

@@ -44,6 +44,10 @@ rclcpp_components_register_node(obstacle_collision_checker
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+  ament_add_ros_isolated_gtest(test_obstacle_collision_checker
+    test/test_obstacle_collision_checker.cpp
+  )
+  target_link_libraries(test_obstacle_collision_checker obstacle_collision_checker)
 endif()
 
 ament_auto_package(

control/obstacle_collision_checker/package.xml

@@ -26,6 +26,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>vehicle_info_util</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp

@@ -59,12 +59,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;
       }
     }
   }

control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp

@@ -0,0 +1,46 @@
+// Copyright 2022 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "../src/obstacle_collision_checker_node/obstacle_collision_checker.cpp"  // NOLINT
+#include "gtest/gtest.h"
+
+TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory)
+{
+  pcl::PointCloud<pcl::PointXYZ> pcl;
+  autoware_auto_planning_msgs::msg::Trajectory trajectory;
+  pcl::PointXYZ pcl_point;
+  autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point;
+  pcl_point.y = 0.0;
+  traj_point.pose.position.y = 0.99;
+  for (double x = 0.0; x < 10.0; x += 1.0) {
+    pcl_point.x = x;
+    traj_point.pose.position.x = x;
+    trajectory.points.push_back(traj_point);
+    pcl.push_back(pcl_point);
+  }
+  // radius < 1: all points are filtered
+  for (auto radius = 0.0; radius <= 0.99; radius += 0.1) {
+    const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius);
+    EXPECT_EQ(filtered_pcl.size(), 0ul);
+  }
+  // radius >= 1.0: all points are kept
+  for (auto radius = 1.0; radius < 10.0; radius += 0.1) {
+    const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius);
+    ASSERT_EQ(pcl.size(), filtered_pcl.size());
+    for (size_t i = 0; i < pcl.size(); ++i) {
+      EXPECT_EQ(pcl[i].x, filtered_pcl[i].x);
+      EXPECT_EQ(pcl[i].y, filtered_pcl[i].y);
+    }
+  }
+}