fix warm start condition

Signed-off-by: Mamoru Sobue <[email protected]>

Author: soblin

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -304,7 +304,7 @@ class GoalPlannerModule : public SceneModuleInterface
   bool trigger_thread_on_approach_{false};
 
   // signal path generator and state manager to regenerate path candidates and remain NOT_DECIDED
-  std::optional<bool> prev_lane_change_status_{};
+  std::optional<bool> prev_lane_change_detected_{};
   bool lane_change_status_changed_{false};
 
   // pre-generate lane parking paths in a separate thread

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp

@@ -76,6 +76,7 @@ PathDecisionState PathDecisionStateController::get_next_state(
 
   // if lane change is triggered, reference path for pull over must be reset accordingly
   if (lane_change_status_changed) {
+    RCLCPP_INFO(logger_, "[DecidingPathStatus]: NOT_DECIDED. lane change detected");
     next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED;
     return next_state;
   }

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -618,12 +618,6 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
         vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(),
         use_bus_stop_area_ && goal_searcher.bus_stop_area_available());
     }
-    const auto lane_change_status =
-      goal_planner_utils::find_lane_change_completed_lanelet(
-        getPreviousModuleOutput().path, planner_data_->route_handler->getLaneletMapPtr(),
-        planner_data_->route_handler->getRoutingGraphPtr())
-        .has_value();
-
     // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that
     // planner_data_ is not nullptr, so it is OK to copy as value
     // By copying PlannerData as value, the internal shared member variables are also copied
@@ -634,7 +628,7 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
     lane_parking_request_.value().update(
       *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path,
       path_decision_controller_.get_current_state(), trigger_thread_on_approach_,
-      lane_change_status);
+      lane_change_status_changed_);
     // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as
     // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since
     // behavior_path_planner::run() updates
@@ -697,40 +691,40 @@ void GoalPlannerModule::updateData()
     goal_candidates_ = generateGoalCandidates(goal_searcher_.value(), use_bus_stop_area_);
   }
 
-  const auto lane_change_status =
+  const auto lane_change_detected =
     goal_planner_utils::find_lane_change_completed_lanelet(
       getPreviousModuleOutput().path, planner_data_->route_handler->getLaneletMapPtr(),
       planner_data_->route_handler->getRoutingGraphPtr())
       .has_value();
   lane_change_status_changed_ =
-    prev_lane_change_status_ && prev_lane_change_status_.value() != lane_change_status;
-  prev_lane_change_status_ = lane_change_status;
+    prev_lane_change_detected_ && prev_lane_change_detected_.value() != lane_change_detected;
+  prev_lane_change_detected_ = lane_change_detected;
 
   const lanelet::ConstLanelets current_lanes =
     utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
 
-  if (getCurrentStatus() == ModuleStatus::IDLE) {
-    if (
-      !trigger_thread_on_approach_ &&
-      utils::isAllowedGoalModification(planner_data_->route_handler) &&
-      goal_planner_utils::is_goal_reachable_on_path(
-        current_lanes, *(planner_data_->route_handler), left_side_parking_)) {
-      const double self_to_goal_arc_length = utils::getSignedDistance(
-        planner_data_->self_odometry->pose.pose,
-        planner_data_->route_handler->getOriginalGoalPose(), current_lanes);
-      if (self_to_goal_arc_length < parameters_.pull_over_prepare_length) {
-        trigger_thread_on_approach_ = true;
-        [[maybe_unused]] const auto send_only_request = syncWithThreads();
-        RCLCPP_INFO(
-          getLogger(), "start preparing goal candidates once for goal ahead of %f meter",
-          self_to_goal_arc_length);
-        return;
-      }
+  if (
+    !trigger_thread_on_approach_ &&
+    utils::isAllowedGoalModification(planner_data_->route_handler) &&
+    goal_planner_utils::is_goal_reachable_on_path(
+      current_lanes, *(planner_data_->route_handler), left_side_parking_)) {
+    const double self_to_goal_arc_length = utils::getSignedDistance(
+      planner_data_->self_odometry->pose.pose, planner_data_->route_handler->getOriginalGoalPose(),
+      current_lanes);
+    if (self_to_goal_arc_length < parameters_.pull_over_prepare_length) {
+      trigger_thread_on_approach_ = true;
+      [[maybe_unused]] const auto send_only_request = syncWithThreads();
+      RCLCPP_INFO(
+        getLogger(), "start preparing goal candidates once for goal ahead of %f meter",
+        self_to_goal_arc_length);
+      return;
     }
+  }
+
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
     if (lane_change_status_changed_) {
       [[maybe_unused]] const auto send_only_request = syncWithThreads();
       RCLCPP_INFO(getLogger(), "restart preparing goal candidates since lane_change is detected");
-      return;
     }
     return;
   }
@@ -762,7 +756,6 @@ void GoalPlannerModule::updateData()
 
   const bool upstream_module_has_stopline_except_terminal =
     goal_planner_utils::has_stopline_except_terminal(getPreviousModuleOutput().path);
-
   path_decision_controller_.transit_state(
     pull_over_path_recv, upstream_module_has_stopline_except_terminal, clock_->now(),
     static_target_objects, dynamic_target_objects, planner_data_, occupancy_grid_map_,