Merge pull request #1406 from tier4/RJD-1057-remove-traffic-lights-fr…

…om-entity-manager RJD-1057 (1/5): Remove non-API member functions: EntityManager’s TrafficLight related member functions
tier4 · Nov 8, 2024 · 6b3425c · 6b3425c
2 parents 93b9241 + 409f25f
commit 6b3425c
Show file tree

Hide file tree

Showing 55 changed files with 1,004 additions and 688 deletions.
diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp
@@ -162,7 +162,8 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode
       "obstacle", "ego", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57),
       traffic_simulator::helper::constructActionStatus());
 
-    api_.getConventionalTrafficLight(34802).emplace(traffic_simulator::TrafficLight::Color::green);
+    api_.getConventionalTrafficLights()->setTrafficLightsColor(
+      34802, traffic_simulator::TrafficLight::Color::green);
   }
 
 private:

diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp
@@ -645,40 +645,57 @@ class SimulatorCore
     }
 
     template <typename... Ts>
-    static auto getConventionalTrafficLights(Ts &&... xs) -> decltype(auto)
+    static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto)
     {
-      return core->getConventionalTrafficLights(std::forward<decltype(xs)>(xs)...);
+      return asFieldOperatorApplication(core->getEgoName())
+        .sendCooperateCommand(std::forward<decltype(xs)>(xs)...);
     }
 
+    // TrafficLights - Conventional and V2I
     template <typename... Ts>
-    static auto getV2ITrafficLights(Ts &&... xs) -> decltype(auto)
+    static auto setConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto)
     {
-      return core->getV2ITrafficLights(std::forward<decltype(xs)>(xs)...);
+      return core->getConventionalTrafficLights()->setTrafficLightsState(
+        std::forward<decltype(xs)>(xs)...);
     }
 
     template <typename... Ts>
-    static auto resetConventionalTrafficLightPublishRate(Ts &&... xs) -> decltype(auto)
+    static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto)
     {
-      return core->resetConventionalTrafficLightPublishRate(std::forward<decltype(xs)>(xs)...);
+      return core->getConventionalTrafficLights()->setTrafficLightsConfidence(
+        std::forward<decltype(xs)>(xs)...);
     }
 
     template <typename... Ts>
-    static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto)
+    static auto getConventionalTrafficLightsComposedState(Ts &&... xs) -> decltype(auto)
     {
-      return core->resetV2ITrafficLightPublishRate(std::forward<decltype(xs)>(xs)...);
+      return core->getConventionalTrafficLights()->getTrafficLightsComposedState(
+        std::forward<decltype(xs)>(xs)...);
     }
 
     template <typename... Ts>
-    static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto)
+    static auto compareConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto)
     {
-      return asFieldOperatorApplication(core->getEgoName())
-        .sendCooperateCommand(std::forward<decltype(xs)>(xs)...);
+      return core->getConventionalTrafficLights()->compareTrafficLightsState(
+        std::forward<decltype(xs)>(xs)...);
     }
 
     template <typename... Ts>
-    static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto)
+    static auto resetConventionalTrafficLightPublishRate(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getConventionalTrafficLights()->resetUpdate(std::forward<decltype(xs)>(xs)...);
+    }
+
+    template <typename... Ts>
+    static auto setV2ITrafficLightsState(Ts &&... xs) -> decltype(auto)
+    {
+      return core->getV2ITrafficLights()->setTrafficLightsState(std::forward<decltype(xs)>(xs)...);
+    }
+
+    template <typename... Ts>
+    static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto)
     {
-      return core->setConventionalTrafficLightConfidence(std::forward<decltype(xs)>(xs)...);
+      return core->getV2ITrafficLights()->resetUpdate(std::forward<decltype(xs)>(xs)...);
     }
   };
 };

diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp
@@ -164,11 +164,8 @@ struct ApplyV2ITrafficSignalStateAction : public CustomCommand,
         [[fallthrough]];
 
       case 2:
-        for (auto & traffic_light :
-             getV2ITrafficLights(boost::lexical_cast<std::int64_t>(parameters[0]))) {
-          traffic_light.get().clear();
-          traffic_light.get().set(unquote(parameters.at(1)));
-        }
+        setV2ITrafficLightsState(
+          boost::lexical_cast<std::int64_t>(parameters[0]), unquote(parameters.at(1)));
         break;
 
       default:

diff --git a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_condition.cpp
@@ -38,28 +38,12 @@ auto TrafficSignalCondition::description() const -> String
 
 auto TrafficSignalCondition::evaluate() -> Object
 {
-  if (auto && traffic_lights =
-        getConventionalTrafficLights(boost::lexical_cast<std::int64_t>(name));
-      state == "none") {
+  const auto lanelet_id = boost::lexical_cast<std::int64_t>(name);
+  current_state = getConventionalTrafficLightsComposedState(lanelet_id);
+  if (current_state.empty()) {
     current_state = "none";
-    return asBoolean(std::all_of(
-      std::begin(traffic_lights), std::end(traffic_lights),
-      [](const traffic_simulator::TrafficLight & traffic_light) { return traffic_light.empty(); }));
-  } else {
-    std::stringstream ss;
-    std::string separator = "";
-    for (traffic_simulator::TrafficLight & traffic_light : traffic_lights) {
-      ss << separator << traffic_light;
-      separator = "; ";
-    }
-    current_state = ss.str();
-
-    return asBoolean(std::all_of(
-      std::begin(traffic_lights), std::end(traffic_lights),
-      [this](const traffic_simulator::TrafficLight & traffic_light) {
-        return traffic_light.contains(state);
-      }));
   }
+  return asBoolean(compareConventionalTrafficLightsState(lanelet_id, state));
 }
 }  // namespace syntax
 }  // namespace openscenario_interpreter
diff --git a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp
@@ -28,11 +28,7 @@ TrafficSignalState::TrafficSignalState(const pugi::xml_node & node, Scope & scop
 
 auto TrafficSignalState::evaluate() const -> Object
 {
-  for (traffic_simulator::TrafficLight & traffic_light : getConventionalTrafficLights(id())) {
-    traffic_light.clear();
-    traffic_light.set(state);
-  };
-
+  setConventionalTrafficLightsState(id(), state);
   return unspecified;
 }
 

diff --git a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state_action.cpp b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state_action.cpp
@@ -33,10 +33,7 @@ auto TrafficSignalStateAction::run() noexcept -> void {}
 
 auto TrafficSignalStateAction::start() const -> void
 {
-  for (traffic_simulator::TrafficLight & traffic_light : getConventionalTrafficLights(id())) {
-    traffic_light.clear();
-    traffic_light.set(state);
-  };
+  setConventionalTrafficLightsState(id(), state);
 }
 
 auto TrafficSignalStateAction::id() const -> std::int64_t

diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp
@@ -28,7 +28,7 @@
 #include <traffic_simulator/entity/entity_base.hpp>
 #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator/helper/stop_watch.hpp>
-#include <traffic_simulator/traffic_lights/traffic_light_manager.hpp>
+#include <traffic_simulator/traffic_lights/traffic_lights.hpp>
 #include <traffic_simulator/utils/pose.hpp>
 #include <traffic_simulator_msgs/msg/obstacle.hpp>
 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
@@ -84,7 +84,7 @@ class ActionNode : public BT::ActionNodeBase
       BT::InputPort<std::optional<double>>("target_speed"),
       BT::InputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils"),
       BT::InputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>("canonicalized_entity_status"),
-      BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightManager>>("traffic_light_manager"),
+      BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightsBase>>("traffic_lights"),
       BT::InputPort<traffic_simulator::behavior::Request>("request"),
       BT::OutputPort<std::optional<traffic_simulator_msgs::msg::Obstacle>>("obstacle"),
       BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>("waypoints"),
@@ -112,7 +112,7 @@ class ActionNode : public BT::ActionNodeBase
 protected:
   traffic_simulator::behavior::Request request;
   std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils;
-  std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager;
+  std::shared_ptr<traffic_simulator::TrafficLightsBase> traffic_lights;
   std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> canonicalized_entity_status;
   double current_time;
   double step_time;

diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
@@ -67,7 +67,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
   DEFINE_GETTER_SETTER(RouteLanelets,                                    lanelet::Ids)
   DEFINE_GETTER_SETTER(StepTime,                                         double)
   DEFINE_GETTER_SETTER(TargetSpeed,                                      std::optional<double>)
-  DEFINE_GETTER_SETTER(TrafficLightManager,                              std::shared_ptr<traffic_simulator::TrafficLightManager>)
+  DEFINE_GETTER_SETTER(TrafficLights,                                    std::shared_ptr<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
   DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
   // clang-format on

diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
@@ -71,7 +71,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase
   DEFINE_GETTER_SETTER(RouteLanelets,                                    lanelet::Ids)
   DEFINE_GETTER_SETTER(StepTime,                                         double)
   DEFINE_GETTER_SETTER(TargetSpeed,                                      std::optional<double>)
-  DEFINE_GETTER_SETTER(TrafficLightManager,                              std::shared_ptr<traffic_simulator::TrafficLightManager>)
+  DEFINE_GETTER_SETTER(TrafficLights,                                    std::shared_ptr<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
   DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
   // clang-format on

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -54,9 +54,9 @@ auto ActionNode::getBlackBoardValues() -> void
   if (!getInput<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils", hdmap_utils)) {
     THROW_SIMULATION_ERROR("failed to get input hdmap_utils in ActionNode");
   }
-  if (!getInput<std::shared_ptr<traffic_simulator::TrafficLightManager>>(
-        "traffic_light_manager", traffic_light_manager)) {
-    THROW_SIMULATION_ERROR("failed to get input traffic_light_manager in ActionNode");
+  if (!getInput<std::shared_ptr<traffic_simulator::TrafficLightsBase>>(
+        "traffic_lights", traffic_lights)) {
+    THROW_SIMULATION_ERROR("failed to get input traffic_lights in ActionNode");
   }
   if (!getInput<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>(
         "canonicalized_entity_status", canonicalized_entity_status)) {
@@ -199,28 +199,23 @@ auto ActionNode::getDistanceToTrafficLightStopLine(
   const lanelet::Ids & route_lanelets,
   const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>
 {
-  const auto traffic_light_ids = hdmap_utils->getTrafficLightIdsOnPath(route_lanelets);
-  if (traffic_light_ids.empty()) {
-    return std::nullopt;
-  }
-  std::set<double> collision_points = {};
-  for (const auto id : traffic_light_ids) {
-    using Color = traffic_simulator::TrafficLight::Color;
-    using Status = traffic_simulator::TrafficLight::Status;
-    using Shape = traffic_simulator::TrafficLight::Shape;
-    if (auto && traffic_light = traffic_light_manager->getTrafficLight(id);
-        traffic_light.contains(Color::red, Status::solid_on, Shape::circle) or
-        traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)) {
-      const auto collision_point = hdmap_utils->getDistanceToTrafficLightStopLine(spline, id);
-      if (collision_point) {
-        collision_points.insert(collision_point.value());
+  if (const auto traffic_light_ids = hdmap_utils->getTrafficLightIdsOnPath(route_lanelets);
+      !traffic_light_ids.empty()) {
+    std::set<double> collision_points = {};
+    for (const auto traffic_light_id : traffic_light_ids) {
+      if (traffic_lights->isRequiredStopTrafficLightState(traffic_light_id)) {
+        if (
+          const auto collision_point =
+            hdmap_utils->getDistanceToTrafficLightStopLine(spline, traffic_light_id)) {
+          collision_points.insert(collision_point.value());
+        }
       }
     }
+    if (!collision_points.empty()) {
+      return *collision_points.begin();
+    }
   }
-  if (collision_points.empty()) {
-    return std::nullopt;
-  }
-  return *collision_points.begin();
+  return std::nullopt;
 }
 
 auto ActionNode::getDistanceToStopLine(

diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp
@@ -55,7 +55,7 @@ public:                                         \
   DEFINE_GETTER_SETTER(ReferenceTrajectory,                              std::shared_ptr<math::geometry::CatmullRomSpline>)
   DEFINE_GETTER_SETTER(RouteLanelets,                                    lanelet::Ids)
   DEFINE_GETTER_SETTER(TargetSpeed,                                      std::optional<double>)
-  DEFINE_GETTER_SETTER(TrafficLightManager,                              std::shared_ptr<traffic_simulator::TrafficLightManager>)
+  DEFINE_GETTER_SETTER(TrafficLights,                                    std::shared_ptr<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
   DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
   // clang-format on

diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp
@@ -18,6 +18,8 @@
 #include <geometry/quaternion/slerp.hpp>
 #include <geometry/vector3/hypot.hpp>
 #include <geometry/vector3/operator.hpp>
+#include <geometry_msgs/msg/accel.hpp>
+#include <geometry_msgs/msg/twist.hpp>
 
 namespace entity_behavior
 {

diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt
@@ -37,12 +37,14 @@ ament_auto_add_library(simple_sensor_simulator_component SHARED
   src/sensor_simulation/lidar/lidar_sensor.cpp
   src/sensor_simulation/imu/imu_sensor.cpp
   src/sensor_simulation/lidar/raycaster.cpp
-  src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp
-  src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp
   src/sensor_simulation/occupancy_grid/grid_traversal.cpp
+  src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp
+  src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp
   src/sensor_simulation/primitives/box.cpp
   src/sensor_simulation/primitives/primitive.cpp
+  src/sensor_simulation/primitives/primitive.cpp
   src/sensor_simulation/sensor_simulation.cpp
+  src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp
   src/simple_sensor_simulator.cpp
   src/vehicle_simulation/ego_entity_simulation.cpp
   src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc.cpp

diff --git a/...ple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp b/...ple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp
@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// Copyright 2015 TIER IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

diff --git a/..._sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/..._sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp
@@ -95,24 +95,10 @@ class SensorSimulation
   auto attachPseudoTrafficLightsDetector(
     const double /*current_simulation_time*/,
     const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration,
-    rclcpp::Node & node, std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils) -> void
+    rclcpp::Node & node) -> void
   {
-    if (configuration.architecture_type() == "awf/universe") {
-      using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray;
-      traffic_lights_detectors_.push_back(std::make_unique<traffic_lights::TrafficLightsDetector>(
-        std::make_shared<traffic_simulator::TrafficLightPublisher<Message>>(
-          "/perception/traffic_light_recognition/traffic_signals", &node, hdmap_utils)));
-    } else if (configuration.architecture_type() >= "awf/universe/20230906") {
-      using Message = autoware_perception_msgs::msg::TrafficSignalArray;
-      traffic_lights_detectors_.push_back(std::make_unique<traffic_lights::TrafficLightsDetector>(
-        std::make_shared<traffic_simulator::TrafficLightPublisher<Message>>(
-          "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils)));
-    } else {
-      std::stringstream ss;
-      ss << "Unexpected architecture_type " << std::quoted(configuration.architecture_type())
-         << " given.";
-      throw std::runtime_error(ss.str());
-    }
+    traffic_lights_detectors_.push_back(std::make_unique<traffic_lights::TrafficLightsDetector>(
+      node, configuration.architecture_type()));
   }
 
   auto attachImuSensor(

diff --git a/...lude/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp b/...lude/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp
@@ -16,10 +16,9 @@
 #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp>
 #include <simulation_interface/conversions.hpp>
 #include <string>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
-#include <traffic_simulator/traffic_lights/traffic_light_publisher.hpp>
 
 namespace simple_sensor_simulator
 {
@@ -32,21 +31,49 @@ namespace traffic_lights
  */
 class TrafficLightsDetector
 {
-  const std::shared_ptr<traffic_simulator::TrafficLightPublisherBase> publisher_;
-
 public:
-  explicit TrafficLightsDetector(
-    const std::shared_ptr<traffic_simulator::TrafficLightPublisherBase> & publisher)
-  : publisher_(publisher)
+  template <typename NodeType>
+  explicit TrafficLightsDetector(NodeType & node, const std::string & architecture_type)
+  : publisher_ptr_(makePublisher(node, architecture_type))
   {
   }
 
   auto updateFrame(
     const rclcpp::Time & current_ros_time,
     const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void
   {
-    publisher_->publish(current_ros_time, request);
+    publisher_ptr_->publish(current_ros_time, request);
+  }
+
+private:
+  template <typename NodeType>
+  auto makePublisher(NodeType & node, const std::string & architecture_type)
+    -> std::unique_ptr<TrafficLightsPublisherBase>
+  {
+    /*
+       TrafficLightsDetector in SimpleSensorSimulator publishes using architecture-dependent topics:
+       "/perception/traffic_light_recognition/internal/traffic_signals" for >= "awf/universe/20230906"
+       "/perception/traffic_light_recognition/traffic_signals" for "awf/universe"
+
+       V2ITrafficLights in TrafficSimulator publishes publishes using architecture-independent topics ("awf/universe..."): 
+       "/v2x/traffic_signals" and "/perception/traffic_light_recognition/external/traffic_signals"
+    */
+    if (architecture_type == "awf/universe") {
+      using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray;
+      return std::make_unique<TrafficLightsPublisher<Message>>(
+        &node, "/perception/traffic_light_recognition/traffic_signals");
+    } else if (architecture_type >= "awf/universe/20230906") {
+      using Message = autoware_perception_msgs::msg::TrafficSignalArray;
+      return std::make_unique<TrafficLightsPublisher<Message>>(
+        &node, "/perception/traffic_light_recognition/internal/traffic_signals");
+    } else {
+      std::stringstream ss;
+      ss << "Unexpected architecture_type " << std::quoted(architecture_type) << " given.";
+      throw std::invalid_argument(ss.str());
+    }
   }
+
+  const std::unique_ptr<TrafficLightsPublisherBase> publisher_ptr_;
 };
 }  // namespace traffic_lights
 }  // namespace simple_sensor_simulator