Add PseudoTrafficSignalDetectorConfidenceSetAction@v1

docs/ReleaseNotes.md

@@ -9,6 +9,8 @@ Major Changes :race_car: :red_car: :blue_car:
 | Traffic Light Simulation                                 | Start supporting to traffic light messages in `autoware_perception_msgs`.                                                                                                                                   | `openscenario_interpreter`, `traffic_simulator`, `simple_sensor_simulator` | [#1027](https://github.com/tier4/scenario_simulator_v2/pull/1027) | [HansRobo](https://github.com/HansRobo)       |
 | Detailed control of permissions for autonomous decisions | Add `featureIdentifiersRequiringExternalPermissionForAutonomousDecisions` property for ObjectController. scenario_simulator_v2 controls auto_mode of Autoware planning modules by settings in the property. | `openscenario_interpreter`, `concealer`                                    | [#1092](https://github.com/tier4/scenario_simulator_v2/pull/1092) | [HansRobo](https://github.com/HansRobo)       |
 | OpenSCENARIO `Controller.Properties.Property`            | Support the property `allowGoalModification` to turn on the Autoware feature `allow_goal_modification`.                                                                                                     | `openscenario_interpreter`, `concealer`                                    | [#997](https://github.com/tier4/scenario_simulator_v2/pull/997)   | [yamacir-kit](https://github.com/yamacir-kit) |
+| OpenSCENARIO 1.2 `UserDefinedAction.CustomCommandAction` | Add `PseudoTrafficSignalDetectorConfidenceSetAction@v1`, a CustomCommandAction to control confidences in traffic light topic.                                                                               | `openscenario_interpreter`, `traffic_simulator`                            | [#1111](https://github.com/tier4/scenario_simulator_v2/pull/1111) | [HansRobo](https://github.com/HansRobo)       |
+
 
 Bug Fixes:bug:
 

docs/developer_guide/OpenSCENARIOSupport.md

@@ -126,6 +126,7 @@ Here, the colon (`:`) specified in the `CustomCommandAction.type` is the `sh` co
 | <pre>UserDefinedAction:<br/>  CustomCommandAction:<br/>    type: FaultInjectionAction(<EVENT-NAME\>, ...)</pre>                                                                                       | Same as FaultInjectionAction@v1.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          |
 | <pre>UserDefinedAction:<br/>  CustomCommandAction:<br/>    type: FaultInjectionAction@v1(<EVENT-NAME\>, ...)</pre>                                                                                    | Forward any number of event names to Autoware as `ERROR` level event. Events are forwarded by publishing to the `tier4_simulation_msgs::msg::SimulationEvents` type topic `/simulation/events`. In order to perform fault injection using this CustomCommandAction, Autoware must have a node that receives the above message types. Note that the simulator has no knowledge of the contents of the event name. In other words, what happens to Autoware by this CustomCommandAction depends on Autoware implementation. |
 | <pre>UserDefinedAction:<br/>  CustomCommandAction:<br/>    type: FaultInjectionAction@v2(<ERROR-LEVEL\>, <EVENT-NAME\>)</pre>                                                                         | Forwards a single event to Autoware with the specified error level. Same as `FaultInjectionAction@v1` except that instead of specifying an error level, only one event can be specified at a time. Available error levels are `OK`, `WARN`, `ERROR` and `STALE`.                                                                                                                                                                                                                                                          |
+| <pre>UserDefinedAction:<br/>  CustomCommandAction:<br/>    type: PseudoTrafficSignalDetectorConfidenceSetAction@v1(<LANELET-ID\>, <CONFIDENCE\>)</pre>                                                | Set a confidence value for traffic light topic. This action sets the confidence value to all traffic light bulbs of specified traffic light. If you specify the traffic light by a regulatory element ID, this action sets the confidence value to all traffic lights the regulatory element refers to.                                                                                                                                                                                                                   |
 | <pre>UserDefinedAction:<br/>  CustomCommandAction:<br/>    type: RequestToCooperateCommandAction@v1(<MODULE-NAME\>, <COMMAND\>)</pre>                                                                 | Send an `ACTIVATE` / `DEACTIVATE` command to the module publishing a valid request to cooperate. If the send fails, throw an exception to fail the scenario.                                                                                                                                                                                                                                                                                                                                                              |
 | <pre>UserDefinedAction:<br/>  CustomCommandAction:<br/>    type: V2ITrafficSignalStateAction(<LANELET-ID\>, <STATE\>,    \<br/>                                      <PUBLISH-RATE(optional)\>)</pre> | TrafficSignalStateAction for V2I traffic signal. You can optionally specify the publish rate of the traffic signal topic, but otherwise the functionality is the same as `TrafficSignalStateAction`.                                                                                                                                                                                                                                                                                                                      |
 | <pre>UserDefinedAction:<br/>  CustomCommandAction:<br/>    type: WalkStraightAction(<ENTITY-REF\>, ...)</pre>                                                                                         | Make **pedestrian** entities walk straight without a target.                                                                                                                                                                                                                                                                                                                                                                                                                                                              |

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -571,6 +571,12 @@ class SimulatorCore
       return asFieldOperatorApplication(core->getEgoName())
         .sendCooperateCommand(std::forward<decltype(xs)>(xs)...);
     }
+
+    template <typename... Ts>
+    static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto)
+    {
+      return core->setConventionalTrafficLightConfidence(std::forward<decltype(xs)>(xs)...);
+    }
   };
 };
 }  // namespace openscenario_interpreter

openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp

@@ -92,6 +92,29 @@ struct ApplyFaultInjectionAction : public CustomCommand
   }
 };
 
+template <auto Version>
+struct ApplyPseudoTrafficSignalDetectorConfidenceSetAction
+: public CustomCommand,
+  public SimulatorCore::NonStandardOperation
+{
+  using CustomCommand::CustomCommand;
+
+  auto start(const Scope &) -> void override
+  {
+    static_assert(0 < Version and Version <= 1);
+    if (parameters.size() == 2) {
+      setConventionalTrafficLightConfidence(
+        std::stoi(parameters.at(0)), std::stod(parameters.at(1)));
+    } else {
+      throw Error(
+        "An unexpected number of arguments were passed to "
+        "PseudoTrafficSignalDetectorConfidenceSetAction. "
+        "Expected 2 arguments, but actually passed ",
+        parameters.size(), ".");
+    }
+  }
+};
+
 template <auto Version>
 struct ApplyRequestToCorporateCommandAction : public CustomCommand,
                                               public SimulatorCore::NonStandardOperation
@@ -101,7 +124,14 @@ struct ApplyRequestToCorporateCommandAction : public CustomCommand,
   auto start(const Scope &) -> void override
   {
     static_assert(0 < Version and Version <= 1);
-    sendCooperateCommand(parameters.at(0), parameters.at(1));
+    if (parameters.size() == 2) {
+      sendCooperateCommand(parameters.at(0), parameters.at(1));
+    } else {
+      throw Error(
+        "An unexpected number of arguments were passed to RequestToCooperateCommandAction. "
+        "Expected 2 arguments, but actually passed ",
+        parameters.size(), ".");
+    }
   }
 };
 
@@ -270,9 +300,11 @@ auto makeCustomCommand(const std::string & type, const std::string & content)
   static const std::unordered_map<
     std::string, std::function<std::shared_ptr<CustomCommand>(const std::vector<std::string> &)>>
     commands{
+      // clang-format off
       ELEMENT("FaultInjectionAction", ApplyFaultInjectionAction<1>),
       ELEMENT("FaultInjectionAction@v1", ApplyFaultInjectionAction<1>),
       ELEMENT("FaultInjectionAction@v2", ApplyFaultInjectionAction<2>),
+      ELEMENT("PseudoTrafficSignalDetectorConfidenceSetAction@v1", ApplyPseudoTrafficSignalDetectorConfidenceSetAction<1>),
       ELEMENT("RequestToCooperateCommandAction@v1", ApplyRequestToCorporateCommandAction<1>),
       ELEMENT("V2ITrafficSignalStateAction", ApplyV2ITrafficSignalStateAction),
       ELEMENT("WalkStraightAction", ApplyWalkStraightAction),
@@ -283,6 +315,7 @@ auto makeCustomCommand(const std::string & type, const std::string & content)
       ELEMENT("exitSuccess", ExitSuccess),
       ELEMENT("printParameter", PrintParameter),
       ELEMENT("test", TestCommand),
+      // clang-format on
     };
 #undef ELEMENT
 

simulation/behavior_tree_plugin/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8)
 project(behavior_tree_plugin)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -g -DBOOST_ALLOW_DEPRECATED_HEADERS)
 endif()
 
 find_package(ament_cmake_auto REQUIRED)

simulation/do_nothing_plugin/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8)
 project(do_nothing_plugin)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -g -DBOOST_ALLOW_DEPRECATED_HEADERS)
 endif()
 
 # find dependencies

simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp

@@ -92,7 +92,7 @@ class SensorSimulation
   }
 
   auto attachPseudoTrafficLightsDetector(
-    const double current_simulation_time,
+    const double,
     const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration,
     rclcpp::Node & node, std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils) -> void
   {

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

@@ -328,6 +328,7 @@ class API
   FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit);
   FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit);
   FORWARD_TO_ENTITY_MANAGER(setLinearVelocity);
+  FORWARD_TO_ENTITY_MANAGER(setConventionalTrafficLightConfidence);
   FORWARD_TO_ENTITY_MANAGER(setVelocityLimit);
   FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate);

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

@@ -231,6 +231,13 @@ class EntityManager
     v2i_traffic_light_updater_.resetUpdateRate(rate);
   }
 
+  auto setConventionalTrafficLightConfidence(lanelet::Id id, double confidence) -> void
+  {
+    for (auto & traffic_light : conventional_traffic_light_manager_ptr_->getTrafficLights(id)) {
+      traffic_light.get().setConfidence(confidence);
+    }
+  }
+
 #define FORWARD_TO_HDMAP_UTILS(NAME)                                  \
   template <typename... Ts>                                           \
   decltype(auto) NAME(Ts &&... xs) const                              \

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

@@ -313,14 +313,17 @@ struct TrafficLight
       traffic_light_bulb_proto.set_status(status());
       traffic_light_bulb_proto.set_shape(shape());
       traffic_light_bulb_proto.set_color(color());
-      traffic_light_bulb_proto.set_confidence(1.0);
+      // NOTE: confidence will be overwritten in TrafficLight::operator() for simulation_api_schema::TrafficLight
+      //      traffic_light_bulb_proto.set_confidence(1.0);
 
       return traffic_light_bulb_proto;
     }
   };
 
   const lanelet::Id way_id;
 
+  double confidence = 1.0;
+
   std::set<Bulb> bulbs;
 
   const std::map<Bulb::Hash, std::optional<geometry_msgs::msg::Point>> positions;
@@ -380,6 +383,8 @@ struct TrafficLight
 
   auto set(const std::string & states) -> void;
 
+  auto setConfidence(const float confidence) -> void { this->confidence = confidence; }
+
   friend auto operator<<(std::ostream & os, const TrafficLight & traffic_light) -> std::ostream &;
 
   explicit operator simulation_api_schema::TrafficSignal() const
@@ -388,8 +393,9 @@ struct TrafficLight
 
     traffic_signal_proto.set_id(way_id);
     for (const auto & bulb : bulbs) {
-      *traffic_signal_proto.add_traffic_light_status() =
-        static_cast<simulation_api_schema::TrafficLight>(bulb);
+      auto traffic_light_bulb_proto = static_cast<simulation_api_schema::TrafficLight>(bulb);
+      traffic_light_bulb_proto.set_confidence(confidence);
+      *traffic_signal_proto.add_traffic_light_status() = traffic_light_bulb_proto;
     }
     return traffic_signal_proto;
   }

test_runner/random_test_runner/CMakeLists.txt

@@ -28,7 +28,7 @@ if(NOT CMAKE_CXX_STANDARD)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+  add_compile_options(-Wall -Wextra -Wpedantic -g -DBOOST_ALLOW_DEPRECATED_HEADERS)
 endif()
 
 # find dependencies