From 6a833541b426c3d451d5a67d2b298e8a92bd752c Mon Sep 17 00:00:00 2001
From: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Date: Tue, 7 Nov 2023 17:49:18 +0900
Subject: [PATCH] refactor(lane_change): update state log

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../scene_module/lane_change/interface.cpp    | 68 ++++++++-----------
 1 file changed, 28 insertions(+), 40 deletions(-)

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
index be3b78803c4c6..7534c2c1fe45f 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -76,25 +76,35 @@ bool LaneChangeInterface::isExecutionReady() const
 
 ModuleStatus LaneChangeInterface::updateState()
 {
+  auto log_warn_throttled = [&](const std::string & message) -> void {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      getLogger().get_child(module_type_->getModuleTypeStr()), *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;
   }
 
@@ -102,31 +112,29 @@ ModuleStatus LaneChangeInterface::updateState()
 
   setObjectDebugVisualization();
   if (is_safe) {
+    log_warn_throttled("Lane change path is safe.");
     module_type_->toNormalState();
     return ModuleStatus::RUNNING;
   }
 
-  if (!module_type_->isCancelEnabled()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
-      "Lane change path is unsafe but cancel was not enabled. Continue lane change.");
+  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()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
-      "Lane change path is unsafe but cannot return. Continue lane change.");
-    if (module_type_->isRequiredStop(is_object_coming_from_rear)) {
-      module_type_->toStopState();
-    } else {
-      module_type_->toNormalState();
-    }
+    log_warn_throttled("Lane change path is unsafe but cannot return. Continue lane change.");
+    change_state_if_stop_required();
     return ModuleStatus::RUNNING;
   }
 
@@ -134,53 +142,33 @@ ModuleStatus LaneChangeInterface::updateState()
   const auto threshold = common_parameters.backward_length_buffer_for_end_of_lane;
   const auto status = module_type_->getLaneChangeStatus();
   if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
-      "Lane change path is unsafe but near end of lane. Continue lane change.");
-    if (module_type_->isRequiredStop(is_object_coming_from_rear)) {
-      module_type_->toStopState();
-    } else {
-      module_type_->toNormalState();
-    }
+    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()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
-      "Lane change path is unsafe. Cancel lane change.");
+    log_warn_throttled("Lane change path is unsafe. Cancel lane change.");
     module_type_->toCancelState();
     return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS;
   }
 
   if (!module_type_->isAbortEnabled()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
+    log_warn_throttled(
       "Lane change path is unsafe but abort was not enabled. Continue lane change.");
-    if (module_type_->isRequiredStop(is_object_coming_from_rear)) {
-      module_type_->toStopState();
-    } else {
-      module_type_->toNormalState();
-    }
+    change_state_if_stop_required();
     return ModuleStatus::RUNNING;
   }
 
   const auto found_abort_path = module_type_->getAbortPath();
   if (!found_abort_path) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
+    log_warn_throttled(
       "Lane change path is unsafe but not found abort path. Continue lane change.");
-    if (module_type_->isRequiredStop(is_object_coming_from_rear)) {
-      module_type_->toStopState();
-    } else {
-      module_type_->toNormalState();
-    }
+    change_state_if_stop_required();
     return ModuleStatus::RUNNING;
   }
 
-  RCLCPP_WARN_STREAM_THROTTLE(
-    getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
-    "Lane change path is unsafe. Abort lane change.");
+  log_warn_throttled("Lane change path is unsafe. Abort lane change.");
   module_type_->toAbortState();
   return ModuleStatus::RUNNING;
 }