refactor(lane_change): update lc status in updateData (autowarefounda…

…tion#6088)

* refactor(lane_change): update lc status in updateData

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* update current lane and target lane during requested

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* set is_valid_path as false by default

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* reset flags after finished lane change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -89,7 +89,7 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isAbortState() const override;
 
-  bool isLaneChangeRequired() const override;
+  bool isLaneChangeRequired() override;
 
   bool isStoppedAtRedTrafficLight() const override;
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

@@ -80,7 +80,7 @@ class LaneChangeBase
 
   virtual bool hasFinishedAbort() const = 0;
 
-  virtual bool isLaneChangeRequired() const = 0;
+  virtual bool isLaneChangeRequired() = 0;
 
   virtual bool isAbortState() const = 0;
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp

@@ -45,7 +45,7 @@ struct LaneChangeStatus
   std::vector<lanelet::Id> lane_follow_lane_ids{};
   std::vector<lanelet::Id> lane_change_lane_ids{};
   bool is_safe{false};
-  bool is_valid_path{true};
+  bool is_valid_path{false};
   double start_distance{0.0};
 };
 

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -50,9 +50,6 @@ LaneChangeInterface::LaneChangeInterface(
 void LaneChangeInterface::processOnEntry()
 {
   waitApproval();
-  module_type_->setPreviousModulePaths(
-    getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
-  module_type_->updateLaneChangeStatus();
 }
 
 void LaneChangeInterface::processOnExit()
@@ -80,6 +77,14 @@ void LaneChangeInterface::updateData()
 {
   module_type_->setPreviousModulePaths(
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
+  module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
+  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
+
+  if (isWaitingApproval()) {
+    module_type_->updateLaneChangeStatus();
+  }
+  setObjectDebugVisualization();
+
   module_type_->updateSpecialData();
   module_type_->resetStopPose();
 }
@@ -98,8 +103,6 @@ BehaviorModuleOutput LaneChangeInterface::plan()
     return {};
   }
 
-  module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
-  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
   auto output = module_type_->generateOutput();
   path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
   *prev_approved_path_ = getPreviousModuleOutput().path;
@@ -128,22 +131,14 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   out.reference_path = getPreviousModuleOutput().reference_path;
   out.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
   out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
-
-  module_type_->setPreviousModulePaths(
-    getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
-  module_type_->updateLaneChangeStatus();
-  setObjectDebugVisualization();
+  out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info);
 
   for (const auto & [uuid, data] : module_type_->getDebugData()) {
     const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
     setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
   }
 
-  // change turn signal when the vehicle reaches at the end of the path for waiting lane change
-  out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info);
-
   path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
-
   stop_pose_ = module_type_->getStopPose();
 
   if (!module_type_->isValidPath()) {
@@ -211,6 +206,7 @@ bool LaneChangeInterface::canTransitSuccessState()
   }
 
   if (module_type_->hasFinishedLaneChange()) {
+    module_type_->resetParameters();
     log_debug_throttled("Lane change process has completed.");
     return true;
   }

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -115,17 +115,17 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
   return {true, found_safe_path};
 }
 
-bool NormalLaneChange::isLaneChangeRequired() const
+bool NormalLaneChange::isLaneChangeRequired()
 {
-  const auto current_lanes = getCurrentLanes();
+  status_.current_lanes = getCurrentLanes();
 
-  if (current_lanes.empty()) {
+  if (status_.current_lanes.empty()) {
     return false;
   }
 
-  const auto target_lanes = getLaneChangeLanes(current_lanes, direction_);
+  status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_);
 
-  return !target_lanes.empty();
+  return !status_.target_lanes.empty();
 }
 
 bool NormalLaneChange::isStoppedAtRedTrafficLight() const
@@ -420,8 +420,15 @@ void NormalLaneChange::resetParameters()
   is_abort_approval_requested_ = false;
   current_lane_change_state_ = LaneChangeStates::Normal;
   abort_path_ = nullptr;
+  status_ = {};
 
   object_debug_.clear();
+  object_debug_after_approval_.clear();
+  debug_filtered_objects_.current_lane.clear();
+  debug_filtered_objects_.target_lane.clear();
+  debug_filtered_objects_.other_lane.clear();
+  debug_valid_path_.clear();
+  RCLCPP_DEBUG(logger_, "reset all flags and debug information.");
 }
 
 TurnSignalInfo NormalLaneChange::updateOutputTurnSignal()