diff --git a/common/autoware_test_utils/test_map/overlap_map.osm b/common/autoware_test_utils/test_map/overlap_map.osm
new file mode 100644
index 0000000000000..d4fec4f72aeaa
--- /dev/null
+++ b/common/autoware_test_utils/test_map/overlap_map.osm
@@ -0,0 +1,974 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp b/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp
index 3e2afc2da6a95..fe32700f3332b 100644
--- a/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp
+++ b/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp
@@ -299,6 +299,12 @@ class PlannerManager
return result;
}
+ /**
+ * @brief find and set the closest lanelet within the route to current route lanelet
+ * @param planner data.
+ */
+ void updateCurrentRouteLanelet(const std::shared_ptr & data);
+
void generateCombinedDrivableArea(
BehaviorModuleOutput & output, const std::shared_ptr & data) const;
@@ -307,12 +313,12 @@ class PlannerManager
* @param planner data.
* @return reference path.
*/
- BehaviorModuleOutput getReferencePath(const std::shared_ptr & data);
+ BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const;
/**
* @brief publish the root reference path and current route lanelet
*/
- void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path);
+ void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) const;
/**
* @brief stop and unregister the module from manager.
diff --git a/planning/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/autoware_behavior_path_planner/src/planner_manager.cpp
index 3f440615bdc2c..104ad7f9ce9d0 100644
--- a/planning/autoware_behavior_path_planner/src/planner_manager.cpp
+++ b/planning/autoware_behavior_path_planner/src/planner_manager.cpp
@@ -113,8 +113,11 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da
const bool is_any_module_running =
is_any_approved_module_running || is_any_candidate_module_running_or_idle;
+ updateCurrentRouteLanelet(data);
+
const bool is_out_of_route = utils::isEgoOutOfRoute(
- data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler);
+ data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal,
+ data->route_handler);
if (!is_any_module_running && is_out_of_route) {
BehaviorModuleOutput output = utils::createGoalAroundPath(data);
@@ -416,7 +419,7 @@ BehaviorModuleOutput PlannerManager::runKeepLastModules(
return output;
}
-BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr & data)
+void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr & data)
{
const auto & route_handler = data->route_handler;
const auto & pose = data->self_odometry->pose.pose;
@@ -426,10 +429,18 @@ BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptrgetClosestRouteLaneletFromLanelet(
+ pose, current_route_lanelet_.value(), &closest_lane, p.ego_nearest_dist_threshold,
+ p.ego_nearest_yaw_threshold)) {
+ current_route_lanelet_ = closest_lane;
+ return;
+ }
+
const auto lanelet_sequence = route_handler->getLaneletSequence(
current_route_lanelet_.value(), pose, backward_length, p.forward_path_length);
- lanelet::ConstLanelet closest_lane{};
const auto could_calculate_closest_lanelet =
lanelet::utils::query::getClosestLaneletWithConstrains(
lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold,
@@ -440,13 +451,18 @@ BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr & data) const
+{
const auto reference_path = utils::getReferencePath(*current_route_lanelet_, data);
publishDebugRootReferencePath(reference_path);
return reference_path;
}
-void PlannerManager::publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path)
+void PlannerManager::publishDebugRootReferencePath(
+ const BehaviorModuleOutput & reference_path) const
{
using visualization_msgs::msg::Marker;
MarkerArray array;
diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp
index e4b8020404b50..ca6416d015489 100644
--- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp
+++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp
@@ -231,7 +231,8 @@ bool isInLaneletWithYawThreshold(
const double radius = 0.0);
bool isEgoOutOfRoute(
- const Pose & self_pose, const std::optional & modified_goal,
+ const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
+ const std::optional & modified_goal,
const std::shared_ptr & route_handler);
bool isEgoWithinOriginalLane(
diff --git a/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp
index 4610f7584ea29..7f58c8ef6e56c 100644
--- a/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp
+++ b/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp
@@ -489,7 +489,8 @@ bool isInLaneletWithYawThreshold(
}
bool isEgoOutOfRoute(
- const Pose & self_pose, const std::optional & modified_goal,
+ const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
+ const std::optional & modified_goal,
const std::shared_ptr & route_handler)
{
const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
@@ -509,12 +510,16 @@ bool isEgoOutOfRoute(
// If ego vehicle is over goal on goal lane, return true
const double yaw_threshold = tier4_autoware_utils::deg2rad(90);
- if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) {
+ if (
+ closest_road_lane.id() == goal_lane.id() &&
+ isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) {
constexpr double buffer = 1.0;
const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose);
const auto goal_arc_coord =
lanelet::utils::getArcCoordinates({goal_lane}, route_handler->getGoalPose());
if (ego_arc_coord.length > goal_arc_coord.length + buffer) {
+ RCLCPP_WARN_STREAM(
+ rclcpp::get_logger("behavior_path_planner").get_child("util"), "ego pose is beyond goal");
return true;
} else {
return false;
@@ -526,14 +531,6 @@ bool isEgoOutOfRoute(
const bool is_in_shoulder_lane = !route_handler->getShoulderLaneletsAtPose(self_pose).empty();
// Check if ego vehicle is in road lane
const bool is_in_road_lane = std::invoke([&]() {
- lanelet::ConstLanelet closest_road_lane;
- if (!route_handler->getClosestLaneletWithinRoute(self_pose, &closest_road_lane)) {
- RCLCPP_WARN_STREAM(
- rclcpp::get_logger("behavior_path_planner").get_child("util"),
- "cannot find closest road lanelet");
- return false;
- }
-
if (lanelet::utils::isInLanelet(self_pose, closest_road_lane)) {
return true;
}
@@ -548,6 +545,7 @@ bool isEgoOutOfRoute(
return false;
});
+
if (!is_in_shoulder_lane && !is_in_road_lane) {
return true;
}
@@ -1308,26 +1306,26 @@ lanelet::ConstLanelets extendNextLane(
{
if (lanes.empty()) return lanes;
- auto extended_lanes = lanes;
+ const auto next_lanes = route_handler->getNextLanelets(lanes.back());
+ if (next_lanes.empty()) return lanes;
// Add next lane
- const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back());
- if (!next_lanes.empty()) {
- std::optional target_next_lane;
- if (!only_in_route) {
- target_next_lane = next_lanes.front();
- }
- // use the next lane in route if it exists
- for (const auto & next_lane : next_lanes) {
- if (route_handler->isRouteLanelet(next_lane)) {
- target_next_lane = next_lane;
- }
- }
- if (target_next_lane) {
- extended_lanes.push_back(*target_next_lane);
+ auto extended_lanes = lanes;
+ std::optional target_next_lane;
+ 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;
+ break;
}
+ if (!only_in_route && !target_next_lane) target_next_lane = next_lane;
+ }
+ if (target_next_lane) {
+ extended_lanes.push_back(*target_next_lane);
}
-
return extended_lanes;
}
@@ -1337,24 +1335,25 @@ lanelet::ConstLanelets extendPrevLane(
{
if (lanes.empty()) return lanes;
- auto extended_lanes = lanes;
+ const auto prev_lanes = route_handler->getPreviousLanelets(lanes.front());
+ if (prev_lanes.empty()) return lanes;
// Add previous lane
- const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front());
- if (!prev_lanes.empty()) {
- std::optional target_prev_lane;
- if (!only_in_route) {
- target_prev_lane = prev_lanes.front();
- }
- // use the previous lane in route if it exists
- for (const auto & prev_lane : prev_lanes) {
- if (route_handler->isRouteLanelet(prev_lane)) {
- target_prev_lane = prev_lane;
- }
- }
- if (target_prev_lane) {
- extended_lanes.insert(extended_lanes.begin(), *target_prev_lane);
+ auto extended_lanes = lanes;
+ std::optional target_prev_lane;
+ for (const auto & prev_lane : prev_lanes) {
+ // skip overlapping lanes
+ if (prev_lane.id() == lanes.back().id()) continue;
+
+ // if route lane, set target and break
+ if (route_handler->isRouteLanelet(prev_lane)) {
+ target_prev_lane = prev_lane;
+ break;
}
+ if (!only_in_route && !target_prev_lane) target_prev_lane = prev_lane;
+ }
+ if (target_prev_lane) {
+ extended_lanes.insert(extended_lanes.begin(), *target_prev_lane);
}
return extended_lanes;
}
diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp
index a1ef66f5757f5..e1c41eee2fb20 100644
--- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp
+++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp
@@ -39,20 +39,6 @@
namespace
{
using RouteSections = std::vector;
-RouteSections combine_consecutive_route_sections(
- const RouteSections & route_sections1, const RouteSections & route_sections2)
-{
- RouteSections route_sections;
- route_sections.reserve(route_sections1.size() + route_sections2.size());
- if (!route_sections1.empty()) {
- // remove end route section because it is overlapping with first one in next route_section
- route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1);
- }
- if (!route_sections2.empty()) {
- route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end());
- }
- return route_sections;
-}
bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point)
{
@@ -422,15 +408,14 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
RCLCPP_WARN(logger, "Failed to plan route.");
return route_msg;
}
+
for (const auto & lane : path_lanelets) {
+ if (!all_route_lanelets.empty() && lane.id() == all_route_lanelets.back().id()) continue;
all_route_lanelets.push_back(lane);
}
- // create local route sections
- route_handler_.setRouteLanelets(path_lanelets);
- const auto local_route_sections = route_handler_.createMapSegments(path_lanelets);
- route_sections = combine_consecutive_route_sections(route_sections, local_route_sections);
}
route_handler_.setRouteLanelets(all_route_lanelets);
+ route_sections = route_handler_.createMapSegments(all_route_lanelets);
auto goal_pose = points.back();
if (param_.enable_correct_goal_pose) {
diff --git a/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp
index 5d7c78e0c1976..976db246275d6 100644
--- a/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp
+++ b/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp
@@ -116,6 +116,8 @@ class RouteHandler
// for lanelet
bool getPreviousLaneletsWithinRoute(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const;
+ bool getNextLaneletsWithinRoute(
+ const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * next_lanelets) const;
bool getNextLaneletWithinRoute(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const;
@@ -260,6 +262,22 @@ class RouteHandler
bool getClosestLaneletWithConstrainsWithinRoute(
const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
const double yaw_threshold) const;
+
+ /**
+ * Finds the closest route lanelet to the search pose satisfying the distance and yaw constraints
+ * with respect to a reference lanelet, the search set will include previous route lanelets,
+ * next route lanelets, and neighbors of reference lanelet. Returns false if failed to find
+ * lanelet.
+ * @param search_pose pose to find closest lanelet to
+ * @param reference_lanelet reference lanelet to decide the search set
+ * @param dist_threshold distance constraint closest lanelet must be within
+ * @param yaw_threshold yaw constraint closest lanelet direction must be within
+ */
+ bool getClosestRouteLaneletFromLanelet(
+ const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet,
+ lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
+ const double yaw_threshold) const;
+
lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const;
lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const;
lanelet::ConstLanelets getLaneletSequence(
diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp
index 213cca3bd5d80..dfe79a7a758af 100644
--- a/planning/autoware_route_handler/src/route_handler.cpp
+++ b/planning/autoware_route_handler/src/route_handler.cpp
@@ -616,28 +616,41 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo(
lanelet::ConstLanelet current_lanelet = lanelet;
double length = 0;
lanelet::ConstLanelets previous_lanelets;
+
+ auto checkForLoop =
+ [&lanelet](const lanelet::ConstLanelets & lanelets_to_check, const bool is_route_lanelets) {
+ if (is_route_lanelets) {
+ return std::none_of(
+ lanelets_to_check.begin(), lanelets_to_check.end(),
+ [lanelet](auto & prev_llt) { return lanelet.id() != prev_llt.id(); });
+ }
+ return std::any_of(
+ lanelets_to_check.begin(), lanelets_to_check.end(),
+ [lanelet](auto & prev_llt) { return lanelet.id() == prev_llt.id(); });
+ };
+
+ auto isNewLanelet = [&lanelet,
+ &lanelet_sequence_backward](const lanelet::ConstLanelet & lanelet_to_check) {
+ if (lanelet.id() == lanelet_to_check.id()) return false;
+ return std::none_of(
+ lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(),
+ [&lanelet_to_check](auto & backward) { return (backward.id() == lanelet_to_check.id()); });
+ };
+
while (rclcpp::ok() && length < min_length) {
previous_lanelets.clear();
+ bool is_route_lanelets = true;
if (!getPreviousLaneletsWithinRoute(current_lanelet, &previous_lanelets)) {
if (only_route_lanes) break;
- const auto previous_lanelets = getPreviousLanelets(current_lanelet);
+ previous_lanelets = getPreviousLanelets(current_lanelet);
if (previous_lanelets.empty()) break;
+ is_route_lanelets = false;
}
- // loop check
- if (std::any_of(previous_lanelets.begin(), previous_lanelets.end(), [lanelet](auto & prev_llt) {
- return lanelet.id() == prev_llt.id();
- })) {
- break;
- }
+
+ if (checkForLoop(previous_lanelets, is_route_lanelets)) break;
for (const auto & prev_lanelet : previous_lanelets) {
- if (std::any_of(
- lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(),
- [prev_lanelet, lanelet](auto & backward) {
- return (backward.id() == prev_lanelet.id());
- })) {
- continue;
- }
+ if (!isNewLanelet(prev_lanelet) || exists(goal_lanelets_, prev_lanelet)) continue;
lanelet_sequence_backward.push_back(prev_lanelet);
length +=
static_cast(boost::geometry::length(prev_lanelet.centerline().basicLineString()));
@@ -922,8 +935,32 @@ bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute(
route_lanelets_, search_pose, closest_lanelet, dist_threshold, yaw_threshold);
}
-bool RouteHandler::getNextLaneletWithinRoute(
- const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const
+bool RouteHandler::getClosestRouteLaneletFromLanelet(
+ const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet,
+ lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
+ const double yaw_threshold) const
+{
+ lanelet::ConstLanelets previous_lanelets, next_lanelets, lanelet_sequence;
+ if (getPreviousLaneletsWithinRoute(reference_lanelet, &previous_lanelets)) {
+ lanelet_sequence = previous_lanelets;
+ }
+
+ lanelet_sequence.push_back(reference_lanelet);
+
+ if (getNextLaneletsWithinRoute(reference_lanelet, &next_lanelets)) {
+ lanelet_sequence.insert(lanelet_sequence.end(), next_lanelets.begin(), next_lanelets.end());
+ }
+
+ if (lanelet::utils::query::getClosestLaneletWithConstrains(
+ lanelet_sequence, search_pose, closest_lanelet, dist_threshold, yaw_threshold)) {
+ return true;
+ }
+
+ return false;
+}
+
+bool RouteHandler::getNextLaneletsWithinRoute(
+ const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * next_lanelets) const
{
if (exists(goal_lanelets_, lanelet)) {
return false;
@@ -932,12 +969,23 @@ bool RouteHandler::getNextLaneletWithinRoute(
const auto start_lane_id = route_ptr_->segments.front().preferred_primitive.id;
const auto following_lanelets = routing_graph_ptr_->following(lanelet);
+ next_lanelets->clear();
for (const auto & llt : following_lanelets) {
if (start_lane_id != llt.id() && exists(route_lanelets_, llt)) {
- *next_lanelet = llt;
- return true;
+ next_lanelets->push_back(llt);
}
}
+ return !(next_lanelets->empty());
+}
+
+bool RouteHandler::getNextLaneletWithinRoute(
+ const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const
+{
+ lanelet::ConstLanelets next_lanelets{};
+ if (getNextLaneletsWithinRoute(lanelet, &next_lanelets)) {
+ *next_lanelet = next_lanelets.front();
+ return true;
+ }
return false;
}
@@ -2142,6 +2190,9 @@ lanelet::ConstLanelets RouteHandler::getMainLanelets(
const lanelet::ConstLanelets & path_lanelets) const
{
auto lanelet_sequence = getLaneletSequence(path_lanelets.back());
+
+ RCLCPP_INFO_STREAM(logger_, "getMainLanelets: lanelet_sequence = " << lanelet_sequence);
+
lanelet::ConstLanelets main_lanelets;
while (!lanelet_sequence.empty()) {
main_lanelets.insert(main_lanelets.begin(), lanelet_sequence.begin(), lanelet_sequence.end());
diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp
index 5ee8aa8034abd..eb39b37a81fc3 100644
--- a/planning/autoware_route_handler/test/test_route_handler.cpp
+++ b/planning/autoware_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/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp
index 292cbaa6ba7d9..06e05b75c3932 100644
--- a/planning/autoware_route_handler/test/test_route_handler.hpp
+++ b/planning/autoware_route_handler/test/test_route_handler.hpp
@@ -44,14 +44,9 @@ class TestRouteHandler : public ::testing::Test
public:
TestRouteHandler()
{
- const auto autoware_test_utils_dir =
- ament_index_cpp::get_package_share_directory("autoware_test_utils");
- const auto lanelet2_path = autoware_test_utils_dir + "/test_map/2km_test.osm";
- constexpr double center_line_resolution = 5.0;
- const auto map_bin_msg =
- autoware::test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution);
- route_handler_ = std::make_shared(map_bin_msg);
- set_lane_change_test_route();
+ autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils");
+ set_route_handler("/test_map/2km_test.osm");
+ set_test_route("/test_route/lane_change_test_route.yaml");
}
TestRouteHandler(const TestRouteHandler &) = delete;
@@ -60,15 +55,26 @@ 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 = autoware_test_utils_dir + relative_path;
+ const auto map_bin_msg =
+ autoware::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("autoware_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(autoware::test_utils::parse_lanelet_route_file(rh_test_route));
}
std::shared_ptr route_handler_;
+ std::string autoware_test_utils_dir;
+ static constexpr double center_line_resolution = 5.0;
};
} // namespace autoware::route_handler::test
diff --git a/planning/autoware_route_handler/test_route/overlap_test_route.yaml b/planning/autoware_route_handler/test_route/overlap_test_route.yaml
new file mode 100644
index 0000000000000..595d22f4204eb
--- /dev/null
+++ b/planning/autoware_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