fix(behavior_path_planner): suppress reseting root lanelet (#9129)

fix(behavior_path_planner): suppress resseting root lanelet Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Oct 30, 2024 · 77b1181 · 77b1181
1 parent bc76816
commit 77b1181
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 6 deletions.
diff --git a/...autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/...autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp
@@ -551,8 +551,10 @@ class PlannerManager
   /**
    * @brief find and set the closest lanelet within the route to current route lanelet
    * @param planner data.
+   * @param is any approved module running.
    */
-  void updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data);
+  void updateCurrentRouteLanelet(
+    const std::shared_ptr<PlannerData> & data, const bool is_any_approved_module_running);
 
   void generateCombinedDrivableArea(
     BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const;

diff --git a/...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -351,9 +351,9 @@ void BehaviorPathPlannerNode::run()
     if (!is_first_time && !has_same_route_id) {
       RCLCPP_INFO(get_logger(), "New uuid route is received. Resetting modules.");
       planner_manager_->reset();
+      planner_manager_->resetCurrentRouteLanelet(planner_data_);
       planner_data_->prev_modified_goal.reset();
     }
-    planner_manager_->resetCurrentRouteLanelet(planner_data_);
   }
   const auto controlled_by_autoware_autonomously =
     planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS &&

diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp
@@ -135,7 +135,7 @@ 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);
+  updateCurrentRouteLanelet(data, is_any_approved_module_running);
 
   const bool is_out_of_route = utils::isEgoOutOfRoute(
     data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal,
@@ -228,7 +228,8 @@ void PlannerManager::generateCombinedDrivableArea(
   utils::extractObstaclesFromDrivableArea(output.path, di.obstacles);
 }
 
-void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data)
+void PlannerManager::updateCurrentRouteLanelet(
+  const std::shared_ptr<PlannerData> & data, const bool is_any_approved_module_running)
 {
   const auto & route_handler = data->route_handler;
   const auto & pose = data->self_odometry->pose.pose;
@@ -256,10 +257,11 @@ void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData
       p.ego_nearest_yaw_threshold) ||
     lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane);
 
-  if (could_calculate_closest_lanelet)
+  if (could_calculate_closest_lanelet) {
     *current_route_lanelet_ = closest_lane;
-  else
+  } else if (!is_any_approved_module_running) {
     resetCurrentRouteLanelet(data);
+  }
 }
 
 BehaviorModuleOutput PlannerManager::getReferencePath(