From 27ecddd5755e246e549dc4e8a6805cb05d2b488c Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 3 Jun 2024 11:41:36 +0900 Subject: [PATCH] add route handler unit tests for overlapping route case Signed-off-by: mohammad alqudah --- .../src/utils/utils.cpp | 2 +- .../test_map/overlap_map.osm | 974 ++++++++++++++++++ .../route_handler/test/test_route_handler.cpp | 61 ++ .../route_handler/test/test_route_handler.hpp | 24 +- .../test_route/overlap_test_route.yaml | 117 +++ 5 files changed, 1168 insertions(+), 10 deletions(-) create mode 100644 planning/planning_test_utils/test_map/overlap_map.osm create mode 100644 planning/route_handler/test_route/overlap_test_route.yaml diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 9c883457a1d62..4640bb59ed6a0 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1316,7 +1316,7 @@ lanelet::ConstLanelets extendNextLane( for (const auto & next_lane : next_lanes) { // skip overlapping lanes if (next_lane.id() == lanes.front().id()) continue; - + // if route lane, set target and break if (route_handler->isRouteLanelet(next_lane)) { target_next_lane = next_lane; diff --git a/planning/planning_test_utils/test_map/overlap_map.osm b/planning/planning_test_utils/test_map/overlap_map.osm new file mode 100644 index 0000000000000..d4fec4f72aeaa --- /dev/null +++ b/planning/planning_test_utils/test_map/overlap_map.osm @@ -0,0 +1,974 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index 212fc0ce84d89..bfac80ff95c58 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -14,6 +14,8 @@ #include "test_route_handler.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include @@ -45,6 +47,65 @@ TEST_F(TestRouteHandler, getGoalLaneId) ASSERT_EQ(goal_lane.id(), 5088); } +TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) +{ + set_route_handler("/test_map/overlap_map.osm"); + ASSERT_FALSE(route_handler_->isHandlerReady()); + + geometry_msgs::msg::Pose start_pose, goal_pose; + start_pose.position = tier4_autoware_utils::createPoint(3728.870361, 73739.281250, 0); + start_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, -0.513117, 0.858319); + goal_pose.position = tier4_autoware_utils::createPoint(3729.961182, 73727.328125, 0); + goal_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, 0.234831, 0.972036); + + lanelet::ConstLanelets path_lanelets; + ASSERT_TRUE( + route_handler_->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)); + ASSERT_EQ(path_lanelets.size(), 12); + ASSERT_EQ(path_lanelets.front().id(), 168); + ASSERT_EQ(path_lanelets.back().id(), 345); + + route_handler_->setRouteLanelets(path_lanelets); + ASSERT_TRUE(route_handler_->isHandlerReady()); + + rclcpp::init(0, nullptr); + ASSERT_TRUE(rclcpp::ok()); + + auto lanelet_sequence = route_handler_->getLaneletSequence(path_lanelets.back()); + ASSERT_EQ(lanelet_sequence.size(), 12); + ASSERT_EQ(lanelet_sequence.front().id(), 168); + ASSERT_EQ(lanelet_sequence.back().id(), 345); +} + +TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) +{ + set_route_handler("/test_map/overlap_map.osm"); + set_test_route("/test_route/overlap_test_route.yaml"); + ASSERT_TRUE(route_handler_->isHandlerReady()); + + geometry_msgs::msg::Pose reference_pose, search_pose; + + lanelet::ConstLanelet reference_lanelet; + reference_pose.position = tier4_autoware_utils::createPoint(3730.88, 73735.3, 0); + reference_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, -0.504626, 0.863338); + const auto found_reference_lanelet = + route_handler_->getClosestLaneletWithinRoute(reference_pose, &reference_lanelet); + ASSERT_TRUE(found_reference_lanelet); + ASSERT_EQ(reference_lanelet.id(), 168); + + lanelet::ConstLanelet closest_lanelet; + search_pose.position = tier4_autoware_utils::createPoint(3736.89, 73730.8, 0); + search_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, 0.223244, 0.974763); + bool found_lanelet = route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lanelet); + ASSERT_TRUE(found_lanelet); + ASSERT_EQ(closest_lanelet.id(), 345); + + found_lanelet = route_handler_->getClosestRouteLaneletFromLanelet( + search_pose, reference_lanelet, &closest_lanelet, 3.0, 1.046); + ASSERT_TRUE(found_lanelet); + ASSERT_EQ(closest_lanelet.id(), 277); +} + // TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) // { // lanelet::ConstLanelet closest_lane; diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 3c6893da3f9d1..de21bcca57c46 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -44,13 +44,9 @@ class TestRouteHandler : public ::testing::Test public: TestRouteHandler() { - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto lanelet2_path = planning_test_utils_dir + "/test_map/2km_test.osm"; - constexpr double center_line_resolution = 5.0; - const auto map_bin_msg = test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); - route_handler_ = std::make_shared(map_bin_msg); - set_lane_change_test_route(); + planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); + set_route_handler("/test_map/2km_test.osm"); + set_test_route("/test_route/lane_change_test_route.yaml"); } TestRouteHandler(const TestRouteHandler &) = delete; @@ -59,14 +55,24 @@ class TestRouteHandler : public ::testing::Test TestRouteHandler & operator=(TestRouteHandler &&) = delete; ~TestRouteHandler() override = default; - void set_lane_change_test_route() + void set_route_handler(const std::string & relative_path) + { + route_handler_.reset(); + const auto lanelet2_path = planning_test_utils_dir + relative_path; + const auto map_bin_msg = test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); + route_handler_ = std::make_shared(map_bin_msg); + } + + void set_test_route(const std::string & route_path) { const auto route_handler_dir = ament_index_cpp::get_package_share_directory("route_handler"); - const auto rh_test_route = route_handler_dir + "/test_route/lane_change_test_route.yaml"; + const auto rh_test_route = route_handler_dir + route_path; route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route)); } std::shared_ptr route_handler_; + std::string planning_test_utils_dir; + static constexpr double center_line_resolution = 5.0; }; } // namespace route_handler::test diff --git a/planning/route_handler/test_route/overlap_test_route.yaml b/planning/route_handler/test_route/overlap_test_route.yaml new file mode 100644 index 0000000000000..595d22f4204eb --- /dev/null +++ b/planning/route_handler/test_route/overlap_test_route.yaml @@ -0,0 +1,117 @@ +header: + stamp: + sec: 1715936953 + nanosec: 67206425 + frame_id: map +start_pose: + position: + x: 3728.8 + y: 73739.2 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.513117 + w: 0.858319 +goal_pose: + position: + x: 3729.961182 + y: 73727.328125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.234831 + w: 0.972036 +segments: + - preferred_primitive: + id: 168 + primitive_type: "" + primitives: + - id: 168 + primitive_type: lane + - preferred_primitive: + id: 277 + primitive_type: "" + primitives: + - id: 277 + primitive_type: lane + - preferred_primitive: + id: 282 + primitive_type: "" + primitives: + - id: 282 + primitive_type: lane + - preferred_primitive: + id: 287 + primitive_type: "" + primitives: + - id: 287 + primitive_type: lane + - preferred_primitive: + id: 292 + primitive_type: "" + primitives: + - id: 292 + primitive_type: lane + - preferred_primitive: + id: 297 + primitive_type: "" + primitives: + - id: 297 + primitive_type: lane + - preferred_primitive: + id: 302 + primitive_type: "" + primitives: + - id: 302 + primitive_type: lane + - preferred_primitive: + id: 307 + primitive_type: "" + primitives: + - id: 307 + primitive_type: lane + - preferred_primitive: + id: 312 + primitive_type: "" + primitives: + - id: 312 + primitive_type: lane + - preferred_primitive: + id: 317 + primitive_type: "" + primitives: + - id: 317 + primitive_type: lane + - preferred_primitive: + id: 322 + primitive_type: "" + primitives: + - id: 322 + primitive_type: lane + - preferred_primitive: + id: 345 + primitive_type: "" + primitives: + - id: 345 + primitive_type: lane +uuid: + uuid: + - 231 + - 254 + - 143 + - 227 + - 194 + - 8 + - 220 + - 88 + - 30 + - 194 + - 172 + - 147 + - 127 + - 143 + - 176 + - 133 +allow_modification: false