refactor(lane_change): use new interface for state (autowarefoundatio…

…n#5950) * refactor(lane_change): use new interface for state Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix some state Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix scenario failed Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix ablc not transitioning to success Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Interplai · May 28, 2024 · 7bc3ee4 · 7bc3ee4
1 parent e00eaa3
commit 7bc3ee4
Show file tree

Hide file tree

Showing 7 changed files with 166 additions and 119 deletions.
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -54,6 +54,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
   const auto & data = avoidance_data_;
 
   if (data.target_objects.empty()) {
+    RCLCPP_WARN(logger_, "no empty objects");
     return false;
   }
 
@@ -73,11 +74,13 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
     std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object);
 
   if (num_of_avoidance_targets < 1) {
+    RCLCPP_WARN(logger_, "no avoidance target");
     return false;
   }
 
   const auto current_lanes = getCurrentLanes();
   if (current_lanes.empty()) {
+    RCLCPP_WARN(logger_, "no empty lanes");
     return false;
   }
 
@@ -106,7 +109,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
 
   const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length);
 
-  RCLCPP_DEBUG(
+  RCLCPP_WARN(
     logger_,
     "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance "
     "%.3f",

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
@@ -69,11 +69,10 @@ class LaneChangeInterface : public SceneModuleInterface
 
   bool isRootLaneletToBeUpdated() const override { return current_state_ == ModuleStatus::SUCCESS; }
 
-  // TODO(someone): remove this, and use base class function
-  [[deprecated]] ModuleStatus updateState() override;
-
   void updateData() override;
 
+  void postProcess() override;
+
   BehaviorModuleOutput plan() override;
 
   BehaviorModuleOutput planWaitingApproval() override;
@@ -118,15 +117,13 @@ class LaneChangeInterface : public SceneModuleInterface
 
   std::unique_ptr<LaneChangeBase> module_type_;
 
-  bool canTransitSuccessState() override { return false; }
-
-  bool canTransitFailureState() override { return false; }
+  PathSafetyStatus post_process_safety_status_;
 
-  bool canTransitIdleToRunningState() override { return false; }
+  bool canTransitSuccessState() override;
 
-  void resetPathIfAbort();
+  bool canTransitFailureState() override;
 
-  void resetLaneChangeModule();
+  bool canTransitIdleToRunningState() override;
 
   void setObjectDebugVisualization() const;
 

diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
@@ -71,7 +71,7 @@ class NormalLaneChange : public LaneChangeBase
 
   PathSafetyStatus isApprovedPathSafe() const override;
 
-  bool isRequiredStop(const bool is_object_coming_from_rear) const override;
+  bool isRequiredStop(const bool is_object_coming_from_rear) override;
 
   bool isNearEndOfCurrentLanes(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,

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
@@ -90,7 +90,7 @@ class LaneChangeBase
 
   virtual bool isEgoOnPreparePhase() const = 0;
 
-  virtual bool isRequiredStop(const bool is_object_coming_from_rear) const = 0;
+  virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0;
 
   virtual PathSafetyStatus isApprovedPathSafe() const = 0;
 

diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -76,110 +76,6 @@ bool LaneChangeInterface::isExecutionReady() const
   return module_type_->isSafe();
 }
 
-ModuleStatus LaneChangeInterface::updateState()
-{
-  auto log_warn_throttled = [&](const std::string & message) -> void {
-    RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 5000, message);
-  };
-
-  if (module_type_->specialExpiredCheck()) {
-    log_warn_throttled("expired check.");
-    if (isWaitingApproval()) {
-      return ModuleStatus::SUCCESS;
-    }
-  }
-
-  if (!isActivated() || isWaitingApproval()) {
-    log_warn_throttled("Is idling.");
-    return ModuleStatus::IDLE;
-  }
-
-  if (!module_type_->isValidPath()) {
-    log_warn_throttled("Is invalid path.");
-    return ModuleStatus::SUCCESS;
-  }
-
-  if (module_type_->isAbortState()) {
-    log_warn_throttled("Ego is in the process of aborting lane change.");
-    return module_type_->hasFinishedAbort() ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING;
-  }
-
-  if (module_type_->hasFinishedLaneChange()) {
-    log_warn_throttled("Completed lane change.");
-    return ModuleStatus::SUCCESS;
-  }
-
-  if (module_type_->isEgoOnPreparePhase() && module_type_->isStoppedAtRedTrafficLight()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      getLogger(), *clock_, 5000, "Ego stopped at traffic light. Canceling lane change");
-    module_type_->toCancelState();
-    return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS;
-  }
-
-  const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe();
-
-  setObjectDebugVisualization();
-  if (is_safe) {
-    log_warn_throttled("Lane change path is safe.");
-    module_type_->toNormalState();
-    return ModuleStatus::RUNNING;
-  }
-
-  const auto change_state_if_stop_required = [&]() -> void {
-    if (module_type_->isRequiredStop(is_object_coming_from_rear)) {
-      module_type_->toStopState();
-    } else {
-      module_type_->toNormalState();
-    }
-  };
-
-  if (!module_type_->isCancelEnabled()) {
-    log_warn_throttled(
-      "Lane change path is unsafe but cancel was not enabled. Continue lane change.");
-    change_state_if_stop_required();
-    return ModuleStatus::RUNNING;
-  }
-
-  if (!module_type_->isAbleToReturnCurrentLane()) {
-    log_warn_throttled("Lane change path is unsafe but cannot return. Continue lane change.");
-    change_state_if_stop_required();
-    return ModuleStatus::RUNNING;
-  }
-
-  const auto threshold = module_type_->getLaneChangeParam().backward_length_buffer_for_end_of_lane;
-  const auto status = module_type_->getLaneChangeStatus();
-  if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) {
-    log_warn_throttled("Lane change path is unsafe but near end of lane. Continue lane change.");
-    change_state_if_stop_required();
-    return ModuleStatus::RUNNING;
-  }
-
-  if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) {
-    log_warn_throttled("Lane change path is unsafe. Cancel lane change.");
-    module_type_->toCancelState();
-    return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS;
-  }
-
-  if (!module_type_->isAbortEnabled()) {
-    log_warn_throttled(
-      "Lane change path is unsafe but abort was not enabled. Continue lane change.");
-    change_state_if_stop_required();
-    return ModuleStatus::RUNNING;
-  }
-
-  const auto found_abort_path = module_type_->calcAbortPath();
-  if (!found_abort_path) {
-    log_warn_throttled(
-      "Lane change path is unsafe but not found abort path. Continue lane change.");
-    change_state_if_stop_required();
-    return ModuleStatus::RUNNING;
-  }
-
-  log_warn_throttled("Lane change path is unsafe. Abort lane change.");
-  module_type_->toAbortState();
-  return ModuleStatus::RUNNING;
-}
-
 void LaneChangeInterface::updateData()
 {
   module_type_->setPreviousModulePaths(
@@ -188,6 +84,11 @@ void LaneChangeInterface::updateData()
   module_type_->resetStopPose();
 }
 
+void LaneChangeInterface::postProcess()
+{
+  post_process_safety_status_ = module_type_->isApprovedPathSafe();
+}
+
 BehaviorModuleOutput LaneChangeInterface::plan()
 {
   resetPathCandidate();
@@ -286,6 +187,130 @@ void LaneChangeInterface::setData(const std::shared_ptr<const PlannerData> & dat
   module_type_->setData(data);
 }
 
+bool LaneChangeInterface::canTransitSuccessState()
+{
+  auto log_debug_throttled = [&](std::string_view message) -> void {
+    RCLCPP_WARN(getLogger(), "%s", message.data());
+  };
+
+  if (module_type_->specialExpiredCheck() && isWaitingApproval()) {
+    log_debug_throttled("Run specialExpiredCheck.");
+    if (isWaitingApproval()) {
+      return true;
+    }
+  }
+
+  if (!module_type_->isValidPath()) {
+    log_debug_throttled("Has no valid path.");
+    return true;
+  }
+
+  if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) {
+    log_debug_throttled("Abort process has completed.");
+    return true;
+  }
+
+  if (module_type_->hasFinishedLaneChange()) {
+    log_debug_throttled("Lane change process has completed.");
+    return true;
+  }
+
+  log_debug_throttled("Lane changing process is ongoing");
+  return false;
+}
+
+bool LaneChangeInterface::canTransitFailureState()
+{
+  auto log_debug_throttled = [&](std::string_view message) -> void {
+    RCLCPP_WARN(getLogger(), "%s", message.data());
+  };
+
+  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;
+  }
+
+  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();
+      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.");
+      return true;
+    }
+  }
+
+  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;
+  }
+
+  if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) {
+    log_debug_throttled("Module require stopping");
+  }
+
+  if (!module_type_->isCancelEnabled()) {
+    log_debug_throttled(
+      "Lane change path is unsafe but cancel was not enabled. Continue lane change.");
+    return false;
+  }
+
+  if (!module_type_->isAbortEnabled()) {
+    log_debug_throttled(
+      "Lane change path is unsafe but abort was not enabled. Continue lane change.");
+    return false;
+  }
+
+  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;
+  }
+
+  log_debug_throttled("Lane change path is unsafe. Abort lane change.");
+  module_type_->toAbortState();
+  return false;
+}
+
+bool LaneChangeInterface::canTransitIdleToRunningState()
+{
+  setObjectDebugVisualization();
+
+  auto log_debug_throttled = [&](std::string_view message) -> void {
+    RCLCPP_WARN(getLogger(), "%s", message.data());
+  };
+
+  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;
+}
+
 void LaneChangeInterface::setObjectDebugVisualization() const
 {
   debug_marker_.markers.clear();

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -1526,11 +1526,17 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const
   return true;
 }
 
-bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) const
+bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear)
 {
   const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane;
-  return isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) &&
-         isAbleToStopSafely() && is_object_coming_from_rear;
+  if (
+    isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) &&
+    isAbleToStopSafely() && is_object_coming_from_rear) {
+    current_lane_change_state_ = LaneChangeStates::Stop;
+    return true;
+  }
+  current_lane_change_state_ = LaneChangeStates::Normal;
+  return false;
 }
 
 bool NormalLaneChange::calcAbortPath()