feat(rtc_interface)!: add new field to rtc cooperate status (autoware…

…foundation#6933) * feat(rtc_interface): add new field Signed-off-by: satoshi-ota <[email protected]> * feat(bvp): support new rtc cooperate status Signed-off-by: satoshi-ota <[email protected]> * feat(bpp): support new rtc cooperate status Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
tier4 · Aug 1, 2024 · b6518dd · b6518dd
1 parent 3cc9924
commit b6518dd
Show file tree

Hide file tree

Showing 14 changed files with 47 additions and 24 deletions.
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp
@@ -57,7 +57,10 @@ void AvoidanceByLaneChangeInterface::updateRTCStatus(
     return (dir == Direction::LEFT) ? "left" : "right";
   });
 
+  const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING;
+
   rtc_interface_ptr_map_.at(direction)->updateCooperateStatus(
-    uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now());
+    uuid_map_.at(direction), isExecutionReady(), state, start_distance, finish_distance,
+    clock_->now());
 }
 }  // namespace behavior_path_planner
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
@@ -77,15 +77,17 @@ class AvoidanceModule : public SceneModuleInterface
   {
     if (candidate.lateral_shift > 0.0) {
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
-        uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change,
-        candidate.finish_distance_to_path_change, clock_->now());
+        uuid_map_.at("left"), isExecutionReady(), State::WAITING_FOR_EXECUTION,
+        candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
+        clock_->now());
       candidate_uuid_ = uuid_map_.at("left");
       return;
     }
     if (candidate.lateral_shift < 0.0) {
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
-        uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change,
-        candidate.finish_distance_to_path_change, clock_->now());
+        uuid_map_.at("right"), isExecutionReady(), State::WAITING_FOR_EXECUTION,
+        candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
+        clock_->now());
       candidate_uuid_ = uuid_map_.at("right");
       return;
     }

diff --git a/...ning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp b/...ning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp
@@ -28,6 +28,7 @@
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
+#include <tier4_rtc_msgs/msg/state.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 namespace behavior_path_planner
@@ -49,6 +50,7 @@ using visualization_msgs::msg::MarkerArray;
 // tier4 msgs
 using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
+using tier4_rtc_msgs::msg::State;
 
 // tier4 utils functions
 using tier4_autoware_utils::appendMarkerArray;

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
@@ -41,6 +41,7 @@
 #include <tier4_planning_msgs/msg/stop_factor.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 <unique_identifier_msgs/msg/uuid.hpp>
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
@@ -67,6 +68,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
 using tier4_planning_msgs::msg::StopReasonArray;
+using tier4_rtc_msgs::msg::State;
 using unique_identifier_msgs::msg::UUID;
 using visualization_msgs::msg::MarkerArray;
 using PlanResult = PathWithLaneId::SharedPtr;
@@ -503,8 +505,9 @@ class SceneModuleInterface
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
       if (ptr) {
+        const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING;
         ptr->updateCooperateStatus(
-          uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance,
+          uuid_map_.at(module_name), isExecutionReady(), state, start_distance, finish_distance,
           clock_->now());
       }
     }

diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp
@@ -79,7 +79,8 @@ void BlindSpotModuleManager::launchNewModules(
       logger_.get_child("blind_spot_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);
   }
 }
 

diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/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>(

diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/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);
     }
   }
 }

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/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

diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/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);
     }
   }
 }

diff --git a/...locity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/...locity_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); }

diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/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);
 }

diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
@@ -132,7 +132,8 @@ void TrafficLightModuleManager::launchNewModules(
         logger_.get_child("traffic_light_module"), clock_));
       generateUUID(lane_id);
       updateRTCStatus(
-        getUUID(lane_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
+        getUUID(lane_id), true, State::WAITING_FOR_EXECUTION,
+        std::numeric_limits<double>::lowest(), path.header.stamp);
     }
   }
 }

diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp
@@ -24,6 +24,7 @@
 #include "tier4_rtc_msgs/msg/cooperate_status.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
 #include "tier4_rtc_msgs/msg/module.hpp"
+#include "tier4_rtc_msgs/msg/state.hpp"
 #include "tier4_rtc_msgs/srv/auto_mode.hpp"
 #include "tier4_rtc_msgs/srv/cooperate_commands.hpp"
 #include <unique_identifier_msgs/msg/uuid.hpp>
@@ -41,6 +42,7 @@ using tier4_rtc_msgs::msg::CooperateResponse;
 using tier4_rtc_msgs::msg::CooperateStatus;
 using tier4_rtc_msgs::msg::CooperateStatusArray;
 using tier4_rtc_msgs::msg::Module;
+using tier4_rtc_msgs::msg::State;
 using tier4_rtc_msgs::srv::AutoMode;
 using tier4_rtc_msgs::srv::CooperateCommands;
 using unique_identifier_msgs::msg::UUID;
@@ -51,8 +53,8 @@ class RTCInterface
   RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc = true);
   void publishCooperateStatus(const rclcpp::Time & stamp);
   void updateCooperateStatus(
-    const UUID & uuid, const bool safe, const double start_distance, const double finish_distance,
-    const rclcpp::Time & stamp);
+    const UUID & uuid, const bool safe, const uint8_t state, const double start_distance,
+    const double finish_distance, const rclcpp::Time & stamp);
   void removeCooperateStatus(const UUID & uuid);
   void clearCooperateStatus();
   bool isActivated(const UUID & uuid) const;

diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp
@@ -201,8 +201,8 @@ void RTCInterface::onTimer()
 }
 
 void RTCInterface::updateCooperateStatus(
-  const UUID & uuid, const bool safe, const double start_distance, const double finish_distance,
-  const rclcpp::Time & stamp)
+  const UUID & uuid, const bool safe, const uint8_t state, const double start_distance,
+  const double finish_distance, const rclcpp::Time & stamp)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   // Find registered status which has same uuid
@@ -218,6 +218,7 @@ void RTCInterface::updateCooperateStatus(
     status.module = module_;
     status.safe = safe;
     status.command_status.type = Command::DEACTIVATE;
+    status.state.type = State::WAITING_FOR_EXECUTION;
     status.start_distance = start_distance;
     status.finish_distance = finish_distance;
     status.auto_mode = is_auto_mode_enabled_;
@@ -228,6 +229,7 @@ void RTCInterface::updateCooperateStatus(
   // If the registered status is found, update status
   itr->stamp = stamp;
   itr->safe = safe;
+  itr->state.type = state;
   itr->start_distance = start_distance;
   itr->finish_distance = finish_distance;
 }