Feature/v2i remain time

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -616,6 +616,12 @@ class SimulatorCore
     {
       return core->setConventionalTrafficLightConfidence(std::forward<decltype(xs)>(xs)...);
     }
+
+    template <typename... Ts>
+    static auto setV2ITrafficLightExtraInfo(Ts &&... xs) -> decltype(auto)
+    {
+      return core->setV2ITrafficLightExtraInfo(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;
@@ -102,6 +103,8 @@ struct TrafficSignalController : private SimulatorCore::ConditionEvaluation
 
   auto notifyBegin() -> void;
 
+  auto restTimeToRed(const lanelet::Id) const -> double;
+
   auto shouldChangePhaseToBegin() -> bool;
 };
 }  // namespace syntax

openscenario/openscenario_interpreter/src/syntax/traffic_signal_controller.cpp

@@ -99,20 +99,67 @@ auto TrafficSignalController::cycleTime() const -> double
 
 auto TrafficSignalController::evaluate() -> Object
 {
-  if (shouldChangePhaseToBegin()) {
-    return changePhaseTo(std::begin(phases));
-  } else if (currentPhaseExceeded()) {
-    return changePhaseTo(std::next(current_phase));
-  } else {
-    return unspecified;
+  auto updated = [&]() {
+    if (shouldChangePhaseToBegin()) {
+      return changePhaseTo(std::begin(phases));
+    } else if (currentPhaseExceeded()) {
+      return changePhaseTo(std::next(current_phase));
+    } else {
+      return unspecified;
+    }
+  }();
+
+  double current_phase_rest_time =
+    (*current_phase).duration - (evaluateSimulationTime() - current_phase_started_at);
+
+  for (const auto traffic_signal_state : (*current_phase).traffic_signal_states) {
+    setV2ITrafficLightExtraInfo(
+      boost::lexical_cast<std::int64_t>(traffic_signal_state.traffic_signal_id),
+      current_phase_rest_time, restTimeToRed(traffic_signal_state.id()));
   }
+  return updated;
 }
 
 auto TrafficSignalController::notifyBegin() -> void
 {
   change_to_begin_time = evaluateSimulationTime() + delay;
 }
 
+auto TrafficSignalController::restTimeToRed(const lanelet::Id traffic_signal_id) const -> double
+{
+  auto is_red = [](const TrafficSignalState & state) {
+    return state.state.find("red") != std::string::npos;
+  };
+
+  auto find_state = [traffic_signal_id](const auto & phase) {
+    return std::find_if(
+      (*phase).traffic_signal_states.begin(), (*phase).traffic_signal_states.end(),
+      [&](const auto & state) { return state.id() == traffic_signal_id; });
+  };
+
+  (*current_phase).traffic_signal_states.front().state;
+  if (current_phase == std::end(phases)) {
+    return 0;
+  } else if (is_red(*find_state(current_phase))) {
+    return 0;
+  } else {
+    double rest_time_to_red =
+      (*current_phase).duration - (evaluateSimulationTime() - current_phase_started_at);
+    auto iterator = current_phase;
+    iterator++;
+    while (not is_red(*find_state(iterator))) {
+      // if there is INF phase to red phase, "rest_time_to_red == cycleTime()" is true (INF == INF) and return 0
+      if (rest_time_to_red >= cycleTime()) {
+        // no red phase or some INF phases to red phase
+        return 0;
+      }
+      rest_time_to_red += (*iterator).duration;
+      iterator++;
+    }
+    return rest_time_to_red;
+  }
+}
+
 auto TrafficSignalController::shouldChangePhaseToBegin() -> bool
 {
   if (reference.empty()) {

openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp

@@ -33,6 +33,12 @@ auto TrafficSignalState::evaluate() const -> Object
     traffic_light.set(state);
   };
 
+  // set same state to V2I
+  for (traffic_simulator::TrafficLight & traffic_light : getV2ITrafficLights(id())) {
+    traffic_light.clear();
+    traffic_light.set(state);
+  };
+
   return unspecified;
 }
 

simulation/simulation_interface/include/simulation_interface/conversions.hpp

@@ -203,6 +203,8 @@ auto toMsg(
         return TrafficLightBulbMessageType::GREEN;
       case TrafficLight_Color_WHITE:
         return TrafficLightBulbMessageType::WHITE;
+      case TrafficLight_Color_UNKNOWN_COLOR:
+        return TrafficLightBulbMessageType::UNKNOWN;
       default:
         return TrafficLightBulbMessageType::UNKNOWN;
     }
@@ -232,6 +234,8 @@ auto toMsg(
         return TrafficLightBulbMessageType::DOWN_RIGHT_ARROW;
       case TrafficLight_Shape_CROSS:
         return TrafficLightBulbMessageType::CROSS;
+      case TrafficLight_Shape_UNKNOWN_SHAPE:
+        return TrafficLightBulbMessageType::UNKNOWN;
       default:
         return TrafficLightBulbMessageType::UNKNOWN;
     }
@@ -246,6 +250,8 @@ auto toMsg(
         return TrafficLightBulbMessageType::SOLID_ON;
       case TrafficLight_Status_FLASHING:
         return TrafficLightBulbMessageType::FLASHING;
+      case TrafficLight_Status_UNKNOWN_STATUS:
+        return TrafficLightBulbMessageType::UNKNOWN;
       default:
         return TrafficLightBulbMessageType::UNKNOWN;
     }

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -381,6 +381,7 @@ class API
   FORWARD_TO_ENTITY_MANAGER(setMapPose);
   FORWARD_TO_ENTITY_MANAGER(setTwist);
   FORWARD_TO_ENTITY_MANAGER(setVelocityLimit);
+  FORWARD_TO_ENTITY_MANAGER(setV2ITrafficLightExtraInfo);
   FORWARD_TO_ENTITY_MANAGER(toMapPose);
 
 private:

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -42,6 +42,7 @@
 #include <traffic_simulator/traffic_lights/configurable_rate_updater.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light_publisher.hpp>
+#include <traffic_simulator/traffic_lights/v2i_traffic_light_info_publisher.hpp>
 #include <traffic_simulator/utils/node_parameters.hpp>
 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
@@ -108,6 +109,7 @@ class EntityManager
   const std::shared_ptr<TrafficLightMarkerPublisher> v2i_traffic_light_marker_publisher_ptr_;
   const std::shared_ptr<TrafficLightPublisherBase> v2i_traffic_light_legacy_topic_publisher_ptr_;
   const std::shared_ptr<TrafficLightPublisherBase> v2i_traffic_light_publisher_ptr_;
+  const std::shared_ptr<V2ITrafficLightInfoPublisher> v2i_traffic_light_info_publisher_ptr_;
   ConfigurableRateUpdater v2i_traffic_light_updater_, conventional_traffic_light_updater_;
 
 public:
@@ -177,6 +179,9 @@ class EntityManager
       makeV2ITrafficLightPublisher("/v2x/traffic_signals", node, hdmap_utils_ptr_)),
     v2i_traffic_light_publisher_ptr_(makeV2ITrafficLightPublisher(
       "/perception/traffic_light_recognition/external/traffic_signals", node, hdmap_utils_ptr_)),
+    v2i_traffic_light_info_publisher_ptr_(std::make_shared<V2ITrafficLightInfoPublisher>(
+      "/v2i/external/v2i_traffic_light_info", node, conventional_traffic_light_manager_ptr_,
+      hdmap_utils_ptr_)),
     v2i_traffic_light_updater_(
       node,
       [this]() {
@@ -186,8 +191,10 @@ class EntityManager
         v2i_traffic_light_legacy_topic_publisher_ptr_->publish(
           clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest());
       }),
-    conventional_traffic_light_updater_(
-      node, [this]() { conventional_traffic_light_marker_publisher_ptr_->publish(); })
+    conventional_traffic_light_updater_(node, [this]() {
+      conventional_traffic_light_marker_publisher_ptr_->publish();
+      v2i_traffic_light_info_publisher_ptr_->publish();
+    })
   {
     updateHdmapMarker();
   }
@@ -236,6 +243,13 @@ class EntityManager
     }
   }
 
+  auto setV2ITrafficLightExtraInfo(
+    lanelet::Id id, double current_phase_rest_time, double rest_time_to_red) -> void
+  {
+    v2i_traffic_light_info_publisher_ptr_->setTrafficLightExtraInfo(
+      id, current_phase_rest_time, rest_time_to_red);
+  }
+
   // clang-format off
 #define FORWARD_TO_HDMAP_UTILS(NAME)                                  \
   /*!                                                                 \

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

@@ -45,13 +45,15 @@ struct TrafficLight
       yellow,
       red,
       white,
+      unknown,
     } value;
 
     // clang-format off
-    static_assert(static_cast<std::uint8_t>(green ) == 0b0000'0000);
-    static_assert(static_cast<std::uint8_t>(yellow) == 0b0000'0001);
-    static_assert(static_cast<std::uint8_t>(red   ) == 0b0000'0010);
-    static_assert(static_cast<std::uint8_t>(white ) == 0b0000'0011);
+    static_assert(static_cast<std::uint8_t>(green  ) == 0b0000'0000);
+    static_assert(static_cast<std::uint8_t>(yellow ) == 0b0000'0001);
+    static_assert(static_cast<std::uint8_t>(red    ) == 0b0000'0010);
+    static_assert(static_cast<std::uint8_t>(white  ) == 0b0000'0011);
+    static_assert(static_cast<std::uint8_t>(unknown) == 0b0000'0100);
     // clang-format on
 
     constexpr Color(const Value value = green) : value(value) {}
@@ -64,6 +66,7 @@ struct TrafficLight
       std::make_pair("red", red),
       std::make_pair("white", white),
       std::make_pair("yellow", yellow),
+      std::make_pair("unknown", unknown),
 
       // BACKWARD COMPATIBILITY
       std::make_pair("Green", green),
@@ -132,26 +135,29 @@ struct TrafficLight
       circle,
       cross,
       arrow,
+      unknown,
     };
 
     // clang-format off
-    static_assert(static_cast<std::uint8_t>(Category::circle) == 0b0000'0000);
-    static_assert(static_cast<std::uint8_t>(Category::cross ) == 0b0000'0001);
-    static_assert(static_cast<std::uint8_t>(Category::arrow ) == 0b0000'0010);
+    static_assert(static_cast<std::uint8_t>(Category::circle ) == 0b0000'0000);
+    static_assert(static_cast<std::uint8_t>(Category::cross  ) == 0b0000'0001);
+    static_assert(static_cast<std::uint8_t>(Category::arrow  ) == 0b0000'0010);
+    static_assert(static_cast<std::uint8_t>(Category::unknown) == 0b0000'0011);
     // clang-format on
 
     enum Value : std::uint16_t {
       // clang-format off
-      circle      =                      static_cast<std::uint8_t>(Category::circle),
-      cross       =                      static_cast<std::uint8_t>(Category::cross ),
-      left        = (0b0000'1000 << 8) | static_cast<std::uint8_t>(Category::arrow ),
-      down        = (0b0000'0100 << 8) | static_cast<std::uint8_t>(Category::arrow ),
-      up          = (0b0000'0010 << 8) | static_cast<std::uint8_t>(Category::arrow ),
-      right       = (0b0000'0001 << 8) | static_cast<std::uint8_t>(Category::arrow ),
-      lower_left  = (0b0000'1100 << 8) | static_cast<std::uint8_t>(Category::arrow ),
-      upper_left  = (0b0000'1010 << 8) | static_cast<std::uint8_t>(Category::arrow ),
-      lower_right = (0b0000'0101 << 8) | static_cast<std::uint8_t>(Category::arrow ),
-      upper_right = (0b0000'0011 << 8) | static_cast<std::uint8_t>(Category::arrow ),
+      circle      =                      static_cast<std::uint8_t>(Category::circle ),
+      cross       =                      static_cast<std::uint8_t>(Category::cross  ),
+      left        = (0b0000'1000 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      down        = (0b0000'0100 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      up          = (0b0000'0010 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      right       = (0b0000'0001 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      lower_left  = (0b0000'1100 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      upper_left  = (0b0000'1010 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      lower_right = (0b0000'0101 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      upper_right = (0b0000'0011 << 8) | static_cast<std::uint8_t>(Category::arrow  ),
+      unknown     =                      static_cast<std::uint8_t>(Category::unknown),
       // clang-format on
     } value;
 
@@ -166,6 +172,7 @@ struct TrafficLight
     static_assert(static_cast<std::uint16_t>(upper_left ) == 0b0000'1010'0000'0010);
     static_assert(static_cast<std::uint16_t>(lower_right) == 0b0000'0101'0000'0010);
     static_assert(static_cast<std::uint16_t>(upper_right) == 0b0000'0011'0000'0010);
+    static_assert(static_cast<std::uint16_t>(unknown    ) == 0b0000'0000'0000'0011);
     // clang-format on
 
     constexpr Shape(const Value value = circle) : value(value) {}
@@ -183,6 +190,7 @@ struct TrafficLight
       std::make_pair("upperLeft", Shape::upper_left),
       std::make_pair("lowerRight", Shape::lower_right),
       std::make_pair("upperRight", Shape::upper_right),
+      std::make_pair("unknown", Shape::unknown),
 
       // BACKWARD COMPATIBILITY
       std::make_pair("straight", Shape::up),
@@ -262,6 +270,8 @@ struct TrafficLight
             return simulation_api_schema::TrafficLight_Color_RED;
           case Color::white:
             return simulation_api_schema::TrafficLight_Color_WHITE;
+          case Color::unknown:
+            return simulation_api_schema::TrafficLight_Color_UNKNOWN_COLOR;
           default:
             throw common::SyntaxError(std::get<Color>(value), " is not supported color.");
         }
@@ -304,6 +314,8 @@ struct TrafficLight
             return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW;
           case Shape::upper_right:
             return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW;
+          case Shape::unknown:
+            return simulation_api_schema::TrafficLight_Shape_UNKNOWN_SHAPE;
           default:
             throw common::SyntaxError(std::get<Shape>(value), " is not supported as a shape.");
         }