feat(behavior_path_planner_common): update RTC state to SUCCEEDED (autowarefoundation#7119)

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Author: rej55

planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -46,6 +46,7 @@
 
 #include <algorithm>
 #include <any>
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -184,6 +185,10 @@ class SceneModuleInterface
   {
     RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__);
 
+    if (getCurrentStatus() == ModuleStatus::SUCCESS) {
+      updateRTCStatusForSuccess();
+    }
+
     clearWaitingApproval();
     removeRTCStatus();
     publishRTCStatus();
@@ -510,6 +515,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)