override updateState for new and old arch.

Signed-off-by: Muhammad Zulfaqar <[email protected]>
kyoichi-sugahara · May 12, 2023 · 4c509e3 · 4c509e3
1 parent b47f9ba
commit 4c509e3
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 2 deletions.
diff --git a/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp
@@ -168,6 +168,8 @@ class LaneChangeBTModule : public LaneChangeBTInterface
     const std::string & name, rclcpp::Node & node,
     const std::shared_ptr<LaneChangeParameters> & parameters);
 
+  ModuleStatus updateState() override;
+
 protected:
   void updateRTCStatus(const double start_distance, const double finish_distance) override;
 };

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
@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/scene_module/lane_change/interface.hpp"
 
+#include "behavior_path_planner/module_status.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
@@ -97,8 +98,11 @@ ModuleStatus LaneChangeInterface::updateState()
   }
 
   if (module_type_->isCancelConditionSatisfied()) {
-    current_state_ =
-      module_type_->isCancelEnabled() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING;
+    if (module_type_->isCancelEnabled()) {
+      current_state_ = isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::FAILURE;
+    } else {
+      current_state_ = ModuleStatus::RUNNING;
+    }
     return current_state_;
   }
 
@@ -474,6 +478,33 @@ LaneChangeBTModule::LaneChangeBTModule(
 {
 }
 
+ModuleStatus LaneChangeBTModule::updateState()
+{
+  if (!module_type_->isValidPath()) {
+    current_state_ = ModuleStatus::FAILURE;
+    return current_state_;
+  }
+
+  if (module_type_->isAbortState()) {
+    current_state_ = ModuleStatus::RUNNING;
+    return current_state_;
+  }
+
+  if (module_type_->isCancelConditionSatisfied()) {
+    current_state_ =
+      module_type_->isCancelEnabled() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING;
+    return current_state_;
+  }
+
+  if (module_type_->hasFinishedLaneChange()) {
+    current_state_ = ModuleStatus::SUCCESS;
+    return current_state_;
+  }
+
+  current_state_ = ModuleStatus::RUNNING;
+  return current_state_;
+}
+
 void LaneChangeBTModule::updateRTCStatus(const double start_distance, const double finish_distance)
 {
   const auto direction = std::invoke([&]() -> std::string {