Merge pull request #1734 from tier4/fix/multiple_states_on_phase

Author: HansRobo

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -749,6 +749,20 @@ class SimulatorCore
         std::forward<decltype(xs)>(xs)...);
     }
 
+    template <typename... Ts>
+    static auto clearConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getConventionalTrafficLights()->clearTrafficLightsState(
+        std::forward<decltype(xs)>(xs)...);
+    }
+
+    template <typename... Ts>
+    static auto addConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getConventionalTrafficLights()->addTrafficLightsState(
+        std::forward<decltype(xs)>(xs)...);
+    }
+
     template <typename... Ts>
     static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto)
     {
@@ -782,6 +796,19 @@ class SimulatorCore
       return core->getV2ITrafficLights()->setTrafficLightsState(std::forward<decltype(xs)>(xs)...);
     }
 
+    template <typename... Ts>
+    static auto clearV2ITrafficLightsState(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getV2ITrafficLights()->clearTrafficLightsState(
+        std::forward<decltype(xs)>(xs)...);
+    }
+
+    template <typename... Ts>
+    static auto addV2ITrafficLightsState(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getV2ITrafficLights()->addTrafficLightsState(std::forward<decltype(xs)>(xs)...);
+    }
+
     template <typename... Ts>
     static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto)
     {

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/phase.hpp

@@ -15,11 +15,14 @@
 #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__PHASE_HPP_
 #define OPENSCENARIO_INTERPRETER__SYNTAX__PHASE_HPP_
 
+#include <map>
 #include <openscenario_interpreter/scope.hpp>
+#include <openscenario_interpreter/simulator_core.hpp>
 #include <openscenario_interpreter/syntax/double.hpp>
 #include <openscenario_interpreter/syntax/string.hpp>
 #include <openscenario_interpreter/syntax/traffic_signal_state.hpp>
 #include <pugixml.hpp>
+#include <vector>
 
 namespace openscenario_interpreter
 {
@@ -36,7 +39,7 @@ inline namespace syntax
  *  </xsd:complexType>
  *
  * -------------------------------------------------------------------------- */
-struct Phase
+struct Phase : private SimulatorCore::NonStandardOperation
 {
   // Name of the phase.
   const String name;
@@ -55,6 +58,12 @@ struct Phase
    * ------------------------------------------------------------------------ */
   const std::list<TrafficSignalState> traffic_signal_states;
 
+  // Grouped traffic signal states by their ID and type
+  const std::map<
+    std::pair<lanelet::Id, TrafficSignalState::TrafficSignalType>,
+    std::vector<const TrafficSignalState *>>
+    grouped_states;
+
   explicit Phase(const pugi::xml_node &, Scope &);
 
   auto evaluate() const -> Object;

openscenario/openscenario_interpreter/src/syntax/phase.cpp

@@ -23,14 +23,36 @@ inline namespace syntax
 Phase::Phase(const pugi::xml_node & node, Scope & scope)
 : name(readAttribute<String>("name", node, scope)),
   duration(readAttribute<Double>("duration", node, scope, Double::infinity())),
-  traffic_signal_states(readElements<TrafficSignalState, 0>("TrafficSignalState", node, scope))
+  traffic_signal_states(readElements<TrafficSignalState, 0>("TrafficSignalState", node, scope)),
+  grouped_states([this]() {
+    std::map<
+      std::pair<lanelet::Id, TrafficSignalState::TrafficSignalType>,
+      std::vector<const TrafficSignalState *>>
+      groups;
+    for (const auto & traffic_signal_state : traffic_signal_states) {
+      auto key =
+        std::make_pair(traffic_signal_state.id(), traffic_signal_state.trafficSignalType());
+      groups[key].push_back(&traffic_signal_state);
+    }
+    return groups;
+  }())
 {
 }
 
 auto Phase::evaluate() const -> Object
 {
-  for (auto && traffic_signal_state : traffic_signal_states) {
-    traffic_signal_state.evaluate();
+  for (const auto & [key, states] : grouped_states) {
+    const auto & [id, type] = key;
+    // clear states before adding
+    if (type == TrafficSignalState::TrafficSignalType::conventional) {
+      clearConventionalTrafficLightsState(id);
+    } else if (type == TrafficSignalState::TrafficSignalType::v2i) {
+      clearV2ITrafficLightsState(id);
+    }
+    // then add states
+    for (size_t i = 0; i < states.size(); ++i) {
+      states[i]->evaluate();
+    }
   }
 
   return unspecified;

openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp

@@ -47,10 +47,10 @@ auto TrafficSignalState::evaluate() const -> Object
 {
   switch (trafficSignalType()) {
     case TrafficSignalType::conventional:
-      setConventionalTrafficLightsState(id(), state);
+      addConventionalTrafficLightsState(id(), state);
       break;
     case TrafficSignalType::v2i:
-      setV2ITrafficLightsState(id(), state);
+      addV2ITrafficLightsState(id(), state);
       break;
     default:
       throw Error("Unknown traffic signal type has set to TrafficSignalState");

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

@@ -67,6 +67,10 @@ class TrafficLightsBase
 
   auto setTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> void;
 
+  auto clearTrafficLightsState(const lanelet::Id lanelet_id) -> void;
+
+  auto addTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> void;
+
   auto setTrafficLightsConfidence(const lanelet::Id lanelet_id, const double confidence) -> void;
 
   auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string;

simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp

@@ -67,9 +67,22 @@ auto TrafficLightsBase::setTrafficLightsColor(
 
 auto TrafficLightsBase::setTrafficLightsState(
   const lanelet::Id lanelet_id, const std::string & state) -> void
+{
+  clearTrafficLightsState(lanelet_id);
+  addTrafficLightsState(lanelet_id, state);
+}
+
+auto TrafficLightsBase::clearTrafficLightsState(const lanelet::Id lanelet_id) -> void
 {
   for (const auto & traffic_light : getTrafficLights(lanelet_id)) {
     traffic_light.get().clear();
+  }
+}
+
+auto TrafficLightsBase::addTrafficLightsState(
+  const lanelet::Id lanelet_id, const std::string & state) -> void
+{
+  for (const auto & traffic_light : getTrafficLights(lanelet_id)) {
     traffic_light.get().set(state);
   }
 }

simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal_common.cpp

@@ -526,3 +526,17 @@ TYPED_TEST(TrafficLightsInternalTest, generateAutowarePerceptionTrafficLightGrou
   EXPECT_NEAR(msg.traffic_light_groups[0].elements[1].confidence, expected_confidence, eps);
 }
 #endif  // __has_include(<autoware_perception_msgs/msg/traffic_light_group_array.hpp>)
+
+TYPED_TEST(TrafficLightsInternalTest, addAndClearTrafficLightsState)
+{
+  this->lights->addTrafficLightsState(this->id, "green solidOn circle");
+  this->lights->addTrafficLightsState(this->id, "red solidOn circle");
+
+  const auto state_before_clear = this->lights->getTrafficLightsComposedState(this->id);
+  EXPECT_TRUE(state_before_clear.find("green") != std::string::npos);
+  EXPECT_TRUE(state_before_clear.find("red") != std::string::npos);
+
+  this->lights->clearTrafficLightsState(this->id);
+  const auto state_after_clear = this->lights->getTrafficLightsComposedState(this->id);
+  EXPECT_TRUE(state_after_clear.empty());
+}