add initial unit test (for cut_predicted_path_beyond_line)

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

Author: maxime-clem

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt

@@ -9,4 +9,15 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   DIRECTORY src
 )
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_filter_predicted_objects.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+endif()
+
 ament_auto_package(INSTALL_TO_SHARE config)

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml

@@ -12,8 +12,8 @@
 
   <license>Apache License 2.0</license>
 
-  <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
+
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_motion_velocity_planner_common</depend>
@@ -31,6 +31,7 @@
   <depend>vehicle_info_util</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024-2024 TIER IV, Inc.
+// Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -30,6 +30,8 @@ void cut_predicted_path_beyond_line(
   autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
 {
+  if (predicted_path.path.empty() || stop_line.size() < 2) return;
+
   auto stop_line_idx = 0UL;
   bool found = false;
   lanelet::BasicSegment2d path_segment;

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp

@@ -0,0 +1,58 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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/filter_predicted_objects.hpp"
+
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
+
+#include <gtest/gtest.h>
+#include <lanelet2_core/geometry/LineString.h>
+
+TEST(TestCollisionDistance, CutPredictedPathBeyondLine)
+{
+  using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line;
+  autoware_auto_perception_msgs::msg::PredictedPath predicted_path;
+  lanelet::BasicLineString2d stop_line;
+  double object_front_overhang = 0.0;
+  const auto eps = 1e-9;
+
+  EXPECT_NO_THROW(cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang));
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.0;
+  pose.position.y = 0.0;
+  predicted_path.path.push_back(pose);
+  pose.position.y = 1.0;
+  predicted_path.path.push_back(pose);
+  pose.position.y = 2.0;
+  predicted_path.path.push_back(pose);
+  pose.position.y = 3.0;
+  predicted_path.path.push_back(pose);
+
+  cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang);
+  EXPECT_EQ(predicted_path.path.size(), 4UL);
+  for (auto i = 0UL; i < predicted_path.path.size(); ++i) {
+    EXPECT_EQ(predicted_path.path[i].position.x, 0.0);
+    EXPECT_NEAR(predicted_path.path[i].position.y, static_cast<double>(i), eps);
+  }
+  stop_line = {{-1.0, 2.0}, {1.0, 2.0}};
+  cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang);
+  EXPECT_EQ(predicted_path.path.size(), 2UL);
+  for (auto i = 0UL; i < predicted_path.path.size(); ++i) {
+    EXPECT_EQ(predicted_path.path[i].position.x, 0.0);
+    EXPECT_NEAR(predicted_path.path[i].position.y, static_cast<double>(i), eps);
+  }
+}

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt

@@ -9,17 +9,6 @@ autoware_package()
 #   DIRECTORY src
 # )
 
-if(BUILD_TESTING)
-  # ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
-  #   test/src/test_node_interface.cpp TODO(Maxime)
-  # )
-  # target_link_libraries(test_${PROJECT_NAME}
-  #   gtest_main
-  #   ${PROJECT_NAME}_lib
-  # )
-  # target_include_directories(test_${PROJECT_NAME} PRIVATE src)
-endif()
-
 ament_auto_package(INSTALL_TO_SHARE
   include
 )