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

) Signed-off-by: Fumiya Watanabe <[email protected]>
autowarefoundation · May 27, 2024 · 92b5114 · 92b5114
1 parent 49f91a2
commit 92b5114
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/..._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)