handle lane_change status change in module side

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/decision_state.hpp

@@ -55,7 +55,7 @@ class PathDecisionStateController
     const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects,
     const std::shared_ptr<const PlannerData> planner_data,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const bool is_current_safe, const bool lane_change_status,
+    const bool is_current_safe, const bool lane_change_status_changed,
     const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher,
     std::vector<autoware_utils::Polygon2d> & ego_polygons_expanded);
 
@@ -66,8 +66,6 @@ class PathDecisionStateController
 
   PathDecisionState current_state_{};
 
-  std::optional<bool> prev_lane_change_status_{};
-
   /**
    * @brief update current state and save old current state to prev state
    */

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

@@ -45,7 +45,7 @@ class LaneParkingRequest
     const PlannerData & planner_data, const ModuleStatus & current_status,
     const BehaviorModuleOutput & upstream_module_output,
     const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data,
-    const bool trigger_thread_on_approach, const bool lane_change_status);
+    const bool trigger_thread_on_approach, const bool lane_change_status_changed);
 
   const autoware_utils::LinearRing2d vehicle_footprint_;
   const GoalCandidates goal_candidates_;
@@ -69,7 +69,6 @@ class LaneParkingRequest
   std::optional<PullOverPath> pull_over_path_;  //<! pull over path selected by main thread
   PathDecisionState prev_data_;
   bool trigger_thread_on_approach_{false};
-  std::optional<bool> prev_lane_change_status_{};
   bool lane_change_status_changed_{false};
 };
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp

@@ -31,14 +31,10 @@ void PathDecisionStateController::transit_state(
   const PredictedObjects & dynamic_target_objects,
   const std::shared_ptr<const PlannerData> planner_data,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const bool is_current_safe, const bool lane_change_status,
+  const bool is_current_safe, const bool lane_change_status_changed,
   const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher,
   std::vector<autoware_utils::Polygon2d> & ego_polygons_expanded)
 {
-  const bool lane_change_status_changed =
-    prev_lane_change_status_ && prev_lane_change_status_.value() != lane_change_status;
-  prev_lane_change_status_ = lane_change_status;
-
   const auto next_state = get_next_state(
     pull_over_path_opt, upstream_module_has_stopline, now, static_target_objects,
     dynamic_target_objects, planner_data, occupancy_grid_map, is_current_safe,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -702,9 +702,8 @@ void GoalPlannerModule::updateData()
       getPreviousModuleOutput().path, planner_data_->route_handler->getLaneletMapPtr(),
       planner_data_->route_handler->getRoutingGraphPtr())
       .has_value();
-  if (prev_lane_change_status_ && prev_lane_change_status_.value() != lane_change_status) {
-    lane_change_status_changed_ = true;
-  }
+  lane_change_status_changed_ =
+    prev_lane_change_status_ && prev_lane_change_status_.value() != lane_change_status;
   prev_lane_change_status_ = lane_change_status;
 
   const lanelet::ConstLanelets current_lanes =
@@ -767,7 +766,7 @@ void GoalPlannerModule::updateData()
   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_,
-    is_current_safe, lane_change_status, parameters_, goal_searcher,
+    is_current_safe, lane_change_status_changed_, parameters_, goal_searcher,
     debug_data_.ego_polygons_expanded);
   const auto new_decision_state = path_decision_controller_.get_current_state();
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp

@@ -23,7 +23,7 @@ void LaneParkingRequest::update(
   const PlannerData & planner_data, const ModuleStatus & current_status,
   const BehaviorModuleOutput & upstream_module_output,
   const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data,
-  const bool trigger_thread_on_approach, const bool lane_change_status)
+  const bool trigger_thread_on_approach, const bool lane_change_status_changed)
 {
   planner_data_ = std::make_shared<PlannerData>(planner_data);
   planner_data_->route_handler = std::make_shared<RouteHandler>(*(planner_data.route_handler));
@@ -32,10 +32,7 @@ void LaneParkingRequest::update(
   pull_over_path_ = pull_over_path;
   prev_data_ = prev_data;
   trigger_thread_on_approach_ = trigger_thread_on_approach;
-
-  lane_change_status_changed_ =
-    prev_lane_change_status_ && prev_lane_change_status_.value() != lane_change_status;
-  prev_lane_change_status_ = lane_change_status;
+  lane_change_status_changed_ = lane_change_status_changed;
 }
 
 void FreespaceParkingRequest::initializeOccupancyGridMap(