fix abort issue

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · Jan 11, 2024 · 26a48db · 26a48db
1 parent 195aa6f
commit 26a48db
Showing 1 changed file with 5 additions and 10 deletions.
diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -84,6 +84,7 @@ void LaneChangeInterface::updateData()
   module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
   module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
   module_type_->updateSpecialData();
+  setObjectDebugVisualization();
   module_type_->resetStopPose();
 }
 
@@ -224,6 +225,10 @@ 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");
@@ -288,16 +293,6 @@ bool LaneChangeInterface::canTransitFailureState()
 
 bool LaneChangeInterface::canTransitIdleToWaitingApprovalState()
 {
-  setObjectDebugVisualization();
-
-  auto log_debug_throttled = [&](std::string_view message) -> void {
-    RCLCPP_DEBUG(getLogger(), "%s", message.data());
-  };
-
-  log_debug_throttled(__func__);
-
-  log_debug_throttled("Can lane change safely. Executing lane change.");
-  module_type_->toNormalState();
   return true;
 }