refactor(lane_change): refactor transit failure function (autowarefou…

…ndation#9835) * refactor(lane_change): refactor transit failure function Signed-off-by: Zulfaqar Azmi <[email protected]> * fixed failed scenario Signed-off-by: Zulfaqar Azmi <[email protected]> * remove is abort from debug Signed-off-by: Zulfaqar Azmi <[email protected]> * set is abort state Signed-off-by: Zulfaqar Azmi <[email protected]> * add comments for clarity Signed-off-by: Zulfaqar Azmi <[email protected]> * include what you use. Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]>
tier4 · Jan 10, 2025 · 4dc7201 · 4dc7201
1 parent 0b73c13
commit 4dc7201
Show file tree

Hide file tree

Showing 7 changed files with 111 additions and 125 deletions.
diff --git a/...r_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/...r_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp
@@ -35,6 +35,7 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
+#include <utility>
 
 namespace autoware::behavior_path_planner
 {
@@ -120,6 +121,8 @@ class LaneChangeInterface : public SceneModuleInterface
     }
   }
 
+  std::pair<LaneChangeStates, std::string_view> check_transit_failure();
+
   void updateDebugMarker() const;
 
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
@@ -134,6 +137,8 @@ class LaneChangeInterface : public SceneModuleInterface
   bool is_abort_path_approved_{false};
 
   bool is_abort_approval_requested_{false};
+
+  lane_change::InterfaceDebug interface_debug_;
 };
 }  // namespace autoware::behavior_path_planner
 

diff --git a/...ath_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/...ath_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp
@@ -48,6 +48,7 @@ enum class States {
   Cancel,
   Abort,
   Stop,
+  Warning,
 };
 
 struct PhaseInfo

diff --git a/...th_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp b/...th_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp
@@ -58,7 +58,6 @@ struct Debug
   double distance_to_abort_finished{std::numeric_limits<double>::max()};
   bool is_able_to_return_to_current_lane{true};
   bool is_stuck{false};
-  bool is_abort{false};
 
   void reset()
   {
@@ -83,9 +82,14 @@ struct Debug
     distance_to_abort_finished = std::numeric_limits<double>::max();
     is_able_to_return_to_current_lane = true;
     is_stuck = false;
-    is_abort = false;
   }
 };
+
+struct InterfaceDebug
+{
+  std::string_view failing_reason;
+  LaneChangeStates lc_state;
+};
 }  // namespace autoware::behavior_path_planner::lane_change
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_
diff --git a/...th_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/...th_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp
@@ -32,6 +32,7 @@ namespace marker_utils::lane_change_markers
 using autoware::behavior_path_planner::FilteredLanesObjects;
 using autoware::behavior_path_planner::LaneChangePath;
 using autoware::behavior_path_planner::lane_change::Debug;
+using autoware::behavior_path_planner::lane_change::InterfaceDebug;
 using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
 MarkerArray showAllValidLaneChangePath(
@@ -42,9 +43,12 @@ MarkerArray createLaneChangingVirtualWallMarker(
 MarkerArray showFilteredObjects(
   const FilteredLanesObjects & filtered_objects, const std::string & ns);
 MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
-MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
+MarkerArray showExecutionInfo(
+  const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data,
+  const geometry_msgs::msg::Pose & ego_pose);
 MarkerArray createDebugMarkerArray(
-  const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
+  const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data,
+  const geometry_msgs::msg::Pose & ego_pose);
 
 }  // namespace marker_utils::lane_change_markers
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_
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
@@ -139,9 +139,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
       const auto force_activated = std::any_of(
         rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
         [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
-      const bool safe = force_activated ? false : true;
       updateRTCStatus(
-        path.start_distance_to_path_change, path.finish_distance_to_path_change, safe,
+        path.start_distance_to_path_change, path.finish_distance_to_path_change, !force_activated,
         State::RUNNING);
     }
   }
@@ -220,9 +219,7 @@ bool LaneChangeInterface::canTransitSuccessState()
 
   if (module_type_->specialExpiredCheck() && isWaitingApproval()) {
     log_debug_throttled("Run specialExpiredCheck.");
-    if (isWaitingApproval()) {
-      return true;
-    }
+    return true;
   }
 
   if (module_type_->hasFinishedLaneChange()) {
@@ -237,13 +234,6 @@ bool LaneChangeInterface::canTransitSuccessState()
 
 bool LaneChangeInterface::canTransitFailureState()
 {
-  auto log_debug_throttled = [&](std::string_view message) -> void {
-    RCLCPP_DEBUG(getLogger(), "%s", message.data());
-  };
-
-  updateDebugMarker();
-  log_debug_throttled(__func__);
-
   const auto force_activated = std::any_of(
     rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
     [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
@@ -253,121 +243,106 @@ bool LaneChangeInterface::canTransitFailureState()
     return false;
   }
 
-  if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) {
-    log_debug_throttled("Abort process has on going.");
-    return false;
-  }
+  const auto [state, reason] = check_transit_failure();
 
-  if (isWaitingApproval()) {
-    if (module_type_->is_near_regulatory_element()) {
-      log_debug_throttled("Ego is close to regulatory element. Cancel lane change");
-      updateRTCStatus(
-        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
-        State::FAILED);
-      return true;
-    }
-    log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL");
-    return false;
-  }
+  interface_debug_.failing_reason = reason;
+  interface_debug_.lc_state = state;
 
-  if (!module_type_->isValidPath()) {
-    log_debug_throttled("Transit to failure state due not to find valid path");
+  updateDebugMarker();
+
+  if (state == LaneChangeStates::Cancel) {
     updateRTCStatus(
       std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
       State::FAILED);
+    module_type_->toCancelState();
     return true;
   }
 
-  if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) {
-    log_debug_throttled("Abort process has completed.");
-    updateRTCStatus(
-      std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
-      State::FAILED);
-    return true;
+  if (state == LaneChangeStates::Abort) {
+    module_type_->toAbortState();
+    return false;
   }
 
-  if (module_type_->is_near_terminal()) {
-    log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change");
+  // Note: Ideally, if it is unsafe, but for some reason, we can't abort or cancel, then we should
+  // stop. Note: This feature is not working properly for now.
+  const auto [is_safe, unsafe_trailing_obj] = post_process_safety_status_;
+  if (!is_safe && module_type_->isRequiredStop(unsafe_trailing_obj)) {
+    module_type_->toStopState();
+    return false;
+  }
+
+  module_type_->toNormalState();
+  return false;
+}
 
-    if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) {
-      log_debug_throttled("Module require stopping");
+std::pair<LaneChangeStates, std::string_view> LaneChangeInterface::check_transit_failure()
+{
+  if (module_type_->isAbortState()) {
+    if (module_type_->hasFinishedAbort()) {
+      return {LaneChangeStates::Cancel, "Aborted"};
     }
-    return false;
+    return {LaneChangeStates::Abort, "Aborting"};
   }
 
-  if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) {
-    if (module_type_->isStoppedAtRedTrafficLight()) {
-      log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change");
-      module_type_->toCancelState();
-      updateRTCStatus(
-        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
-        State::FAILED);
-      return true;
+  if (isWaitingApproval()) {
+    if (module_type_->is_near_regulatory_element()) {
+      return {LaneChangeStates::Cancel, "CloseToRegElement"};
     }
+    return {LaneChangeStates::Normal, "WaitingForApproval"};
+  }
 
+  if (!module_type_->isValidPath()) {
+    return {LaneChangeStates::Cancel, "InvalidPath"};
+  }
+
+  const auto is_preparing = module_type_->isEgoOnPreparePhase();
+  const auto can_return_to_current = module_type_->isAbleToReturnCurrentLane();
+
+  // regardless of safe and unsafe, we want to cancel lane change.
+  if (is_preparing) {
     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;
-    }
-
-    if (module_type_->isAbleToReturnCurrentLane()) {
-      log_debug_throttled("It's possible to return to current lane. Cancel lane change.");
-      updateRTCStatus(
-        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
-        State::FAILED);
-      return true;
+    if (force_deactivated && can_return_to_current) {
+      return {LaneChangeStates::Cancel, "ForceDeactivation"};
     }
   }
 
   if (post_process_safety_status_.is_safe) {
-    log_debug_throttled("Can't transit to failure state. Ego is lane changing, and it's safe.");
-    return false;
+    return {LaneChangeStates::Normal, "SafeToLaneChange"};
   }
 
-  if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) {
-    log_debug_throttled("Module require stopping");
+  if (!module_type_->isCancelEnabled()) {
+    return {LaneChangeStates::Warning, "CancelDisabled"};
   }
 
-  if (!module_type_->isCancelEnabled()) {
-    log_debug_throttled(
-      "Lane change path is unsafe but cancel was not enabled. Continue lane change.");
-    return false;
+  // We also check if the ego can return to the current lane, as prepare segment might be out of the
+  // lane, for example, during an evasive maneuver around a static object.
+  if (is_preparing && can_return_to_current) {
+    return {LaneChangeStates::Cancel, "SafeToCancel"};
+  }
+
+  if (module_type_->is_near_terminal()) {
+    return {LaneChangeStates::Warning, "TooNearTerminal"};
   }
 
   if (!module_type_->isAbortEnabled()) {
-    log_debug_throttled(
-      "Lane change path is unsafe but abort was not enabled. Continue lane change.");
-    return false;
+    return {LaneChangeStates::Warning, "AbortDisabled"};
   }
 
-  if (!module_type_->isAbleToReturnCurrentLane()) {
-    log_debug_throttled("It's is not possible to return to original lane. Continue lane change.");
-    return false;
+  // To prevent the lane module from having to check rear objects in the current lane, we limit the
+  // abort maneuver to cases where the ego vehicle is still in the current lane.
+  if (!can_return_to_current) {
+    return {LaneChangeStates::Warning, "TooLateToAbort"};
   }
 
   const auto found_abort_path = module_type_->calcAbortPath();
   if (!found_abort_path) {
-    log_debug_throttled(
-      "Lane change path is unsafe but abort path not found. Continue lane change.");
-    return false;
+    return {LaneChangeStates::Warning, "AbortPathNotFound"};
   }
 
-  log_debug_throttled("Lane change path is unsafe. Abort lane change.");
-  module_type_->toAbortState();
-  return false;
+  return {LaneChangeStates::Abort, "SafeToAbort"};
 }
 
 void LaneChangeInterface::updateDebugMarker() const
@@ -377,7 +352,8 @@ void LaneChangeInterface::updateDebugMarker() const
     return;
   }
   using marker_utils::lane_change_markers::createDebugMarkerArray;
-  debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose());
+  debug_marker_ = createDebugMarkerArray(
+    interface_debug_, module_type_->getDebugData(), module_type_->getEgoPose());
 }
 
 MarkerArray LaneChangeInterface::getModuleVirtualWall()

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -892,7 +892,6 @@ bool NormalLaneChange::isAbleToStopSafely() const
 bool NormalLaneChange::hasFinishedAbort() const
 {
   if (!abort_path_) {
-    lane_change_debug_.is_abort = true;
     return true;
   }
 
@@ -901,7 +900,6 @@ bool NormalLaneChange::hasFinishedAbort() const
   lane_change_debug_.distance_to_abort_finished = distance_to_finish;
 
   const auto has_finished_abort = distance_to_finish < 0.0;
-  lane_change_debug_.is_abort = has_finished_abort;
 
   return has_finished_abort;
 }
@@ -916,12 +914,7 @@ bool NormalLaneChange::isAbortState() const
     return false;
   }
 
-  if (!abort_path_) {
-    return false;
-  }
-
-  lane_change_debug_.is_abort = true;
-  return true;
+  return abort_path_ != nullptr;
 }
 
 int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const
@@ -1491,14 +1484,8 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const
 bool NormalLaneChange::isRequiredStop(const bool is_trailing_object)
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
-  if (
-    common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() &&
-    is_trailing_object) {
-    current_lane_change_state_ = LaneChangeStates::Stop;
-    return true;
-  }
-  current_lane_change_state_ = LaneChangeStates::Normal;
-  return false;
+  return common_data_ptr_->transient_data.is_ego_near_current_terminal_start &&
+         isAbleToStopSafely() && is_trailing_object;
 }
 
 bool NormalLaneChange::calcAbortPath()
-Original file line number
+Diff line change
@@ Expand Up / @@ -48,6 +48,7 @@ enum class States { @@
       Cancel,
       Abort,
       Stop,
+      Warning,
     };
     struct PhaseInfo
@@ Expand Down @@