feat(bvp): support new rtc cooperate status

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_velocity_blind_spot_module/src/manager.cpp

@@ -73,7 +73,8 @@ void BlindSpotModuleManager::launchNewModules(
       clock_));
     generateUUID(module_id);
     updateRTCStatus(
-      getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
+      getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits<double>::lowest(),
+      path.header.stamp);
   }
 }
 

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -190,8 +190,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
       clock_));
     generateUUID(crosswalk_lanelet_id);
     updateRTCStatus(
-      getUUID(crosswalk_lanelet_id), true, std::numeric_limits<double>::lowest(),
-      path.header.stamp);
+      getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION,
+      std::numeric_limits<double>::lowest(), path.header.stamp);
   };
 
   const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(

planning/behavior_velocity_detection_area_module/src/manager.cpp

@@ -66,7 +66,8 @@ void DetectionAreaModuleManager::launchNewModules(
         logger_.get_child("detection_area_module"), clock_));
       generateUUID(module_id);
       updateRTCStatus(
-        getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
+        getUUID(module_id), true, State::WAITING_FOR_EXECUTION,
+        std::numeric_limits<double>::lowest(), path.header.stamp);
     }
   }
 }

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -345,10 +345,10 @@ void IntersectionModuleManager::launchNewModules(
     const UUID uuid = getUUID(new_module->getModuleId());
     const auto occlusion_uuid = new_module->getOcclusionUUID();
     rtc_interface_.updateCooperateStatus(
-      uuid, true, std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(),
-      clock_->now());
+      uuid, true, State::RUNNING, std::numeric_limits<double>::lowest(),
+      std::numeric_limits<double>::lowest(), clock_->now());
     occlusion_rtc_interface_.updateCooperateStatus(
-      occlusion_uuid, true, std::numeric_limits<double>::lowest(),
+      occlusion_uuid, true, State::RUNNING, std::numeric_limits<double>::lowest(),
       std::numeric_limits<double>::lowest(), clock_->now());
     registerModule(std::move(new_module));
   }
@@ -404,12 +404,13 @@ void IntersectionModuleManager::sendRTC(const Time & stamp)
     const UUID uuid = getUUID(scene_module->getModuleId());
     const bool safety =
       scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired());
-    updateRTCStatus(uuid, safety, scene_module->getDistance(), stamp);
+    updateRTCStatus(uuid, safety, State::RUNNING, scene_module->getDistance(), stamp);
     const auto occlusion_uuid = intersection_module->getOcclusionUUID();
     const auto occlusion_distance = intersection_module->getOcclusionDistance();
     const auto occlusion_safety = intersection_module->getOcclusionSafety();
     occlusion_rtc_interface_.updateCooperateStatus(
-      occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
+      occlusion_uuid, occlusion_safety, State::RUNNING, occlusion_distance, occlusion_distance,
+      stamp);
 
     // ==========================================================================================
     // module debug data

planning/behavior_velocity_no_stopping_area_module/src/manager.cpp

@@ -68,7 +68,8 @@ void NoStoppingAreaModuleManager::launchNewModules(
         clock_));
       generateUUID(module_id);
       updateRTCStatus(
-        getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
+        getUUID(module_id), true, State::WAITING_FOR_EXECUTION,
+        std::numeric_limits<double>::lowest(), path.header.stamp);
     }
   }
 }

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -31,6 +31,7 @@
 #include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/stop_reason.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
+#include <tier4_rtc_msgs/msg/state.hpp>
 #include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
@@ -58,6 +59,7 @@ using tier4_debug_msgs::msg::Float64Stamped;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
 using tier4_rtc_msgs::msg::Module;
+using tier4_rtc_msgs::msg::State;
 using unique_identifier_msgs::msg::UUID;
 
 struct ObjectOfInterest
@@ -235,9 +237,10 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
   virtual void setActivation();
 
   void updateRTCStatus(
-    const UUID & uuid, const bool safe, const double distance, const Time & stamp)
+    const UUID & uuid, const bool safe, const uint8_t state, const double distance,
+    const Time & stamp)
   {
-    rtc_interface_.updateCooperateStatus(uuid, safe, distance, distance, stamp);
+    rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp);
   }
 
   void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); }

planning/behavior_velocity_planner_common/src/scene_module_interface.cpp

@@ -211,7 +211,8 @@ void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp)
 {
   for (const auto & scene_module : scene_modules_) {
     const UUID uuid = getUUID(scene_module->getModuleId());
-    updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
+    const auto state = scene_module->isActivated() ? State::RUNNING : State::WAITING_FOR_EXECUTION;
+    updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp);
   }
   publishRTCStatus(stamp);
 }

planning/behavior_velocity_traffic_light_module/src/manager.cpp

@@ -133,7 +133,8 @@ void TrafficLightModuleManager::launchNewModules(
         planner_param_, logger_.get_child("traffic_light_module"), clock_));
       generateUUID(module_id);
       updateRTCStatus(
-        getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
+        getUUID(module_id), true, State::WAITING_FOR_EXECUTION,
+        std::numeric_limits<double>::lowest(), path.header.stamp);
     }
   }
 }