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