perf(behavior_path_planner): improve getReferencePath to support very long routes (autowarefoundation#6739) (#1278)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp

@@ -67,7 +67,7 @@ class LaneChangeInterface : public SceneModuleInterface
 
   bool isExecutionReady() const override;
 
-  bool isRootLaneletToBeUpdated() const override
+  bool isCurrentRouteLaneletToBeReset() const override
   {
     return getCurrentStatus() == ModuleStatus::SUCCESS;
   }

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -404,7 +404,7 @@ void NormalLaneChange::insertStopPoint(
 
 PathWithLaneId NormalLaneChange::getReferencePath() const
 {
-  return utils::getCenterLinePathFromRootLanelet(
+  return utils::getCenterLinePathFromLanelet(
     status_.lane_change_path.info.target_lanes.front(), planner_data_);
 }
 

planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md

@@ -532,67 +532,33 @@ When the manager removes succeeded modules, the last added module's output is us
 
 ## Reference path generation
 
-The root reference path is generated from the centerline of the **lanelet sequence** that obtained from the **root lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running.
+The reference path is generated from the centerline of the **lanelet sequence** obtained from the **current route lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running.
 
 <figure markdown>
   ![root_generation](../image/manager/root_generation.svg){width=500}
   <figcaption>root reference path generation</figcaption>
 </figure>
 
-The root lanelet is the closest lanelet within the route, and the update timing is based on Ego's operation mode state.
+The **current route lanelet** keeps track of the route lanelet currently followed by the planner.
+It is initialized as the closest lanelet within the route.
+It is then updated as ego travels along the route such that (1) it follows the previous **current route lanelet** and (2) it is the closest lanelet within the route.
 
-- the state is `OperationModeState::AUTONOMOUS`: Update only when the ego moves to right or left lane by lane change module.
-- the state is **NOT** `OperationModeState::AUTONOMOUS`: Update at the beginning of every planning cycle.
+The **current route lanelet** can be reset to the closest lanelet within the route, ignoring whether it follows the previous **current route lanelet** .
 
-![root_lanelet](../image/manager/root_lanelet.svg)
+![current_route_lanelet](../image/manager/current_route_lanelet.svg)
 
 The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow.
 
-For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** update **root lanelet** even if the avoidance maneuver is finished.
+For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** reset the **current route lanelet**, even if the avoidance maneuver is finished.
 
 ![avoidance](../image/manager/avoidance.svg)
 
-On the other hand, if the lane change is successful, the manager updates **root lanelet** because the lane that Ego should follow changes.
+On the other hand, if the lane change is successful, the manager resets the **current route lanelet** because the lane that Ego should follow changes.
 
 ![lane_change](../image/manager/lane_change.svg)
 
-In addition, while manual driving, the manager always updates **root lanelet** because the pilot may move to an adjacent lane regardless of the decision of the autonomous driving system.
-
-```c++
-  /**
-   * @brief get reference path from root_lanelet_ centerline.
-   * @param planner data.
-   * @return reference path.
-   */
-  BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data) const
-  {
-    const auto & route_handler = data->route_handler;
-    const auto & pose = data->self_odometry->pose.pose;
-    const auto p = data->parameters;
-
-    constexpr double extra_margin = 10.0;
-    const auto backward_length =
-      std::max(p.backward_path_length, p.backward_path_length + extra_margin);
-
-    const auto lanelet_sequence = route_handler->getLaneletSequence(
-      root_lanelet_.value(), pose, backward_length, std::numeric_limits<double>::max());
-
-    lanelet::ConstLanelet closest_lane{};
-    if (lanelet::utils::query::getClosestLaneletWithConstrains(
-          lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold,
-          p.ego_nearest_yaw_threshold)) {
-      return utils::getReferencePath(closest_lane, data);
-    }
-
-    if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) {
-      return utils::getReferencePath(closest_lane, data);
-    }
-
-    return {};  // something wrong.
-  }
-```
-
-Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L202-L227)
+In addition, while manually driving (i.e., either the `OperationModeState` is different from `AUTONOMOUS` or the Autoware control is not engaged), the manager resets the **current route lanelet** at each iteration because the ego vehicle may move to an adjacent lane regardless of the decision of the autonomous driving system.
+The only exception is when a module is already approved, allowing testing the module's behavior while manually driving.
 
 ## Drivable area generation