test(path_generator): add tests

* add tests Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * adapt test to new test manager Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * migrate to autoware_internal_planning_msgs Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * use intersection map for unit tests Signed-off-by: mitukou1109 <mitukou1109@gmail.com> --------- Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix pre-commit Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> fix pre-commit Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Feb 28, 2025 · 99f1633 · 99f1633
1 parent e551981
commit 99f1633
Show file tree

Hide file tree

Showing 8 changed files with 1,208 additions and 2 deletions.
diff --git a/planning/autoware_path_generator/CMakeLists.txt b/planning/autoware_path_generator/CMakeLists.txt
@@ -16,7 +16,7 @@ target_link_libraries(${PROJECT_NAME}
   path_generator_parameters
 )
 
-target_compile_options(${PROJECT_NAME} PRIVATE
+target_compile_options(${PROJECT_NAME} PUBLIC
   -Wno-error=deprecated-declarations
 )
 
@@ -25,4 +25,18 @@ rclcpp_components_register_node(${PROJECT_NAME}
   EXECUTABLE path_generator_node
 )
 
-ament_auto_package()
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_path_generator_node_interface.cpp
+    test/test_lanelet.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    test_route
+)
diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml
@@ -21,12 +21,18 @@
   <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
+  <depend>autoware_planning_test_manager</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_trajectory</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>generate_parameter_library</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

diff --git a/planning/autoware_path_generator/test/test_lanelet.cpp b/planning/autoware_path_generator/test/test_lanelet.cpp
@@ -0,0 +1,234 @@
+// Copyright 2025 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 "utils_test.hpp"
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+
+#include <gtest/gtest.h>
+
+namespace autoware::path_generator
+{
+TEST_F(UtilsTest, getPreviousLaneletWithinRoute)
+{
+  {  // Normal case
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50);
+
+    const auto prev_lanelet = utils::get_previous_lanelet_within_route(lanelet, planner_data_);
+
+    ASSERT_TRUE(prev_lanelet.has_value());
+    ASSERT_EQ(prev_lanelet->id(), 125);
+  }
+
+  {  // The given lanelet is at the start of the route
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10323);
+
+    const auto prev_lanelet = utils::get_previous_lanelet_within_route(lanelet, planner_data_);
+
+    ASSERT_FALSE(prev_lanelet.has_value());
+  }
+}
+
+TEST_F(UtilsTest, getNextLaneletWithinRoute)
+{
+  {  // Normal case
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50);
+
+    const auto next_lanelet = utils::get_next_lanelet_within_route(lanelet, planner_data_);
+
+    ASSERT_TRUE(next_lanelet.has_value());
+    ASSERT_EQ(next_lanelet->id(), 122);
+  }
+
+  {  // The given lanelet is at the end of the route
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(122);
+
+    const auto next_lanelet = utils::get_next_lanelet_within_route(lanelet, planner_data_);
+
+    ASSERT_FALSE(next_lanelet.has_value());
+  }
+}
+
+TEST_F(UtilsTest, getLaneletsWithinRouteUpTo)
+{
+  if (!rclcpp::ok()) {
+    rclcpp::init(0, nullptr);
+  }
+
+  {  // Normal case
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50);
+    const auto distance = 30.0;
+
+    const auto prev_lanelets =
+      utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance);
+
+    ASSERT_TRUE(prev_lanelets.has_value());
+    ASSERT_EQ(prev_lanelets->size(), 1);
+    ASSERT_EQ(prev_lanelets->at(0).id(), 125);
+  }
+
+  {  // The given distance exceeds the route section
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50);
+    const auto distance = 100.0;
+
+    const auto prev_lanelets =
+      utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance);
+
+    ASSERT_TRUE(prev_lanelets.has_value());
+    ASSERT_EQ(prev_lanelets->size(), 2);
+    ASSERT_EQ(prev_lanelets->at(0).id(), 10323);
+    ASSERT_EQ(prev_lanelets->at(1).id(), 125);
+  }
+
+  {  // The given distance is negative
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50);
+    const auto distance = -30.0;
+
+    const auto prev_lanelets =
+      utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance);
+
+    ASSERT_TRUE(prev_lanelets.has_value());
+    ASSERT_TRUE(prev_lanelets->empty());
+  }
+
+  {  // The given lanelet is not within the route
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10257);
+    const auto distance = 30.0;
+
+    const auto prev_lanelets =
+      utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance);
+
+    ASSERT_FALSE(prev_lanelets.has_value());
+  }
+}
+
+TEST_F(UtilsTest, getLaneletsWithinRouteAfter)
+{
+  if (!rclcpp::ok()) {
+    rclcpp::init(0, nullptr);
+  }
+
+  {  // Normal case
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(125);
+    const auto distance = 30.0;
+
+    const auto next_lanelets =
+      utils::get_lanelets_within_route_after(lanelet, planner_data_, distance);
+
+    ASSERT_TRUE(next_lanelets.has_value());
+    ASSERT_EQ(next_lanelets->size(), 2);
+    ASSERT_EQ(next_lanelets->at(0).id(), 50);
+    ASSERT_EQ(next_lanelets->at(1).id(), 122);
+  }
+
+  {  // The given distance exceeds the route section
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(125);
+    const auto distance = 100.0;
+
+    const auto next_lanelets =
+      utils::get_lanelets_within_route_after(lanelet, planner_data_, distance);
+
+    ASSERT_TRUE(next_lanelets.has_value());
+    ASSERT_EQ(next_lanelets->size(), 2);
+    ASSERT_EQ(next_lanelets->at(0).id(), 50);
+    ASSERT_EQ(next_lanelets->at(1).id(), 122);
+  }
+
+  {  // The given distance is negative
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(125);
+    const auto distance = -30.0;
+
+    const auto next_lanelets =
+      utils::get_lanelets_within_route_after(lanelet, planner_data_, distance);
+
+    ASSERT_TRUE(next_lanelets.has_value());
+    ASSERT_TRUE(next_lanelets->empty());
+  }
+
+  {  // The given lanelet is not within the route
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10257);
+    const auto distance = 30.0;
+
+    const auto next_lanelets =
+      utils::get_lanelets_within_route_after(lanelet, planner_data_, distance);
+
+    ASSERT_FALSE(next_lanelets.has_value());
+  }
+}
+
+TEST_F(UtilsTest, getLaneletsWithinRoute)
+{
+  if (!rclcpp::ok()) {
+    rclcpp::init(0, nullptr);
+  }
+
+  {  // Normal case
+    const auto pose = autoware::test_utils::createPose(3765.606, 73746.328, 0.0, 0.0, 0.0, 0.0);
+    const auto lanelet = get_lanelet_closest_to_pose(pose);
+    const auto backward_distance = 40.0;
+    const auto forward_distance = 40.0;
+
+    const auto lanelets = utils::get_lanelets_within_route(
+      lanelet, planner_data_, pose, backward_distance, forward_distance);
+
+    ASSERT_TRUE(lanelets.has_value());
+    ASSERT_EQ(lanelets->size(), 3);
+    ASSERT_EQ(lanelets->at(0).id(), 125);
+    ASSERT_EQ(lanelets->at(1).id(), 50);
+    ASSERT_EQ(lanelets->at(2).id(), 122);
+  }
+
+  {  // The given backward distance is too small to search for the previous lanelets
+    const auto pose = autoware::test_utils::createPose(3765.606, 73746.328, 0.0, 0.0, 0.0, 0.0);
+    const auto lanelet = get_lanelet_closest_to_pose(pose);
+    const auto backward_distance = 5.0;
+    const auto forward_distance = 40.0;
+
+    const auto lanelets = utils::get_lanelets_within_route(
+      lanelet, planner_data_, pose, backward_distance, forward_distance);
+
+    ASSERT_TRUE(lanelets.has_value());
+    ASSERT_EQ(lanelets->size(), 2);
+    ASSERT_EQ(lanelets->at(0).id(), 50);
+    ASSERT_EQ(lanelets->at(1).id(), 122);
+  }
+
+  {  // The given forward distance is too small to search for the next lanelets
+    const auto pose = autoware::test_utils::createPose(3765.606, 73746.328, 0.0, 0.0, 0.0, 0.0);
+    const auto lanelet = get_lanelet_closest_to_pose(pose);
+    const auto backward_distance = 40.0;
+    const auto forward_distance = 5.0;
+
+    const auto lanelets = utils::get_lanelets_within_route(
+      lanelet, planner_data_, pose, backward_distance, forward_distance);
+
+    ASSERT_TRUE(lanelets.has_value());
+    ASSERT_EQ(lanelets->size(), 2);
+    ASSERT_EQ(lanelets->at(0).id(), 125);
+    ASSERT_EQ(lanelets->at(1).id(), 50);
+  }
+
+  {  // The given lanelet is not within the route
+    const auto pose = autoware::test_utils::createPose(3746.81, 73775.84, 0.0, 0.0, 0.0, 0.0);
+    const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10257);
+    const auto backward_distance = 40.0;
+    const auto forward_distance = 40.0;
+
+    const auto lanelets = utils::get_lanelets_within_route(
+      lanelet, planner_data_, pose, backward_distance, forward_distance);
+
+    ASSERT_FALSE(lanelets.has_value());
+  }
+}
+}  // namespace autoware::path_generator
diff --git a/planning/autoware_path_generator/test/test_path_generator_node_interface.cpp b/planning/autoware_path_generator/test/test_path_generator_node_interface.cpp
@@ -0,0 +1,72 @@
+// Copyright 2025 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/node.hpp"
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
+#include <autoware_test_utils/autoware_test_utils.hpp>
+
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
+{
+  rclcpp::init(0, nullptr);
+
+  auto test_manager =
+    std::make_shared<autoware::planning_test_manager::PlanningInterfaceTestManager>();
+
+  const auto autoware_test_utils_dir =
+    ament_index_cpp::get_package_share_directory("autoware_test_utils");
+  const auto path_generator_dir =
+    ament_index_cpp::get_package_share_directory("autoware_path_generator");
+
+  const auto node_options = rclcpp::NodeOptions{}.arguments(
+    {"--ros-args", "--params-file",
+     autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file",
+     autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file",
+     path_generator_dir + "/config/path_generator.param.yaml"});
+
+  auto test_target_node = std::make_shared<autoware::path_generator::PathGenerator>(node_options);
+
+  // publish necessary topics from test_manager
+  test_manager->publishInput(
+    test_target_node, "path_generator/input/vector_map", autoware::test_utils::makeMapBinMsg());
+  test_manager->publishInput(
+    test_target_node, "path_generator/input/odometry", autoware::test_utils::makeOdometry());
+
+  // create subscriber in test_manager
+  test_manager->subscribeOutput<autoware_internal_planning_msgs::msg::PathWithLaneId>(
+    "path_generator/output/path");
+
+  const std::string route_topic_name = "path_generator/input/route";
+
+  // test with normal trajectory
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithBehaviorNormalRoute(test_target_node, route_topic_name));
+
+  EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
+
+  // test with trajectory with empty/one point/overlapping point
+  ASSERT_NO_THROW_WITH_ERROR_MSG(
+    test_manager->testWithAbnormalRoute(test_target_node, route_topic_name));
+
+  rclcpp::shutdown();
+}