Merge pull request #1357 from tier4/feature/traffic_light_group

mock/cpp_mock_scenarios/launch/mock_test.launch.py

@@ -70,7 +70,7 @@ def on_stdout_output(event: launch.Event) -> None:
             print(Color.GREEN + "Scenario Succeed" + Color.END)
 
 def architecture_types():
-    return ["awf/universe", "awf/universe/20230906"]
+    return ["awf/universe", "awf/universe/20230906", "awf/universe/20240605"]
 
 
 def default_autoware_launch_package_of(architecture_type):
@@ -81,6 +81,7 @@ def default_autoware_launch_package_of(architecture_type):
     return {
         "awf/universe": "autoware_launch",
         "awf/universe/20230906": "autoware_launch",
+        "awf/universe/20240605": "autoware_launch",
     }[architecture_type]
 
 
@@ -92,6 +93,7 @@ def default_autoware_launch_file_of(architecture_type):
     return {
         "awf/universe": "planning_simulator.launch.xml",
         "awf/universe/20230906": "planning_simulator.launch.xml",
+        "awf/universe/20240605": "planning_simulator.launch.xml",
     }[architecture_type]
 
 def default_rviz_config_file():

simulation/simple_sensor_simulator/CMakeLists.txt

@@ -44,7 +44,6 @@ ament_auto_add_library(simple_sensor_simulator_component SHARED
   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

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

@@ -19,8 +19,6 @@
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
-#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
-#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <iomanip>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>

simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp

@@ -15,10 +15,16 @@
 #ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_
 #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_
 
+#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.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/traffic_lights/traffic_light_publisher.hpp>
+
+#if __has_include(<autoware_perception_msgs/msg/traffic_light_group_array.hpp>)
+#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
+#endif
 
 namespace simple_sensor_simulator
 {
@@ -48,32 +54,40 @@ class TrafficLightsDetector
 private:
   template <typename NodeType>
   auto makePublisher(NodeType & node, const std::string & architecture_type)
-    -> std::unique_ptr<TrafficLightsPublisherBase>
+    -> std::unique_ptr<traffic_simulator::TrafficLightPublisherBase>
   {
     /*
        TrafficLightsDetector in SimpleSensorSimulator publishes using architecture-dependent topics:
-       "/perception/traffic_light_recognition/internal/traffic_signals" for >= "awf/universe/20230906"
+       "/perception/traffic_light_recognition/internal/traffic_signals" for >= "awf/universe/20240605"
+       "/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>>(
+      return std::make_unique<traffic_simulator::TrafficLightPublisher<Message>>(
         &node, "/perception/traffic_light_recognition/traffic_signals");
-    } else if (architecture_type >= "awf/universe/20230906") {
+    } else if (architecture_type == "awf/universe/20230906") {
       using Message = autoware_perception_msgs::msg::TrafficSignalArray;
-      return std::make_unique<TrafficLightsPublisher<Message>>(
+      return std::make_unique<traffic_simulator::TrafficLightPublisher<Message>>(
+        &node, "/perception/traffic_light_recognition/internal/traffic_signals");
+#if __has_include(<autoware_perception_msgs/msg/traffic_light_group_array.hpp>)
+    } else if (architecture_type >= "awf/universe/20240605") {
+      using Message = autoware_perception_msgs::msg::TrafficLightGroupArray;
+      return std::make_unique<traffic_simulator::TrafficLightPublisher<Message>>(
         &node, "/perception/traffic_light_recognition/internal/traffic_signals");
+#endif
     } else {
       std::stringstream ss;
-      ss << "Unexpected architecture_type " << std::quoted(architecture_type) << " given.";
+      ss << "Unexpected architecture_type " << std::quoted(architecture_type)
+         << " given for traffic light.";
       throw std::invalid_argument(ss.str());
     }
   }
 
-  const std::unique_ptr<TrafficLightsPublisherBase> publisher_ptr_;
+  const std::unique_ptr<traffic_simulator::TrafficLightPublisherBase> publisher_ptr_;
 };
 }  // namespace traffic_lights
 }  // namespace simple_sensor_simulator

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

@@ -19,7 +19,6 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_broadcaster.h>
 
-#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <memory>
 #include <optional>
 #include <rclcpp/node_interfaces/get_node_topics_interface.hpp>
@@ -48,7 +47,6 @@
 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
 #include <traffic_simulator_msgs/msg/entity_status_with_trajectory_array.hpp>
-#include <traffic_simulator_msgs/msg/traffic_light_array_v1.hpp>
 #include <traffic_simulator_msgs/msg/vehicle_parameters.hpp>
 #include <type_traits>
 #include <unordered_map>

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

@@ -27,7 +27,9 @@ namespace traffic_simulator
 class TrafficLightPublisherBase
 {
 public:
-  virtual auto publish(const TrafficLightsBase & traffic_lights) const -> void = 0;
+  virtual auto publish(
+    const rclcpp::Time & current_ros_time,
+    const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void = 0;
   virtual ~TrafficLightPublisherBase() = default;
 };
 
@@ -38,22 +40,23 @@ class TrafficLightPublisher : public TrafficLightPublisherBase
   template <typename NodeTypePointer>
   explicit TrafficLightPublisher(
     const NodeTypePointer & node_ptr, const std::string & topic_name,
-    const std::string & frame = "camera_link")
+    const std::string & frame = "camera_link")  // DIRTY HACK!!!
   : TrafficLightPublisherBase(),
     frame_(frame),
-    clock_ptr_(node_ptr->get_clock()),
     traffic_light_state_array_publisher_(rclcpp::create_publisher<MessageType>(
       node_ptr, topic_name, rclcpp::QoS(10).transient_local()))
   {
   }
 
   ~TrafficLightPublisher() override = default;
 
-  auto publish(const TrafficLightsBase & traffic_lights) const -> void override;
+  auto publish(
+    const rclcpp::Time & current_ros_time,
+    const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void override;
 
 private:
   const std::string frame_;
-  const rclcpp::Clock::SharedPtr clock_ptr_;
+
   const typename rclcpp::Publisher<MessageType>::SharedPtr traffic_light_state_array_publisher_;
 };
 }  // namespace traffic_simulator

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

@@ -17,6 +17,11 @@
 
 #include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
+
+#if __has_include(<autoware_perception_msgs/msg/traffic_light_group_array.hpp>)
+#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
+#endif
+
 #include <traffic_simulator/traffic_lights/traffic_light_publisher.hpp>
 #include <traffic_simulator/traffic_lights/traffic_lights_base.hpp>
 
@@ -40,7 +45,8 @@ class ConventionalTrafficLights : public TrafficLightsBase
 private:
   auto update() const -> void
   {
-    backward_compatible_publisher_ptr_->publish(*this);
+    backward_compatible_publisher_ptr_->publish(
+      clock_ptr_->now(), generateUpdateTrafficLightsRequest());
     if (isAnyTrafficLightChanged()) {
       marker_publisher_ptr_->deleteMarkers();
     }
@@ -70,8 +76,10 @@ class V2ITrafficLights : public TrafficLightsBase
 private:
   auto update() const -> void override
   {
-    publisher_ptr_->publish(*this);
-    legacy_topic_publisher_ptr_->publish(*this);
+    const auto now = clock_ptr_->now();
+    const auto request = generateUpdateTrafficLightsRequest();
+    publisher_ptr_->publish(now, request);
+    legacy_topic_publisher_ptr_->publish(now, request);
     if (isAnyTrafficLightChanged()) {
       marker_publisher_ptr_->deleteMarkers();
     }
@@ -91,10 +99,16 @@ class V2ITrafficLights : public TrafficLightsBase
        "/perception/traffic_light_recognition/internal/traffic_signals" for >= "awf/universe/20230906"
        "/perception/traffic_light_recognition/traffic_signals" for "awf/universe"
     */
-    if (architecture_type.find("awf/universe") != std::string::npos) {
+    if (architecture_type <= "awf/universe/20230906") {
       return std::make_unique<
         TrafficLightPublisher<autoware_perception_msgs::msg::TrafficSignalArray>>(
         node_ptr, topic_name);
+#if __has_include(<autoware_perception_msgs/msg/traffic_light_group_array.hpp>)
+    } else if (architecture_type >= "awf/universe/20240605") {
+      return std::make_unique<
+        TrafficLightPublisher<autoware_perception_msgs::msg::TrafficLightGroupArray>>(
+        node_ptr, topic_name);
+#endif
     } else {
       throw common::SemanticError(
         "Unexpected architecture_type ", std::quoted(architecture_type),

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

@@ -24,7 +24,6 @@
 #include <traffic_simulator/traffic_lights/configurable_rate_updater.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp>
-#include <traffic_simulator_msgs/msg/traffic_light_array_v1.hpp>
 #include <unordered_map>
 #include <vector>
 
@@ -75,8 +74,6 @@ class TrafficLightsBase
   auto generateUpdateTrafficLightsRequest() const
     -> simulation_api_schema::UpdateTrafficLightsRequest;
 
-  auto generateTrafficSimulatorV1Msg() const -> traffic_simulator_msgs::msg::TrafficLightArrayV1;
-
 protected:
   virtual auto update() const -> void = 0;