feat(lane_change): force deactivation in prepare phase (autowarefound…

…ation#8235) transfer to cancel state when force deactivated Signed-off-by: Go Sakayori <[email protected]>
a-maumau · Jul 29, 2024 · 5093495 · 5093495
1 parent e87a038
commit 5093495
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
@@ -275,6 +275,19 @@ bool LaneChangeInterface::canTransitFailureState()
       return true;
     }
 
+    const auto force_deactivated = std::any_of(
+      rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
+      [&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); });
+
+    if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) {
+      log_debug_throttled("Cancel lane change due to force deactivation");
+      module_type_->toCancelState();
+      updateRTCStatus(
+        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+        State::FAILED);
+      return true;
+    }
+
     if (post_process_safety_status_.is_safe) {
       log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe.");
       return false;