From 92b51143028ad113a77abcd8fb5e7f424c629769 Mon Sep 17 00:00:00 2001
From: Fumiya Watanabe <rej55.g@gmail.com>
Date: Mon, 27 May 2024 14:46:10 +0900
Subject: [PATCH] feat(behavior_path_planner_common): update RTC state to
 SUCCEEDED (#7119)

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
---
 .../interface/scene_module_interface.hpp         | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
index bd7949a83a1d7..1035aff5f7093 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
@@ -47,6 +47,7 @@
 
 #include <algorithm>
 #include <any>
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -186,6 +187,10 @@ class SceneModuleInterface
   {
     RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__);
 
+    if (getCurrentStatus() == ModuleStatus::SUCCESS) {
+      updateRTCStatusForSuccess();
+    }
+
     clearWaitingApproval();
     unlockNewModuleLaunch();
     unlockOutputPath();
@@ -499,6 +504,17 @@ class SceneModuleInterface
     }
   }
 
+  void updateRTCStatusForSuccess()
+  {
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->updateCooperateStatus(
+          uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits<double>::lowest(),
+          std::numeric_limits<double>::lowest(), clock_->now());
+      }
+    }
+  }
+
   void setObjectsOfInterestData(
     const geometry_msgs::msg::Pose & obj_pose,
     const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name)