refactor(lane_change): remove waitApproval dependencies

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · Jan 10, 2024 · f0fe5ce · f0fe5ce
1 parent 188ceaf
commit f0fe5ce
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 39 deletions.
diff --git a/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp
@@ -96,25 +96,6 @@ class LaneChangeInterface : public SceneModuleInterface
   TurnSignalInfo getCurrentTurnSignalInfo(
     const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info);
 
-  // TODO(someone): remove this, and use base class function
-  [[deprecated]] BehaviorModuleOutput run() override
-  {
-    updateData();
-
-    if (!isWaitingApproval()) {
-      return plan();
-    }
-
-    // module is waiting approval. Check it.
-    if (isActivated()) {
-      RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan().");
-      return plan();
-    } else {
-      RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate().");
-      return planWaitingApproval();
-    }
-  }
-
 protected:
   std::shared_ptr<LaneChangeParameters> parameters_;
 

diff --git a/...ior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/...ior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp
@@ -237,7 +237,7 @@ class LaneChangeBase
   LaneChangeStatus status_{};
   PathShifter path_shifter_{};
 
-  LaneChangeStates current_lane_change_state_{};
+  LaneChangeStates current_lane_change_state_{LaneChangeStates::Normal};
 
   std::shared_ptr<LaneChangeParameters> lane_change_parameters_{};
   std::shared_ptr<LaneChangePath> abort_path_{};

diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -49,9 +49,10 @@ LaneChangeInterface::LaneChangeInterface(
 
 void LaneChangeInterface::processOnEntry()
 {
-  waitApproval();
   module_type_->setPreviousModulePaths(
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
+  module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
+  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
   module_type_->updateLaneChangeStatus();
 }
 
@@ -80,6 +81,8 @@ 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);
   module_type_->updateSpecialData();
   module_type_->resetStopPose();
 }
@@ -98,8 +101,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;
@@ -112,7 +113,6 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   }
 
   updateSteeringFactorPtr(output);
-  clearWaitingApproval();
 
   return output;
 }
@@ -129,8 +129,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   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();
 
@@ -227,11 +225,6 @@ bool LaneChangeInterface::canTransitFailureState()
 
   log_debug_throttled(__func__);
 
-  if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) {
-    log_debug_throttled("Abort process has on going.");
-    return false;
-  }
-
   if (isWaitingApproval()) {
     log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL");
     return false;
@@ -298,14 +291,6 @@ bool LaneChangeInterface::canTransitIdleToWaitingApprovalState()
 
   log_debug_throttled(__func__);
 
-  if (!isActivated() || isWaitingApproval()) {
-    if (module_type_->specialRequiredCheck()) {
-      return true;
-    }
-    log_debug_throttled("Module is idling.");
-    return false;
-  }
-
   log_debug_throttled("Can lane change safely. Executing lane change.");
   module_type_->toNormalState();
   return true;

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -420,6 +420,7 @@ void NormalLaneChange::resetParameters()
   is_abort_approval_requested_ = false;
   current_lane_change_state_ = LaneChangeStates::Normal;
   abort_path_ = nullptr;
+  status_ = {};
 
   object_debug_.clear();
 }