Feature/v2i prediction

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -781,6 +781,20 @@ class SimulatorCore
     {
       return core->getV2ITrafficLights()->resetUpdate(std::forward<decltype(xs)>(xs)...);
     }
+
+    template <typename... Ts>
+    static auto setV2ITrafficLightsStatePrediction(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getV2ITrafficLights()->setTrafficLightsStatePrediction(
+        std::forward<decltype(xs)>(xs)...);
+    }
+
+    template <typename... Ts>
+    static auto clearV2ITrafficLightsStatePrediction(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getV2ITrafficLights()->clearTrafficLightsStatePrediction(
+        std::forward<decltype(xs)>(xs)...);
+    }
   };
 };
 }  // namespace openscenario_interpreter

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/traffic_signal_controller.hpp

@@ -40,7 +40,8 @@ inline namespace syntax
  *  </xsd:complexType>
  *
  * -------------------------------------------------------------------------- */
-struct TrafficSignalController : private SimulatorCore::ConditionEvaluation
+struct TrafficSignalController : private SimulatorCore::ConditionEvaluation,
+                                 private SimulatorCore::NonStandardOperation
 {
   // ID of the traffic signal controller in the road network.
   const String name;
@@ -103,6 +104,8 @@ struct TrafficSignalController : private SimulatorCore::ConditionEvaluation
   auto notifyBegin() -> void;
 
   auto shouldChangePhaseToBegin() -> bool;
+
+  auto updatePredictions() -> void;
 };
 }  // namespace syntax
 }  // namespace openscenario_interpreter

openscenario/openscenario_interpreter/src/syntax/traffic_signal_controller.cpp

@@ -108,6 +108,53 @@ auto TrafficSignalController::evaluate() -> Object
   }
 }
 
+auto TrafficSignalController::updatePredictions() -> void
+{
+  if (current_phase != std::end(phases)) {
+    clearV2ITrafficLightsStatePrediction();
+    /*
+    NOTE:
+      This parameter was set with the help of developers familiar with the implementation
+      of ROS driver nodes for V2I traffic lights.
+      However, the specifications, including this parameter, may change in the future.
+   */
+    constexpr size_t OUTPUT_PREDICTION_SIZE = 6;
+    double accumulated_time = 0.0;
+    auto phase_iterator = current_phase;
+    std::size_t extracted_phases_number = 0;
+    std::unordered_map<lanelet::Id, std::vector<std::pair<double, std::string>>> predictions_by_id;
+
+    auto extract_prediction = [&](const auto & phase, const double phase_time_seconds) -> bool {
+      accumulated_time += phase_time_seconds;
+      bool is_added = false;
+      for (const auto & traffic_signal_state : (*phase).traffic_signal_states) {
+        if (
+          traffic_signal_state.trafficSignalType() == TrafficSignalState::TrafficSignalType::v2i) {
+          is_added = true;
+          predictions_by_id[traffic_signal_state.id()].emplace_back(
+            accumulated_time, traffic_signal_state.state);
+        }
+      }
+      return is_added;
+    };
+
+    while (extracted_phases_number < OUTPUT_PREDICTION_SIZE) {
+      ++phase_iterator;
+      if (extracted_phases_number == 0 && accumulated_time > cycleTime()) {
+        break;
+      } else if (extract_prediction(phase_iterator, (*phase_iterator).duration)) {
+        ++extracted_phases_number;
+      }
+    }
+
+    for (const auto & [lanelet_id, predictions] : predictions_by_id) {
+      for (const auto & [time_offset, state] : predictions) {
+        setV2ITrafficLightsStatePrediction(lanelet_id, state, time_offset);
+      }
+    }
+  }
+}
+
 auto TrafficSignalController::notifyBegin() -> void
 {
   change_to_begin_time = evaluateSimulationTime() + delay;

simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp

@@ -29,7 +29,8 @@ class TrafficLightPublisherBase
 public:
   virtual auto publish(
     const rclcpp::Time & current_ros_time,
-    const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void = 0;
+    const simulation_api_schema::UpdateTrafficLightsRequest & request,
+    const TrafficLightStatePredictions * predictions = nullptr) const -> void = 0;
 
   virtual ~TrafficLightPublisherBase() = default;
 };
@@ -53,14 +54,16 @@ class TrafficLightPublisher : public TrafficLightPublisherBase
 
   static auto generateMessage(
     const rclcpp::Time &, const simulation_api_schema::UpdateTrafficLightsRequest & request,
-    const std::string & frame = "") -> std::unique_ptr<MessageType>;
+    const std::string & frame = "", const TrafficLightStatePredictions * predictions = nullptr)
+    -> std::unique_ptr<MessageType>;
 
   auto publish(
     const rclcpp::Time & current_ros_time,
-    const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void override
+    const simulation_api_schema::UpdateTrafficLightsRequest & request,
+    const TrafficLightStatePredictions * predictions = nullptr) const -> void override
   {
     traffic_light_state_array_publisher_->publish(
-      generateMessage(current_ros_time, request, frame_));
+      generateMessage(current_ros_time, request, frame_, predictions));
   }
 
 private:

simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp

@@ -75,13 +75,18 @@ class V2ITrafficLights : public TrafficLightsBase
 
   ~V2ITrafficLights() override = default;
 
+  auto setTrafficLightsStatePrediction(
+    const lanelet::Id lanelet_way_id, const std::string & state, double time_ahead_seconds) -> void;
+
+  auto clearTrafficLightsStatePrediction() -> void;
+
 private:
   auto update() const -> void override
   {
     const auto now = clock_ptr_->now();
     const auto request = generateUpdateTrafficLightsRequest();
-    publisher_ptr_->publish(now, request);
-    legacy_topic_publisher_ptr_->publish(now, request);
+    publisher_ptr_->publish(now, request, &predictions_);
+    legacy_topic_publisher_ptr_->publish(now, request, &predictions_);
     if (isAnyTrafficLightChanged()) {
       marker_publisher_ptr_->deleteMarkers();
     }
@@ -126,6 +131,8 @@ class V2ITrafficLights : public TrafficLightsBase
 
   const std::unique_ptr<TrafficLightPublisherBase> publisher_ptr_;
   const std::unique_ptr<TrafficLightPublisherBase> legacy_topic_publisher_ptr_;
+
+  mutable TrafficLightStatePredictions predictions_;
 };
 
 class TrafficLights

simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp

@@ -93,5 +93,9 @@ class TrafficLightsBase
   const std::unique_ptr<TrafficLightMarkerPublisher> marker_publisher_ptr_;
   ConfigurableRateUpdater rate_updater_;
 };
+
+using TrafficLightStatePredictions = std::unordered_map<
+  lanelet::Id,
+  std::vector<std::pair<rclcpp::Time, std::vector<simulation_api_schema::TrafficLight>>>>;
 }  // namespace traffic_simulator
 #endif  // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_

simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp

@@ -15,6 +15,7 @@
 #include <simulation_interface/conversions.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light_publisher.hpp>
 #include <traffic_simulator_msgs/msg/traffic_light_array_v1.hpp>
+#include <type_traits>
 
 // This message will be deleted in the future
 #if __has_include(<autoware_perception_msgs/msg/traffic_signal_array.hpp>)
@@ -27,11 +28,23 @@
 
 namespace traffic_simulator
 {
+template <typename T, typename = void>
+struct HasMemberPredictions : std::false_type
+{
+};
+
+template <typename T>
+struct HasMemberPredictions<T, std::void_t<decltype(std::declval<T>().predictions)>>
+: std::true_type
+{
+};
+
 #if __has_include(<autoware_perception_msgs/msg/traffic_signal_array.hpp>)
 template <>
 auto TrafficLightPublisher<autoware_perception_msgs::msg::TrafficSignalArray>::generateMessage(
   const rclcpp::Time & current_ros_time,
-  const simulation_api_schema::UpdateTrafficLightsRequest & request, const std::string &)
+  const simulation_api_schema::UpdateTrafficLightsRequest & request, const std::string &,
+  const TrafficLightStatePredictions *)
   -> std::unique_ptr<autoware_perception_msgs::msg::TrafficSignalArray>
 {
   auto message = std::make_unique<autoware_perception_msgs::msg::TrafficSignalArray>();
@@ -63,7 +76,8 @@ auto TrafficLightPublisher<autoware_perception_msgs::msg::TrafficSignalArray>::g
 template <>
 auto TrafficLightPublisher<traffic_simulator_msgs::msg::TrafficLightArrayV1>::generateMessage(
   const rclcpp::Time &, const simulation_api_schema::UpdateTrafficLightsRequest & request,
-  const std::string &) -> std::unique_ptr<traffic_simulator_msgs::msg::TrafficLightArrayV1>
+  const std::string &, const TrafficLightStatePredictions *)
+  -> std::unique_ptr<traffic_simulator_msgs::msg::TrafficLightArrayV1>
 {
   auto message = std::make_unique<traffic_simulator_msgs::msg::TrafficLightArrayV1>();
 
@@ -87,31 +101,66 @@ auto TrafficLightPublisher<traffic_simulator_msgs::msg::TrafficLightArrayV1>::ge
 template <>
 auto TrafficLightPublisher<autoware_perception_msgs::msg::TrafficLightGroupArray>::generateMessage(
   const rclcpp::Time & current_ros_time,
-  const simulation_api_schema::UpdateTrafficLightsRequest & request, const std::string &)
+  const simulation_api_schema::UpdateTrafficLightsRequest & request, const std::string &,
+  const TrafficLightStatePredictions * predictions)
   -> std::unique_ptr<autoware_perception_msgs::msg::TrafficLightGroupArray>
 {
-  auto message = std::make_unique<autoware_perception_msgs::msg::TrafficLightGroupArray>();
+  using autoware_perception_msgs::msg::PredictedTrafficLightState;
+  using autoware_perception_msgs::msg::TrafficLightElement;
+  using autoware_perception_msgs::msg::TrafficLightGroup;
 
-  message->stamp = current_ros_time;
-
-  using TrafficLightGroupType = autoware_perception_msgs::msg::TrafficLightGroup;
-  using TrafficLightBulbType = autoware_perception_msgs::msg::TrafficLightElement;
+  // NOTE: key is relation ID
+  std::unordered_map<lanelet::Id, TrafficLightGroup> traffic_light_group_map;
+  for (const auto & way_id_level_traffic_light : request.states()) {
+    for (const auto & bulb_proto : way_id_level_traffic_light.traffic_light_status()) {
+      TrafficLightElement bulb_message;
+      simulation_interface::toMsg<TrafficLightElement>(bulb_proto, bulb_message);
+      for (const auto & relation_id : way_id_level_traffic_light.relation_ids()) {
+        traffic_light_group_map[relation_id].elements.push_back(bulb_message);
+      }
+    }
+    if constexpr (HasMemberPredictions<TrafficLightGroup>::value) {
+      if (predictions) {
+        if (const auto prediction_phases = predictions->find(way_id_level_traffic_light.id());
+            prediction_phases != predictions->end()) {
+          for (const auto & [stamp, bulbs] : prediction_phases->second) {
+            for (const auto & relation_id : way_id_level_traffic_light.relation_ids()) {
+              auto prediction = [&]() -> std::reference_wrapper<PredictedTrafficLightState> {
+                auto & predictions_message = traffic_light_group_map[relation_id].predictions;
+                if (auto matched_prediction = std::find_if(
+                      predictions_message.begin(), predictions_message.end(),
+                      [&stamp](const auto & p) { return p.predicted_stamp == stamp; });
+                    matched_prediction != predictions_message.end()) {
+                  return *matched_prediction;
+                } else {
+                  return predictions_message.emplace_back(
+                    PredictedTrafficLightState()
+                      .set__predicted_stamp(stamp)
+                      .set__information_source(
+                        PredictedTrafficLightState::INFORMATION_SOURCE_SIMULATION)
+                      .set__reliability(1.0));
+                }
+              }();
 
-  for (const auto & traffic_light : request.states()) {
-    for (const auto & relation_id : traffic_light.relation_ids()) {
-      // skip if the traffic light has no bulbs
-      if (not traffic_light.traffic_light_status().empty()) {
-        TrafficLightGroupType traffic_light_group_message;
-        traffic_light_group_message.traffic_light_group_id = relation_id;
-        for (auto bulb_status : traffic_light.traffic_light_status()) {
-          TrafficLightBulbType light_bulb_message;
-          simulation_interface::toMsg<TrafficLightBulbType>(bulb_status, light_bulb_message);
-          traffic_light_group_message.elements.push_back(light_bulb_message);
+              for (const auto & bulb_proto : bulbs) {
+                TrafficLightElement bulb_message;
+                simulation_interface::toMsg<TrafficLightElement>(bulb_proto, bulb_message);
+                prediction.get().simultaneous_elements.push_back(bulb_message);
+              }
+            }
+          }
         }
-        message->traffic_light_groups.push_back(traffic_light_group_message);
       }
     }
   }
+
+  auto message = std::make_unique<autoware_perception_msgs::msg::TrafficLightGroupArray>();
+  message->stamp = current_ros_time;
+
+  for (const auto & [relation_id, traffic_light_group] : traffic_light_group_map) {
+    message->traffic_light_groups.push_back(traffic_light_group);
+    message->traffic_light_groups.back().traffic_light_group_id = relation_id;
+  }
   return message;
 }
 #endif  // __has_include(<autoware_perception_msgs/msg/traffic_light_group_array.hpp>)

simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp

@@ -16,6 +16,30 @@
 
 namespace traffic_simulator
 {
+auto V2ITrafficLights::setTrafficLightsStatePrediction(
+  const lanelet::Id lanelet_way_id, const std::string & state, double time_ahead_seconds) -> void
+{
+  const auto predicted_time =
+    clock_ptr_->now() + rclcpp::Duration(std::chrono::duration<double>(time_ahead_seconds));
+  auto & predictions_for_current_traffic_light = predictions_[lanelet_way_id];
+  auto bulb_proto = static_cast<simulation_api_schema::TrafficLight>(TrafficLight::Bulb(state));
+  if (auto prediction = std::find_if(
+        predictions_for_current_traffic_light.begin(), predictions_for_current_traffic_light.end(),
+        [&predicted_time](const auto & pair) {
+          constexpr double TIME_EPSILON = 0.001;  // 1ms
+          return std::abs((pair.first - predicted_time).seconds()) < TIME_EPSILON;
+        });
+      prediction != predictions_for_current_traffic_light.end()) {
+    // merge into existing prediction
+    prediction->second.push_back(bulb_proto);
+  } else {
+    predictions_for_current_traffic_light.emplace_back(
+      predicted_time, std::vector<simulation_api_schema::TrafficLight>{bulb_proto});
+  }
+}
+
+auto V2ITrafficLights::clearTrafficLightsStatePrediction() -> void { predictions_.clear(); }
+
 auto TrafficLights::isAnyTrafficLightChanged() -> bool
 {
   return conventional_traffic_lights_->isAnyTrafficLightChanged() or