fix(route_handler): route handler overlap removal is too conservative

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<PlannerData> & data);
+
   void generateCombinedDrivableArea(
     BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const;
 
@@ -307,12 +313,12 @@ class PlannerManager
    * @param planner data.
    * @return reference path.
    */
-  BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data);
+  BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & 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.

planning/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -113,8 +113,11 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & 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<PlannerData> & data)
+void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & 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_ptr<Plan
   const auto backward_length =
     std::max(p.backward_path_length, p.backward_path_length + extra_margin);
 
+  lanelet::ConstLanelet closest_lane{};
+
+  if (route_handler->getClosestRouteLaneletFromLanelet(
+        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<Plan
     current_route_lanelet_ = closest_lane;
   else
     resetCurrentRouteLanelet(data);
+}
 
+BehaviorModuleOutput PlannerManager::getReferencePath(
+  const std::shared_ptr<PlannerData> & 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;

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<PoseWithUuidStamped> & modified_goal,
+  const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
+  const std::optional<PoseWithUuidStamped> & modified_goal,
   const std::shared_ptr<RouteHandler> & route_handler);
 
 bool isEgoWithinOriginalLane(

planning/autoware_behavior_path_planner_common/src/utils/utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 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.
@@ -489,7 +489,8 @@
 }
 
 bool isEgoOutOfRoute(
-  const Pose & self_pose, const std::optional<PoseWithUuidStamped> & modified_goal,
+  const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
+  const std::optional<PoseWithUuidStamped> & modified_goal,
   const std::shared_ptr<RouteHandler> & route_handler)
 {
   const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
@@ -509,12 +510,16 @@
 
   // 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 @@
   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 @@
 
     return false;
   });
+
   if (!is_in_shoulder_lane && !is_in_road_lane) {
     return true;
   }
@@ -1308,26 +1306,26 @@
 {
   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<lanelet::ConstLanelet> 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<lanelet::ConstLanelet> 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 @@
 {
   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<lanelet::ConstLanelet> 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<lanelet::ConstLanelet> 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;
 }

planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -39,20 +39,6 @@
 namespace
 {
 using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
-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) {

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(