fix(behavior_path_planner): do not return FAILURE in lane change modu…

…le if it uses new architecture (autowarefoundation#3820) (autowarefoundation#545) Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
kosuke55 · May 30, 2023 · 04c7bca · 04c7bca
1 parent 9bad135
commit 04c7bca
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 5 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
@@ -100,6 +100,8 @@ class LaneChangeInterface : public SceneModuleInterface
 
   void resetPathIfAbort();
 
+  void resetLaneChangeModule();
+
   void setObjectDebugVisualization() const;
 
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);

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
@@ -88,13 +88,22 @@ bool LaneChangeInterface::isExecutionReady() const
 ModuleStatus LaneChangeInterface::updateState()
 {
   if (!module_type_->isValidPath()) {
+#ifdef USE_OLD_ARCHITECTURE
     return ModuleStatus::FAILURE;
+#else
+    return ModuleStatus::RUNNING;
+#endif
   }
 
   if (module_type_->isAbortState()) {
-    current_state_ =
-      module_type_->hasFinishedAbort() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING;
-    return current_state_;
+#ifdef USE_OLD_ARCHITECTURE
+    return module_type_->hasFinishedAbort() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING;
+#else
+    if (module_type_->hasFinishedAbort()) {
+      resetLaneChangeModule();
+    }
+    return ModuleStatus::RUNNING;
+#endif
   }
 
   if (module_type_->hasFinishedLaneChange()) {
@@ -152,8 +161,14 @@ ModuleStatus LaneChangeInterface::updateState()
     RCLCPP_WARN_STREAM_THROTTLE(
       getLogger(), *clock_, 5000, "Lane change path is unsafe. Cancel lane change.");
     module_type_->toCancelState();
-    current_state_ = isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::FAILURE;
-    return current_state_;
+#ifdef USE_OLD_ARCHITECTURE
+    return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::FAILURE;
+#else
+    if (!isWaitingApproval()) {
+      resetLaneChangeModule();
+    }
+    return ModuleStatus::RUNNING;
+#endif
   }
 
   if (!module_type_->isAbortEnabled()) {
@@ -190,6 +205,13 @@ ModuleStatus LaneChangeInterface::updateState()
   return current_state_;
 }
 
+void LaneChangeInterface::resetLaneChangeModule()
+{
+  processOnExit();
+  removeRTCStatus();
+  processOnEntry();
+}
+
 void LaneChangeInterface::updateData()
 {
   module_type_->setPreviousModulePaths(