From 3be60cbef354682d2e00dfd992ef86c30b27b9ef Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 13:53:03 +0900 Subject: [PATCH 01/16] feat(traffic_simulator): support TrafficLightGroupArray in TrafficLightPublisher --- .../traffic_light_publisher.cpp | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index e5a0689e24c..99cace47be3 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -14,6 +14,11 @@ #include #include + +#if __has_include() +#include +#endif + #include #include @@ -96,4 +101,39 @@ auto TrafficLightPublisher::pu } traffic_light_state_array_publisher_->publish(message); } + +#if __has_include() + template <> +auto TrafficLightPublisher::publish( + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void +{ + assert(hdmap_utils_ != nullptr); + + autoware_perception_msgs::msg::TrafficLightGroupArray message; + message.stamp = current_ros_time; + for (const auto & traffic_light : request.states()) { + auto relation_ids = + hdmap_utils_->getTrafficLightRegulatoryElementIDsFromTrafficLight(traffic_light.id()); + + for (auto relation_id : relation_ids) { + // skip if the traffic light has no bulbs + if (not traffic_light.traffic_light_status().empty()) { + using TrafficLightGroupType = autoware_perception_msgs::msg::TrafficLightGroup; + TrafficLightGroupType traffic_light_group_message; + traffic_light_group_message.traffic_light_group_id = relation_id; + + for (auto bulb_status : traffic_light.traffic_light_status()) { + using TrafficLightBulbType = autoware_perception_msgs::msg::TrafficLightElement; + TrafficLightBulbType light_bulb_message; + simulation_interface::toMsg(bulb_status, light_bulb_message); + traffic_light_group_message.elements.push_back(light_bulb_message); + } + message.traffic_light_groups.push_back(traffic_light_group_message); + } + } + } + traffic_light_state_array_publisher_->publish(message); +} +#endif } // namespace traffic_simulator From d423431a10fa9979698966cca745b261f91e223d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 13:59:23 +0900 Subject: [PATCH 02/16] feat(traffic_simulator): support TrafficLightGroupArray in SimpleSensorSimulator --- .../sensor_simulation/sensor_simulation.hpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 20f274f3276..47e5f93c316 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -21,6 +21,11 @@ #include #include #include + +#if __has_include() +#include +#endif + #include #include #include @@ -107,10 +112,17 @@ class SensorSimulation traffic_lights_detectors_.push_back(std::make_unique( std::make_shared>( "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); +#if __has_include() + } else if(configuration.architecture_type() == "awf/universe/20240605"){ + using Message = autoware_perception_msgs::msg::TrafficLightGroupArray; + traffic_lights_detectors_.push_back(std::make_unique( + std::make_shared>( + "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); +#endif } else { std::stringstream ss; ss << "Unexpected architecture_type " << std::quoted(configuration.architecture_type()) - << " given."; + << " given for traffic light."; throw std::runtime_error(ss.str()); } } From e53c59736746bf8c83fc4d73fa1df52bdd2f797e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 14:57:01 +0900 Subject: [PATCH 03/16] feat(traffic_simulator): support TrafficLightGroup for V2I traffic lights --- .../traffic_simulator/entity/entity_manager.hpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index b90788d5437..22272ab244c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -20,6 +20,11 @@ #include #include + +#if __has_include() +#include +#endif + #include #include #include @@ -137,10 +142,16 @@ class EntityManager { if (const auto architecture_type = getParameter(node_parameters_, "architecture_type", "awf/universe"); - architecture_type.find("awf/universe") != std::string::npos) { + architecture_type <= "awf/universe/20230906") { return std::make_shared< TrafficLightPublisher>( std::forward(xs)...); +#if __has_include() + } else if(architecture_type == "awf/universe/20240605"){ + return std::make_shared< + TrafficLightPublisher>( + std::forward(xs)...); +#endif } else { throw common::SemanticError( "Unexpected architecture_type ", std::quoted(architecture_type), From 4851ef4a66db4d65f7c080eb16840be9f668e6e6 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 14:58:00 +0900 Subject: [PATCH 04/16] chore(scenario_test_runner): add new architecture_type in launch file --- .../launch/scenario_test_runner.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 4d0b80e55f3..3fdaf972b4c 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -33,7 +33,7 @@ def architecture_types(): - return ["awf/universe", "awf/universe/20230906"] + return ["awf/universe, ""awf/universe/20230906", "awf/universe/2024605"] def default_autoware_launch_package_of(architecture_type): @@ -44,6 +44,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] @@ -55,6 +56,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/2024605": "planning_simulator.launch.xml", }[architecture_type] From a42eb2524255b36f6fd83719b57c9eff24a7f3be Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Aug 2024 19:27:37 +0900 Subject: [PATCH 05/16] chore: apply formatter --- .../sensor_simulation/sensor_simulation.hpp | 6 +++--- .../include/traffic_simulator/entity/entity_manager.hpp | 2 +- .../src/traffic_lights/traffic_light_publisher.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 47e5f93c316..5f60d1c54b1 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -113,11 +113,11 @@ class SensorSimulation std::make_shared>( "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); #if __has_include() - } else if(configuration.architecture_type() == "awf/universe/20240605"){ + } else if (configuration.architecture_type() == "awf/universe/20240605") { using Message = autoware_perception_msgs::msg::TrafficLightGroupArray; traffic_lights_detectors_.push_back(std::make_unique( - std::make_shared>( - "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); + std::make_shared>( + "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); #endif } else { std::stringstream ss; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 22272ab244c..3ac9fe84f14 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -147,7 +147,7 @@ class EntityManager TrafficLightPublisher>( std::forward(xs)...); #if __has_include() - } else if(architecture_type == "awf/universe/20240605"){ + } else if (architecture_type == "awf/universe/20240605") { return std::make_shared< TrafficLightPublisher>( std::forward(xs)...); diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index 99cace47be3..105e9eaa12c 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -103,7 +103,7 @@ auto TrafficLightPublisher::pu } #if __has_include() - template <> +template <> auto TrafficLightPublisher::publish( const rclcpp::Time & current_ros_time, const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void From 4b2c29d41f6faa99b3d10b98213fd506ed8d0d4c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Aug 2024 21:35:11 +0900 Subject: [PATCH 06/16] fix architecture_type and typos --- .../launch/scenario_test_runner.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 3fdaf972b4c..29d76478f2a 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -33,7 +33,7 @@ def architecture_types(): - return ["awf/universe, ""awf/universe/20230906", "awf/universe/2024605"] + return ["awf/universe", "awf/universe/20230906", "awf/universe/20240605"] def default_autoware_launch_package_of(architecture_type): @@ -56,7 +56,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/2024605": "planning_simulator.launch.xml", + "awf/universe/20240605": "planning_simulator.launch.xml", }[architecture_type] From 1f1dcb6afa1d279d152b6a1732a715c222b6f1de Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 5 Nov 2024 11:36:26 +0900 Subject: [PATCH 07/16] docs: add comment about archtecture_type purposes --- .../scenario_test_runner/launch/scenario_test_runner.launch.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 6b2f9fb9aaf..eaf5a2ab722 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -33,6 +33,9 @@ def architecture_types(): + # awf/universe: autoware_auto_perception_msgs/TrafficSignalArray for traffic lights + # awf/universe/20230906: autoware_perception_msgs/TrafficSignalArray for traffic lights + # awf/universe/20240605: autoware_perception_msgs/TrafficLightGroupArray for traffic lights return ["awf/universe", "awf/universe/20230906", "awf/universe/20240605"] From ae7104f08ced886fa01a84d3246ce90ad6b47a9b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 14:37:04 +0900 Subject: [PATCH 08/16] refactor: modify TrafficLightPublisher to publish from proto --- .../traffic_light_publisher.hpp | 10 +++-- .../traffic_lights/traffic_lights.hpp | 9 ++-- .../traffic_lights/traffic_lights_base.hpp | 3 -- .../traffic_light_publisher.cpp | 43 +++++++++++++------ .../traffic_lights/traffic_lights_base.cpp | 11 ----- 5 files changed, 42 insertions(+), 34 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp index 618dda3a59a..7f299f0b76f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp +++ b/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,7 +40,7 @@ class TrafficLightPublisher : public TrafficLightPublisherBase template 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()), @@ -49,7 +51,9 @@ class TrafficLightPublisher : public TrafficLightPublisherBase ~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_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp index e2ae7cbd4b7..eaa4b95302a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp @@ -40,7 +40,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 +71,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(); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp index 8220cf8d6a5..539232841e2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include @@ -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; diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index ae11d459c4e..c6785abce94 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #if __has_include() #include @@ -26,17 +27,17 @@ namespace traffic_simulator { template <> auto TrafficLightPublisher::publish( - const TrafficLightsBase & traffic_lights) const -> void + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void { - const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest(); autoware_auto_perception_msgs::msg::TrafficSignalArray message; - message.header.frame_id = "camera_link"; // DIRTY HACK!!! - message.header.stamp = clock_ptr_->now(); + message.header.frame_id = frame_; + message.header.stamp = current_ros_time; using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; - for (const auto & traffic_light : states_as_proto_request.states()) { + for (const auto & traffic_light : request.states()) { TrafficLightType traffic_light_message; traffic_light_message.map_primitive_id = traffic_light.id(); for (const auto & bulb_status : traffic_light.traffic_light_status()) { @@ -51,17 +52,17 @@ auto TrafficLightPublisher auto TrafficLightPublisher::publish( - const TrafficLightsBase & traffic_lights) const -> void + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void { - const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest(); autoware_perception_msgs::msg::TrafficSignalArray message; - message.stamp = clock_ptr_->now(); + message.stamp = current_ros_time; using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; using TrafficLightBulbType = autoware_perception_msgs::msg::TrafficSignal::_elements_type::value_type; - for (const auto & traffic_light : states_as_proto_request.states()) { + for (const auto & traffic_light : request.states()) { for (const auto & relation_id : traffic_light.relation_ids()) { // skip if the traffic light has no bulbs if (not traffic_light.traffic_light_status().empty()) { @@ -81,9 +82,24 @@ auto TrafficLightPublisher::p template <> auto TrafficLightPublisher::publish( - const TrafficLightsBase & traffic_lights) const -> void + [[maybe_unused]] const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void { - traffic_light_state_array_publisher_->publish(traffic_lights.generateTrafficSimulatorV1Msg()); + traffic_simulator_msgs::msg::TrafficLightArrayV1 message; + + using TrafficLightType = traffic_simulator_msgs::msg::TrafficLightV1; + using TrafficLightBulbType = traffic_simulator_msgs::msg::TrafficLightBulbV1; + for (const auto & traffic_light : request.states()) { + TrafficLightType traffic_light_message; + traffic_light_message.lanelet_way_id = traffic_light.id(); + for (const auto & bulb_status : traffic_light.traffic_light_status()) { + TrafficLightBulbType light_bulb_message; + simulation_interface::toMsg(bulb_status, light_bulb_message); + traffic_light_message.traffic_light_bulbs.push_back(light_bulb_message); + } + message.traffic_lights.push_back(traffic_light_message); + } + traffic_light_state_array_publisher_->publish(message); } #if __has_include() @@ -92,14 +108,13 @@ auto TrafficLightPublisher void { - const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest(); autoware_perception_msgs::msg::TrafficLightGroupArray message; - message.stamp = clock_ptr_->now(); + message.stamp = current_ros_time; using TrafficLightGroupType = autoware_perception_msgs::msg::TrafficLightGroup; using TrafficLightBulbType = autoware_perception_msgs::msg::TrafficLightElement; - for (const auto & traffic_light : states_as_proto_request.states()) { + for (const auto & traffic_light : request.states()) { for (const auto & relation_id : traffic_light.relation_ids()) { // skip if the traffic light has no bulbs if (not traffic_light.traffic_light_status().empty()) { diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp index 620a666d68c..48110784a63 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -107,17 +107,6 @@ auto TrafficLightsBase::generateUpdateTrafficLightsRequest() const return update_traffic_lights_request; } -auto TrafficLightsBase::generateTrafficSimulatorV1Msg() const - -> traffic_simulator_msgs::msg::TrafficLightArrayV1 -{ - traffic_simulator_msgs::msg::TrafficLightArrayV1 traffic_lights_message; - for (const auto & [id, traffic_light] : traffic_lights_map_) { - traffic_lights_message.traffic_lights.push_back( - static_cast(traffic_light)); - } - return traffic_lights_message; -} - // private auto TrafficLightsBase::isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool { From 684195944040c1e4e85449855c2fe2e82565e4b0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 14:55:06 +0900 Subject: [PATCH 09/16] refactor: use traffic light publisher from traffic_simulator in simple_sensor_simulator --- .../traffic_lights_detector.hpp | 20 +++-- .../traffic_lights_publisher.hpp | 56 -------------- .../traffic_lights_publisher.cpp | 76 ------------------- 3 files changed, 14 insertions(+), 138 deletions(-) delete mode 100644 simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp delete mode 100644 simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp index a93fc1e9dd4..1ee2969cdb1 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp @@ -16,9 +16,9 @@ #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_ #include -#include #include #include +#include namespace simple_sensor_simulator { @@ -48,10 +48,11 @@ class TrafficLightsDetector private: template auto makePublisher(NodeType & node, const std::string & architecture_type) - -> std::unique_ptr + -> std::unique_ptr { /* TrafficLightsDetector in SimpleSensorSimulator publishes using architecture-dependent topics: + "/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" @@ -60,20 +61,27 @@ class TrafficLightsDetector */ if (architecture_type == "awf/universe") { using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray; - return std::make_unique>( + return std::make_unique>( &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>( + return std::make_unique>( &node, "/perception/traffic_light_recognition/internal/traffic_signals"); +#if __has_include() + } else if (architecture_type >= "awf/universe/20240605") { + using Message = autoware_perception_msgs::msg::TrafficLightGroupArray; + return std::make_unique>( + &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 publisher_ptr_; + const std::unique_ptr publisher_ptr_; }; } // namespace traffic_lights } // namespace simple_sensor_simulator diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp deleted file mode 100644 index 87ce6f2f61a..00000000000 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ -#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ - -#include -#include -#include -#include - -namespace simple_sensor_simulator -{ -namespace traffic_lights -{ -class TrafficLightsPublisherBase -{ -public: - virtual auto publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void = 0; -}; - -template -class TrafficLightsPublisher : public TrafficLightsPublisherBase -{ -public: - template - explicit TrafficLightsPublisher(const NodeTypePointer & node, const std::string & topic_name) - : TrafficLightsPublisherBase(), - traffic_light_state_array_publisher_( - rclcpp::create_publisher(node, topic_name, rclcpp::QoS(10).transient_local())) - { - } - - auto publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void override; - -private: - const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; -}; -} // namespace traffic_lights -} // namespace simple_sensor_simulator -#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp deleted file mode 100644 index 36b680138c2..00000000000 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// 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. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include - -namespace simple_sensor_simulator -{ -namespace traffic_lights -{ -template <> -auto TrafficLightsPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void -{ - using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; - using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; - - autoware_auto_perception_msgs::msg::TrafficSignalArray traffic_lights_message; - traffic_lights_message.header.frame_id = "camera_link"; // DIRTY HACK!!! - traffic_lights_message.header.stamp = current_ros_time; - for (const auto & traffic_light : request.states()) { - // NOT skip if the traffic light has no bulbs - TrafficLightType single_traffic_light_message; - single_traffic_light_message.map_primitive_id = traffic_light.id(); - for (const auto & bulb_status : traffic_light.traffic_light_status()) { - TrafficLightBulbType light_bulb_message; - simulation_interface::toMsg(bulb_status, light_bulb_message); - single_traffic_light_message.lights.push_back(light_bulb_message); - } - traffic_lights_message.signals.push_back(single_traffic_light_message); - } - traffic_light_state_array_publisher_->publish(traffic_lights_message); -} - -template <> -auto TrafficLightsPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void -{ - using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; - using TrafficLightBulbType = TrafficLightType::_elements_type::value_type; - - autoware_perception_msgs::msg::TrafficSignalArray traffic_lights_message; - traffic_lights_message.stamp = current_ros_time; - for (const auto & traffic_light : request.states()) { - for (const auto & relation_id : traffic_light.relation_ids()) { - // skip if the traffic light has no bulbs - if (not traffic_light.traffic_light_status().empty()) { - TrafficLightType single_traffic_light_message; - single_traffic_light_message.traffic_signal_id = relation_id; - for (const auto & bulb_status : traffic_light.traffic_light_status()) { - TrafficLightBulbType light_bulb_message; - simulation_interface::toMsg(bulb_status, light_bulb_message); - single_traffic_light_message.elements.push_back(light_bulb_message); - } - traffic_lights_message.signals.push_back(single_traffic_light_message); - } - } - } - traffic_light_state_array_publisher_->publish(traffic_lights_message); -} -} // namespace traffic_lights -} // namespace simple_sensor_simulator From 30cc70fa29253f2c4db64bc8dc5430fad1b74648 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 17:20:21 +0900 Subject: [PATCH 10/16] feat: support awf/universe/20240605 as architecture_type in mock_test.launch.py --- mock/cpp_mock_scenarios/launch/mock_test.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 0289e874095..5dfb3e1043e 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/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(): From 574b7a6fd60e3c1b99bf92165baa87905af5d113 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 17:22:24 +0900 Subject: [PATCH 11/16] feat: support awf/universe/20240605 as architecture_type for V2I traffic light --- .../traffic_lights/traffic_lights_detector.hpp | 4 ++-- .../traffic_lights/traffic_lights.hpp | 13 ++++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp index 1ee2969cdb1..5d85cf41da0 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp @@ -53,7 +53,7 @@ class TrafficLightsDetector /* TrafficLightsDetector in SimpleSensorSimulator publishes using architecture-dependent topics: "/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/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..."): @@ -63,7 +63,7 @@ class TrafficLightsDetector using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray; return std::make_unique>( &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>( &node, "/perception/traffic_light_recognition/internal/traffic_signals"); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp index eaa4b95302a..4893edf995c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp @@ -17,6 +17,11 @@ #include #include + +#if __has_include() +#include +#endif + #include #include @@ -94,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>( node_ptr, topic_name); +#if __has_include() + } else if (architecture_type >= "awf/universe/20240605") { + return std::make_unique< + TrafficLightPublisher>( + node_ptr, topic_name); +#endif } else { throw common::SemanticError( "Unexpected architecture_type ", std::quoted(architecture_type), From c00a060d26eef858a16bd62941e1a36a217b3020 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 17:52:08 +0900 Subject: [PATCH 12/16] fix: remove deleted cpp file in CMakeLists.txt --- simulation/simple_sensor_simulator/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index a66a436c362..ae8e1794af2 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/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 From 994cf9e0b436d6f46f483e70ec6a09a938561907 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 19:12:57 +0900 Subject: [PATCH 13/16] refactor: adjust include --- .../sensor_simulation/sensor_simulation.hpp | 6 ------ .../traffic_lights/traffic_lights_detector.hpp | 7 +++++++ .../include/traffic_simulator/entity/entity_manager.hpp | 7 ------- 3 files changed, 7 insertions(+), 13 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index c639ba2d477..a8014b9d690 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -19,12 +19,6 @@ #include #include -#include -#include - -#if __has_include() -#include -#endif #include #include diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp index 5d85cf41da0..1fa25db1c87 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp @@ -20,6 +20,13 @@ #include #include +#include +#include + +#if __has_include() +#include +#endif + namespace simple_sensor_simulator { namespace traffic_lights diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index d48d263ad21..de410fdcd9d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -19,12 +19,6 @@ #include #include -#include - -#if __has_include() -#include -#endif - #include #include #include @@ -53,7 +47,6 @@ #include #include #include -#include #include #include #include From 45dc9fec8747abc64f9a1256868cf72aba032915 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 19:13:33 +0900 Subject: [PATCH 14/16] fix: fix build errors with autoware_perception_msgs/msg/traffic_light_group_array.hpp --- .../src/traffic_lights/traffic_light_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index c6785abce94..3da4d9f6b6b 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -106,7 +106,7 @@ auto TrafficLightPublisher::pu template <> auto TrafficLightPublisher::publish( const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void { autoware_perception_msgs::msg::TrafficLightGroupArray message; From dd9931057f26e66546a8721bd4fd6efbc441a1db Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 11 Nov 2024 22:29:15 +0900 Subject: [PATCH 15/16] apply linter --- .../sensor_simulation/sensor_simulation.hpp | 1 - .../traffic_lights/traffic_lights_detector.hpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index a8014b9d690..55050bbea5d 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -19,7 +19,6 @@ #include #include - #include #include #include diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp index 1fa25db1c87..f075c03887e 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp @@ -15,14 +15,13 @@ #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 +#include #include #include #include #include -#include -#include - #if __has_include() #include #endif From 7e1b4adb4bce68757f623ecb68edd53eac077711 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 12 Nov 2024 15:05:56 +0900 Subject: [PATCH 16/16] refactor: delete unused member variable --- .../traffic_lights/traffic_light_publisher.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp index 7f299f0b76f..6afb9df3ad9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp @@ -43,7 +43,6 @@ class TrafficLightPublisher : public TrafficLightPublisherBase 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( node_ptr, topic_name, rclcpp::QoS(10).transient_local())) { @@ -57,7 +56,7 @@ class TrafficLightPublisher : public TrafficLightPublisherBase private: const std::string frame_; - const rclcpp::Clock::SharedPtr clock_ptr_; + const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; }; } // namespace traffic_simulator