From 1c7685e354a2250684c0935f042b86f83e4c8aea Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 11 Jun 2024 13:54:32 +0200 Subject: [PATCH 01/45] Add TrafficLightSupervisor and move traffic lights to API Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/api/api.hpp | 49 +++++- .../entity/entity_manager.hpp | 111 +++----------- .../traffic_light_supervisor.hpp | 142 ++++++++++++++++++ simulation/traffic_simulator/src/api/api.cpp | 5 +- .../src/entity/entity_manager.cpp | 12 +- 5 files changed, 209 insertions(+), 110 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index fde640ac748..ace11d336a5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -66,6 +67,9 @@ class API explicit API(NodeT && node, const Configuration & configuration, Ts &&... xs) : configuration(configuration), entity_manager_ptr_(std::make_shared(node, configuration)), + traffic_light_supervisor_ptr_(std::make_shared( + node, entity_manager_ptr_->getHdmapUtils(), + getParameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, [this](const auto & entity_name) { @@ -98,6 +102,7 @@ class API zeromq_client_( simulation_interface::protocol, configuration.simulator_host, getZMQSocketPort(*node)) { + entity_manager_ptr_->setTrafficLightSupervisor(traffic_light_supervisor_ptr_); setVerbose(configuration.verbose); if (not configuration.standalone_mode) { @@ -322,6 +327,41 @@ class API const bool allow_spawn_outside_lane = false, const bool require_footprint_fitting = false, const bool random_orientation = false, std::optional random_seed = std::nullopt) -> void; +#define FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(NAME) \ + template \ + decltype(auto) NAME(Ts &&... xs) \ + { \ + assert(traffic_light_supervisor_ptr_); \ + return traffic_light_supervisor_ptr_->NAME(std::forward(xs)...); \ + } \ + static_assert(true, "") + + FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(resetConventionalTrafficLightPublishRate); + FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(resetV2ITrafficLightPublishRate); + FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(setConventionalTrafficLightConfidence); + +#undef FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR + +#define FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(NAME) \ + template \ + decltype(auto) getConventional##NAME(Ts &&... xs) const \ + { \ + assert(traffic_light_supervisor_ptr_); \ + return traffic_light_supervisor_ptr_->getConventionalTrafficLightManager()->get##NAME(xs...); \ + } \ + template \ + decltype(auto) getV2I##NAME(Ts &&... xs) const \ + { \ + assert(traffic_light_supervisor_ptr_); \ + return traffic_light_supervisor_ptr_->getV2ITrafficLightManager()->get##NAME(xs...); \ + } \ + static_assert(true, "") + + FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLight); + FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLights); + +#undef FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER + // clang-format off #define FORWARD_TO_ENTITY_MANAGER(NAME) \ /*! \ @@ -345,8 +385,6 @@ class API FORWARD_TO_ENTITY_MANAGER(entityExists); FORWARD_TO_ENTITY_MANAGER(getBehaviorParameter); FORWARD_TO_ENTITY_MANAGER(getBoundingBox); - FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); - FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); FORWARD_TO_ENTITY_MANAGER(getCurrentAccel); FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getCurrentTwist); @@ -359,8 +397,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getLinearJerk); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); - FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); - FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isInLanelet); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); @@ -372,13 +408,10 @@ class API FORWARD_TO_ENTITY_MANAGER(requestSpeedChange); FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin); - FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); - FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(setAcceleration); FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit); FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter); - FORWARD_TO_ENTITY_MANAGER(setConventionalTrafficLightConfidence); FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit); FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setLinearVelocity); @@ -403,6 +436,8 @@ class API const std::shared_ptr entity_manager_ptr_; + const std::shared_ptr traffic_light_supervisor_ptr_; + const std::shared_ptr traffic_controller_ptr_; const rclcpp::Publisher::SharedPtr clock_pub_; 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 67ad72dbec5..1512ff4266a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -107,17 +108,23 @@ class EntityManager MarkerArray markers_raw_; - const std::shared_ptr conventional_traffic_light_manager_ptr_; - const std::shared_ptr - conventional_traffic_light_marker_publisher_ptr_; - - const std::shared_ptr v2i_traffic_light_manager_ptr_; - const std::shared_ptr v2i_traffic_light_marker_publisher_ptr_; - const std::shared_ptr v2i_traffic_light_legacy_topic_publisher_ptr_; - const std::shared_ptr v2i_traffic_light_publisher_ptr_; - ConfigurableRateUpdater v2i_traffic_light_updater_, conventional_traffic_light_updater_; + std::shared_ptr traffic_light_supervisor_ptr_; public: + /** + * This function is necessary, because TrafficLightSupervisor requires HdMapUtils. + * However, TrafficLightSupervisor is constructed in API and in that scope HdMapUtils is available + * only after EntityManager is constructed. + * This is why TrafficLightSupervisor has to be initialized in API after EntityManager and thus + * TrafficLightSupervisor needs to be passed to EntityManager after initialization. + */ + auto setTrafficLightSupervisor( + const std::shared_ptr & traffic_light_supervisor) + -> void + { + traffic_light_supervisor_ptr_ = traffic_light_supervisor; + } + template auto getOrigin(Node & node) const { @@ -136,22 +143,6 @@ class EntityManager return origin; } - template - auto makeV2ITrafficLightPublisher(Ts &&... xs) -> std::shared_ptr - { - if (const auto architecture_type = - getParameter("architecture_type", "awf/universe"); - architecture_type.find("awf/universe") != std::string::npos) { - return std::make_shared< - TrafficLightPublisher>( - std::forward(xs)...); - } else { - throw common::SemanticError( - "Unexpected architecture_type ", std::quoted(architecture_type), - " given for V2I traffic lights simulation."); - } - } - template > explicit EntityManager(NodeT && node, const Configuration & configuration) : configuration(configuration), @@ -168,29 +159,7 @@ class EntityManager rclcpp::PublisherOptionsWithAllocator())), hdmap_utils_ptr_(std::make_shared( configuration.lanelet2_map_path(), getOrigin(*node))), - markers_raw_(hdmap_utils_ptr_->generateMarker()), - conventional_traffic_light_manager_ptr_( - std::make_shared(hdmap_utils_ptr_)), - conventional_traffic_light_marker_publisher_ptr_( - std::make_shared(conventional_traffic_light_manager_ptr_, node)), - v2i_traffic_light_manager_ptr_(std::make_shared(hdmap_utils_ptr_)), - v2i_traffic_light_marker_publisher_ptr_( - std::make_shared(v2i_traffic_light_manager_ptr_, node)), - v2i_traffic_light_legacy_topic_publisher_ptr_( - makeV2ITrafficLightPublisher("/v2x/traffic_signals", node, hdmap_utils_ptr_)), - v2i_traffic_light_publisher_ptr_(makeV2ITrafficLightPublisher( - "/perception/traffic_light_recognition/external/traffic_signals", node, hdmap_utils_ptr_)), - v2i_traffic_light_updater_( - node, - [this]() { - v2i_traffic_light_marker_publisher_ptr_->publish(); - v2i_traffic_light_publisher_ptr_->publish( - clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); - v2i_traffic_light_legacy_topic_publisher_ptr_->publish( - clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); - }), - conventional_traffic_light_updater_( - node, [this]() { conventional_traffic_light_marker_publisher_ptr_->publish(); }) + markers_raw_(hdmap_utils_ptr_->generateMarker()) { updateHdmapMarker(); } @@ -198,47 +167,6 @@ class EntityManager ~EntityManager() = default; public: -#define FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(NAME) \ - template \ - decltype(auto) getConventional##NAME(Ts &&... xs) const \ - { \ - return conventional_traffic_light_manager_ptr_->get##NAME(xs...); \ - } \ - static_assert(true, ""); \ - template \ - decltype(auto) getV2I##NAME(Ts &&... xs) const \ - { \ - return v2i_traffic_light_manager_ptr_->get##NAME(xs...); \ - } \ - static_assert(true, "") - - FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLights); - FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLight); - -#undef FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER - - auto generateUpdateRequestForConventionalTrafficLights() - { - return conventional_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest(); - } - - auto resetConventionalTrafficLightPublishRate(double rate) -> void - { - conventional_traffic_light_updater_.resetUpdateRate(rate); - } - - auto resetV2ITrafficLightPublishRate(double rate) -> void - { - 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().confidence = confidence; - } - } - // clang-format off #define FORWARD_TO_ENTITY(IDENTIFIER, ...) \ /*! \ @@ -299,8 +227,6 @@ class EntityManager visualization_msgs::msg::MarkerArray makeDebugMarker() const; - bool trafficLightsChanged(); - auto updateNpcLogic(const std::string & name, const double current_time, const double step_time) -> const CanonicalizedEntityStatus &; @@ -471,7 +397,8 @@ class EntityManager std::forward(xs)...)); success) { // FIXME: this ignores V2I traffic lights - iter->second->setTrafficLightManager(conventional_traffic_light_manager_ptr_); + iter->second->setTrafficLightManager( + traffic_light_supervisor_ptr_->getConventionalTrafficLightManager()); return success; } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " is already exists."); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp new file mode 100644 index 00000000000..bf6e4b9db02 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp @@ -0,0 +1,142 @@ +// Copyright 2024 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 TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_SUPERVISOR_HPP_ +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_SUPERVISOR_HPP_ + +#include +#include +#include +#include +#include // std::out_of_range +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +class TrafficLightSupervisor +{ +public: + template + explicit TrafficLightSupervisor( + const NodePointerT & node, const std::shared_ptr & hdmap_utils, + const std::string & architecture_type) + : clock_ptr_(node->get_clock()), + conventional_traffic_light_manager_ptr_(std::make_shared(hdmap_utils)), + conventional_traffic_light_marker_publisher_ptr_( + std::make_shared(conventional_traffic_light_manager_ptr_, node)), + v2i_traffic_light_manager_ptr_(std::make_shared(hdmap_utils)), + v2i_traffic_light_marker_publisher_ptr_( + std::make_shared(v2i_traffic_light_manager_ptr_, node)), + v2i_traffic_light_legacy_topic_publisher_ptr_( + makeV2ITrafficLightPublisher(architecture_type, "/v2x/traffic_signals", node, hdmap_utils)), + v2i_traffic_light_publisher_ptr_(makeV2ITrafficLightPublisher( + architecture_type, "/perception/traffic_light_recognition/external/traffic_signals", node, + hdmap_utils)), + v2i_traffic_light_updater_( + node, + [this]() { + v2i_traffic_light_marker_publisher_ptr_->publish(); + v2i_traffic_light_publisher_ptr_->publish( + clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); + v2i_traffic_light_legacy_topic_publisher_ptr_->publish( + clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); + }), + conventional_traffic_light_updater_( + node, [this]() { conventional_traffic_light_marker_publisher_ptr_->publish(); }) + { + } + + auto getConventionalTrafficLightManager() const -> std::shared_ptr + { + return conventional_traffic_light_manager_ptr_; + } + + auto getV2ITrafficLightManager() const -> std::shared_ptr + { + return v2i_traffic_light_manager_ptr_; + } + + auto resetConventionalTrafficLightPublishRate(double rate) -> void + { + conventional_traffic_light_updater_.resetUpdateRate(rate); + } + + auto resetV2ITrafficLightPublishRate(double rate) -> void + { + 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().confidence = confidence; + } + } + + auto trafficLightsChanged() -> bool + { + return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or + v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); + } + + auto createConventionalTimer(double update_rate) -> void + { + conventional_traffic_light_updater_.createTimer(update_rate); + } + + auto createV2ITimer(double update_rate) -> void + { + v2i_traffic_light_updater_.createTimer(update_rate); + } + +protected: + template + auto makeV2ITrafficLightPublisher(const std::string & architecture_type, Ts &&... xs) + -> std::shared_ptr + { + if (architecture_type.find("awf/universe") != std::string::npos) { + return std::make_shared< + TrafficLightPublisher>( + std::forward(xs)...); + } else { + throw common::SemanticError( + "Unexpected architecture_type ", std::quoted(architecture_type), + " given for V2I traffic lights simulation."); + } + } + +protected: + const rclcpp::Clock::SharedPtr clock_ptr_; + + const std::shared_ptr conventional_traffic_light_manager_ptr_; + const std::shared_ptr + conventional_traffic_light_marker_publisher_ptr_; + + const std::shared_ptr v2i_traffic_light_manager_ptr_; + const std::shared_ptr v2i_traffic_light_marker_publisher_ptr_; + const std::shared_ptr v2i_traffic_light_legacy_topic_publisher_ptr_; + const std::shared_ptr v2i_traffic_light_publisher_ptr_; + + ConfigurableRateUpdater v2i_traffic_light_updater_, conventional_traffic_light_updater_; +}; +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_SUPERVISOR_HPP_ diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 9417d0c9c37..1246d230c28 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -268,8 +268,9 @@ bool API::updateTimeInSim() bool API::updateTrafficLightsInSim() { - if (entity_manager_ptr_->trafficLightsChanged()) { - auto req = entity_manager_ptr_->generateUpdateRequestForConventionalTrafficLights(); + if (traffic_light_supervisor_ptr_->trafficLightsChanged()) { + auto req = traffic_light_supervisor_ptr_->getConventionalTrafficLightManager() + ->generateUpdateTrafficLightsRequest(); return zeromq_client_.call(req).result().success(); } /// @todo handle response diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 0aff818a6f0..dd260a49d2f 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -346,12 +346,6 @@ auto EntityManager::getCurrentAction(const std::string & name) const -> std::str } } -bool EntityManager::trafficLightsChanged() -{ - return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or - v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); -} - void EntityManager::setVerbose(const bool verbose) { configuration.verbose = verbose; @@ -368,7 +362,7 @@ auto EntityManager::updateNpcLogic( std::cout << "update " << name << " behavior" << std::endl; } if (npc_logic_started_) { - entities_[name]->onUpdate(current_time_, step_time_); + entities_[name]->onUpdate(current_time, step_time); } return entities_[name]->getStatus(); } @@ -379,9 +373,9 @@ void EntityManager::update(const double current_time, const double step_time) "EntityManager::update", configuration.verbose); setVerbose(configuration.verbose); if (npc_logic_started_) { - conventional_traffic_light_updater_.createTimer( + traffic_light_supervisor_ptr_->createConventionalTimer( configuration.conventional_traffic_light_publish_rate); - v2i_traffic_light_updater_.createTimer(configuration.v2i_traffic_light_publish_rate); + traffic_light_supervisor_ptr_->createV2ITimer(configuration.v2i_traffic_light_publish_rate); } std::unordered_map all_status; for (auto && [name, entity] : entities_) { From 6b2b7fbdf0aa0930923c05305f34f9f0dd21e48a Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 19 Jun 2024 20:47:38 +0200 Subject: [PATCH 02/45] feat(traffic_lights): develop TrafficLightsBase and TrafficLights --- simulation/traffic_simulator/CMakeLists.txt | 12 +- .../traffic_lights/traffic_lights.hpp | 122 ++++++++++++ .../traffic_lights/traffic_lights_base.hpp | 188 ++++++++++++++++++ .../src/traffic_lights/traffic_lights.cpp | 17 ++ .../traffic_lights/traffic_lights_base.cpp | 17 ++ 5 files changed, 351 insertions(+), 5 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp create mode 100644 simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp create mode 100644 simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index c7e9c3e212a..29872da993f 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -45,17 +45,19 @@ ament_auto_add_library(traffic_simulator SHARED src/entity/vehicle_entity.cpp src/hdmap_utils/hdmap_utils.cpp src/helper/helper.cpp - src/job/job.cpp src/job/job_list.cpp + src/job/job.cpp src/simulation_clock/simulation_clock.cpp - src/traffic/traffic_controller.cpp - src/traffic/traffic_sink.cpp - src/traffic/traffic_source.cpp src/traffic_lights/configurable_rate_updater.cpp - src/traffic_lights/traffic_light.cpp src/traffic_lights/traffic_light_manager.cpp src/traffic_lights/traffic_light_marker_publisher.cpp src/traffic_lights/traffic_light_publisher.cpp + src/traffic_lights/traffic_light.cpp + src/traffic_lights/traffic_lights_base.cpp + src/traffic_lights/traffic_lights.cpp + src/traffic/traffic_controller.cpp + src/traffic/traffic_sink.cpp + src/traffic/traffic_source.cpp src/utils/distance.cpp src/utils/pose.cpp ) 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 new file mode 100644 index 00000000000..c6d88f94377 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp @@ -0,0 +1,122 @@ +// Copyright 2024 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 TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ + +#include +#include +#include +#include + +namespace traffic_simulator +{ +class ConventionalTrafficLights : public TrafficLightsBase +{ +public: + template + ConventionalTrafficLights( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) + : TrafficLightsBase(node_ptr, hdmap_utils) + { + } +}; + +class V2ITrafficLights : public TrafficLightsBase +{ +public: + template + V2ITrafficLights( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils, + const std::string & architecture_type) + : TrafficLightsBase(node_ptr, hdmap_utils), + publisher_ptr_(makePublisher( + architecture_type, "/perception/traffic_light_recognition/external/traffic_signals", node_ptr, + hdmap_utils)), + legacy_publisher_ptr_( + makePublisher(architecture_type, "/v2x/traffic_signals", node_ptr, hdmap_utils)) + { + } + +private: + auto update() -> void + { + TrafficLightsBase::update(); + publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); + legacy_publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); + } + + template + auto makePublisher(const std::string & architecture_type, Ts &&... xs) + -> std::unique_ptr + { + if (architecture_type.find("awf/universe") != std::string::npos) { + return std::make_unique< + TrafficLightPublisher>( + std::forward(xs)...); + } else { + throw common::SemanticError( + "Unexpected architecture_type ", std::quoted(architecture_type), + " given for V2I traffic lights simulation."); + } + } + + const std::unique_ptr publisher_ptr_; + const std::unique_ptr legacy_publisher_ptr_; +}; + +class TrafficLights +{ +public: + template + explicit TrafficLights( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils, + const std::string & architecture_type) + : conventional_traffic_lights_( + std::make_shared(node_ptr, hdmap_utils)), + v2i_traffic_lights_( + std::make_shared(node_ptr, hdmap_utils, architecture_type)) + { + } + + auto isAnyTrafficLightChanged() -> bool + { + return conventional_traffic_lights_->isAnyTrafficLightChanged() or + v2i_traffic_lights_->isAnyTrafficLightChanged(); + } + + auto startTrafficLightsUpdate( + const double conventional_traffic_light_update_rate, + const double v2i_traffic_lights_update_rate) + { + conventional_traffic_lights_->startUpdate(conventional_traffic_light_update_rate); + v2i_traffic_lights_->startUpdate(v2i_traffic_lights_update_rate); + } + + auto getConventionalTrafficLights() const -> std::shared_ptr + { + return conventional_traffic_lights_; + } + + auto getV2ITrafficLights() const -> std::shared_ptr + { + return v2i_traffic_lights_; + } + +private: + const std::shared_ptr conventional_traffic_lights_; + const std::shared_ptr v2i_traffic_lights_; +}; +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ 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 new file mode 100644 index 00000000000..3fe0410b864 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp @@ -0,0 +1,188 @@ +// 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 TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +/* + TrafficLightsBase class is designed in such a way that while trying to perform an operation + on a TrafficLight (add, set, etc.) that is not added to traffic_light_map_, + it adds it first and then performs the operation, so that the methods + here cannot be tagged with the "const" specifier +*/ +class TrafficLightsBase +{ +public: + template + explicit TrafficLightsBase( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) + : hdmap_utils_(hdmap_utils), + clock_ptr_(node_ptr->get_clock()), + rate_updater_(node_ptr, [this]() { update(); }), + marker_publisher_ptr_(std::make_unique(node_ptr)) + { + } + + // update + auto startUpdate(const double update_rate) -> void { rate_updater_.startTimer(update_rate); } + + auto resetUpdate(const double update_rate) -> void { rate_updater_.resetTimer(update_rate); } + + // checks, setters, getters + auto isAnyTrafficLightChanged() const -> bool { return true; } + + auto isRequiredStopTrafficLightState(const lanelet::Id traffic_light_id) -> bool + { + using Color = traffic_simulator::TrafficLight::Color; + using Status = traffic_simulator::TrafficLight::Status; + using Shape = traffic_simulator::TrafficLight::Shape; + const auto & traffic_light = getTrafficLight(traffic_light_id); + return ( + traffic_light.contains(Color::red, Status::solid_on, Shape::circle) or + traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)); + } + + auto compareTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) + { + if (const auto & considered_traffic_lights = getTrafficLights(lanelet_id); + state.empty() || state == "none") { + return std::all_of( + std::begin(considered_traffic_lights), std::end(considered_traffic_lights), + [](const auto & considered_traffic_light) { + return considered_traffic_light.get().empty(); + }); + } else { + return std::all_of( + std::begin(considered_traffic_lights), std::end(considered_traffic_lights), + [&state](const auto & considered_traffic_light) { + return considered_traffic_light.get().contains(state); + }); + } + } + + auto setTrafficLightsColor( + const lanelet::Id lanelet_id, const traffic_simulator::TrafficLight::Color & color) -> void + { + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().emplace(color); + } + } + + auto setTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> void + { + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().clear(); + traffic_light.get().set(state); + } + } + + auto setTrafficLightsConfidence(const lanelet::Id lanelet_id, const double confidence) -> void + { + for (auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().confidence = confidence; + } + } + + auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string + { + std::stringstream ss; + std::string separator = ""; + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + ss << separator << traffic_light; + separator = "; "; + } + return ss.str(); + } + + auto generateUpdateTrafficLightsRequest() const + -> simulation_api_schema::UpdateTrafficLightsRequest + { + simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; + for (auto && [lanelet_id, traffic_light] : traffic_lights_map_) { + *update_traffic_lights_request.add_states() = + static_cast(traffic_light); + } + return update_traffic_lights_request; + } + +protected: + virtual auto update() const -> void + { + if (isAnyTrafficLightChanged()) { + marker_publisher_ptr_->deleteMarkers(); + } + marker_publisher_ptr_->drawMarkers(traffic_lights_map_); + } + + auto isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool + { + return traffic_lights_map_.find(traffic_light_id) != traffic_lights_map_.end(); + } + + auto addTrafficLight(const lanelet::Id traffic_light_id) -> void + { + // emplace will not modify the map if the key already exists + traffic_lights_map_.emplace( + std::piecewise_construct, std::forward_as_tuple(traffic_light_id), + std::forward_as_tuple(traffic_light_id, *hdmap_utils_)); + } + + auto getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight & + { + addTrafficLight(traffic_light_id); + return traffic_lights_map_.at(traffic_light_id); + } + + auto getTrafficLights(const lanelet::Id lanelet_id) + -> std::vector> + { + // if passed id is regulatory element containing traffic_lights, add all of them + // if passed id is single traffic_light - add it, return all added traffic_lights + std::vector> traffic_lights; + if (hdmap_utils_->isTrafficLightRegulatoryElement(lanelet_id)) { + const auto & regulatory_element = hdmap_utils_->getTrafficLightRegulatoryElement(lanelet_id); + for (auto && traffic_light : regulatory_element->trafficLights()) { + traffic_lights.emplace_back(getTrafficLight(traffic_light.id())); + } + } else if (hdmap_utils_->isTrafficLight(lanelet_id)) { + traffic_lights.emplace_back(getTrafficLight(lanelet_id)); + } else { + throw common::scenario_simulator_exception::Error( + "Given lanelet ID ", lanelet_id, + " is neither a traffic light ID not a traffic relation ID."); + } + return traffic_lights; + } + + const std::shared_ptr hdmap_utils_; + const rclcpp::Clock::SharedPtr clock_ptr_; + + std::unordered_map traffic_lights_map_; + const std::unique_ptr marker_publisher_ptr_; + ConfigurableRateUpdater rate_updater_; +}; +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp new file mode 100644 index 00000000000..09fbb074905 --- /dev/null +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp @@ -0,0 +1,17 @@ +// 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. + +namespace traffic_simulator +{ +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp new file mode 100644 index 00000000000..09fbb074905 --- /dev/null +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -0,0 +1,17 @@ +// 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. + +namespace traffic_simulator +{ +} // namespace traffic_simulator From f69d7ca83e50a60863ca0d56a113cae642204dc8 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 19 Jun 2024 20:48:42 +0200 Subject: [PATCH 03/45] feat(traffic_light_manager): use TrafficLightsBase and TrafficLights instead of TrafficLightsManager/Supervisor --- .../src/traffic_simulation_demo.cpp | 3 +- .../simulator_core.hpp | 43 ++++-- .../src/syntax/custom_command_action.cpp | 7 +- .../src/syntax/traffic_signal_condition.cpp | 24 +-- .../src/syntax/traffic_signal_state.cpp | 6 +- .../syntax/traffic_signal_state_action.cpp | 5 +- .../behavior_tree_plugin/action_node.hpp | 6 +- .../pedestrian/behavior_tree.hpp | 2 +- .../vehicle/behavior_tree.hpp | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 42 +++--- .../include/do_nothing_plugin/plugin.hpp | 2 +- simulation/traffic_simulator/CMakeLists.txt | 1 - .../include/traffic_simulator/api/api.hpp | 56 +++---- .../behavior/behavior_plugin_base.hpp | 4 +- .../traffic_simulator/entity/entity_base.hpp | 7 +- .../entity/entity_manager.hpp | 20 ++- .../entity/pedestrian_entity.hpp | 3 +- .../entity/vehicle_entity.hpp | 3 +- .../configurable_rate_updater.hpp | 4 +- .../traffic_lights/traffic_light_manager.hpp | 61 -------- .../traffic_light_marker_publisher.hpp | 49 +++--- .../traffic_light_supervisor.hpp | 142 ------------------ simulation/traffic_simulator/src/api/api.cpp | 6 +- .../src/entity/entity_base.cpp | 6 +- .../src/entity/entity_manager.cpp | 6 +- .../src/entity/pedestrian_entity.cpp | 8 +- .../src/entity/vehicle_entity.cpp | 8 +- .../configurable_rate_updater.cpp | 4 +- .../traffic_lights/traffic_light_manager.cpp | 99 ------------ .../traffic_light_marker_publisher.cpp | 34 ----- .../test_traffic_light_manager.cpp | 140 ++++++++--------- 31 files changed, 224 insertions(+), 579 deletions(-) delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp delete mode 100644 simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 8b85c526883..8b63e97d2e5 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -146,7 +146,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 index 19cc3ec08a1..887f7587a73 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -583,40 +583,57 @@ class SimulatorCore } template - static auto getConventionalTrafficLights(Ts &&... xs) -> decltype(auto) + static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto) { - return core->getConventionalTrafficLights(std::forward(xs)...); + return asFieldOperatorApplication(core->getEgoName()) + .sendCooperateCommand(std::forward(xs)...); } + // TrafficLights - Conventional and V2I template - static auto getV2ITrafficLights(Ts &&... xs) -> decltype(auto) + static auto setConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto) { - return core->getV2ITrafficLights(std::forward(xs)...); + return core->getConventionalTrafficLights()->setTrafficLightsState( + std::forward(xs)...); } template - static auto resetConventionalTrafficLightPublishRate(Ts &&... xs) -> decltype(auto) + static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto) { - return core->resetConventionalTrafficLightPublishRate(std::forward(xs)...); + return core->getConventionalTrafficLights()->setTrafficLightsConfidence( + std::forward(xs)...); } template - static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto) + static auto getConventionalTrafficLightsComposedState(Ts &&... xs) -> decltype(auto) { - return core->resetV2ITrafficLightPublishRate(std::forward(xs)...); + return core->getConventionalTrafficLights()->getTrafficLightsComposedState( + std::forward(xs)...); } template - static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto) + static auto compareConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto) { - return asFieldOperatorApplication(core->getEgoName()) - .sendCooperateCommand(std::forward(xs)...); + return core->getConventionalTrafficLights()->compareTrafficLightsState( + std::forward(xs)...); } template - static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto) + static auto resetConventionalTrafficLightPublishRate(Ts &&... xs) -> decltype(auto) + { + return core->getConventionalTrafficLights()->resetUpdate(std::forward(xs)...); + } + + template + static auto setV2ITrafficLightsState(Ts &&... xs) -> decltype(auto) + { + return core->getV2ITrafficLights()->setTrafficLightsState(std::forward(xs)...); + } + + template + static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto) { - return core->setConventionalTrafficLightConfidence(std::forward(xs)...); + return core->getV2ITrafficLights()->resetUpdate(std::forward(xs)...); } }; }; diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp index a54c9731c54..41253df4f87 100644 --- 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(parameters[0]))) { - traffic_light.get().clear(); - traffic_light.get().set(unquote(parameters.at(1))); - } + setV2ITrafficLightsState( + boost::lexical_cast(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 index e97b8c87a03..0270ca1d671 100644 --- 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(name)); - state == "none") { + const auto lanelet_id = boost::lexical_cast(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 index 7b972e5322c..f4aa0a7d039 100644 --- 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 index 45e1f59b7a9..9571d8d6fce 100644 --- 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 index eda786c8202..37e1a9c06b8 100644 --- 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 #include #include -#include +#include #include #include #include @@ -86,7 +86,7 @@ class ActionNode : public BT::ActionNodeBase BT::InputPort>("target_speed"), BT::InputPort>("hdmap_utils"), BT::InputPort>("entity_status"), - BT::InputPort>("traffic_light_manager"), + BT::InputPort>("traffic_lights"), BT::InputPort("request"), BT::OutputPort>("obstacle"), BT::OutputPort>("non_canonicalized_updated_status"), @@ -113,7 +113,7 @@ class ActionNode : public BT::ActionNodeBase protected: traffic_simulator::behavior::Request request; std::shared_ptr hdmap_utils; - std::shared_ptr traffic_light_manager; + std::shared_ptr traffic_lights; std::shared_ptr 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 index 8f94cad2a07..58503d2d9de 100644 --- 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) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) 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 index 7789a28d9be..eae51146f2c 100644 --- 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) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index b686714ffc2..8ca0574c26f 100644 --- 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>("hdmap_utils", hdmap_utils)) { THROW_SIMULATION_ERROR("failed to get input hdmap_utils in ActionNode"); } - if (!getInput>( - "traffic_light_manager", traffic_light_manager)) { - THROW_SIMULATION_ERROR("failed to get input traffic_light_manager in ActionNode"); + if (!getInput>( + "traffic_lights", traffic_lights)) { + THROW_SIMULATION_ERROR("failed to get input traffic_lights in ActionNode"); } if (!getInput>( "entity_status", entity_status)) { @@ -191,28 +191,26 @@ auto ActionNode::getDistanceToTrafficLightStopLine( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { - const auto traffic_light_ids = hdmap_utils->getTrafficLightIdsOnPath(route_lanelets); - if (traffic_light_ids.empty()) { - return std::nullopt; - } - std::set 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 collision_points = {}; + for (const auto traffic_light_id : traffic_light_ids) { + using Color = traffic_simulator::TrafficLight::Color; + using Status = traffic_simulator::TrafficLight::Status; + using Shape = traffic_simulator::TrafficLight::Shape; + 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 index 3de43ba43b9..57b0bbc5191 100644 --- 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) DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) 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/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 29872da993f..638ddd73fcf 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -49,7 +49,6 @@ ament_auto_add_library(traffic_simulator SHARED src/job/job.cpp src/simulation_clock/simulation_clock.cpp src/traffic_lights/configurable_rate_updater.cpp - src/traffic_lights/traffic_light_manager.cpp src/traffic_lights/traffic_light_marker_publisher.cpp src/traffic_lights/traffic_light_publisher.cpp src/traffic_lights/traffic_light.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 419176b49f5..ceb4229e627 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include @@ -67,7 +67,7 @@ class API explicit API(NodeT && node, const Configuration & configuration, Ts &&... xs) : configuration(configuration), entity_manager_ptr_(std::make_shared(node, configuration)), - traffic_light_supervisor_ptr_(std::make_shared( + traffic_lights_ptr_(std::make_shared( node, entity_manager_ptr_->getHdmapUtils(), getParameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( @@ -103,7 +103,7 @@ class API zeromq_client_( simulation_interface::protocol, configuration.simulator_host, getZMQSocketPort(*node)) { - entity_manager_ptr_->setTrafficLightSupervisor(traffic_light_supervisor_ptr_); + entity_manager_ptr_->setTrafficLights(traffic_lights_ptr_); setVerbose(configuration.verbose); if (not configuration.standalone_mode) { @@ -328,40 +328,26 @@ class API const bool allow_spawn_outside_lane = false, const bool require_footprint_fitting = false, const bool random_orientation = false, std::optional random_seed = std::nullopt) -> void; -#define FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(NAME) \ - template \ - decltype(auto) NAME(Ts &&... xs) \ - { \ - assert(traffic_light_supervisor_ptr_); \ - return traffic_light_supervisor_ptr_->NAME(std::forward(xs)...); \ - } \ - static_assert(true, "") - - FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(resetConventionalTrafficLightPublishRate); - FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(resetV2ITrafficLightPublishRate); - FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR(setConventionalTrafficLightConfidence); - -#undef FORWARD_TO_TRAFFIC_LIGHT_SUPERVISOR - -#define FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(NAME) \ - template \ - decltype(auto) getConventional##NAME(Ts &&... xs) const \ - { \ - assert(traffic_light_supervisor_ptr_); \ - return traffic_light_supervisor_ptr_->getConventionalTrafficLightManager()->get##NAME(xs...); \ - } \ - template \ - decltype(auto) getV2I##NAME(Ts &&... xs) const \ - { \ - assert(traffic_light_supervisor_ptr_); \ - return traffic_light_supervisor_ptr_->getV2ITrafficLightManager()->get##NAME(xs...); \ - } \ + // clang-format off +#define FORWARD_TO_TRAFFIC_LIGHTS(NAME) \ + /*! \ + @brief Forward to arguments to the TrafficLights::NAME function. \ + @return return value of the TrafficLights::NAME function. \ + @note This function was defined by FORWARD_TO_TRAFFIC_LIGHTS macro. \ + */ \ + template \ + decltype(auto) NAME(Ts &&... xs) \ + { \ + assert(traffic_lights_ptr_); \ + return (*traffic_lights_ptr_).NAME(std::forward(xs)...); \ + } \ static_assert(true, "") + // clang-format on - FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLight); - FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLights); + FORWARD_TO_TRAFFIC_LIGHTS(getV2ITrafficLights); + FORWARD_TO_TRAFFIC_LIGHTS(getConventionalTrafficLights); -#undef FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER +#undef FORWARD_TO_TRAFFIC_LIGHTS // clang-format off #define FORWARD_TO_ENTITY_MANAGER(NAME) \ @@ -437,7 +423,7 @@ class API const std::shared_ptr entity_manager_ptr_; - const std::shared_ptr traffic_light_supervisor_ptr_; + const std::shared_ptr traffic_lights_ptr_; const std::shared_ptr traffic_controller_ptr_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index a18d82da316..761e5644106 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -71,7 +71,7 @@ class BehaviorPluginBase DEFINE_GETTER_SETTER(RouteLanelets, "route_lanelets", lanelet::Ids) DEFINE_GETTER_SETTER(StepTime, "step_time", double) DEFINE_GETTER_SETTER(TargetSpeed, "target_speed", std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, "traffic_light_manager", std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, "traffic_lights", std::shared_ptr) DEFINE_GETTER_SETTER(UpdatedStatus, "non_canonicalized_updated_status", std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 81cce21051b..48d2bd8017f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include #include @@ -191,8 +191,7 @@ class EntityBase virtual auto setLinearVelocity(const double linear_velocity) -> void; - virtual void setTrafficLightManager( - const std::shared_ptr &); + virtual void setTrafficLights(const std::shared_ptr &); virtual auto activateOutOfRangeJob( double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, @@ -228,7 +227,7 @@ class EntityBase CanonicalizedEntityStatus status_before_update_; std::shared_ptr hdmap_utils_ptr_; - std::shared_ptr traffic_light_manager_; + std::shared_ptr traffic_lights_; double stand_still_duration_ = 0.0; double traveled_distance_ = 0.0; 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 a3ecce428a3..2f344a4af8d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include @@ -108,21 +108,20 @@ class EntityManager MarkerArray markers_raw_; - std::shared_ptr traffic_light_supervisor_ptr_; + std::shared_ptr traffic_lights_ptr_; public: /** - * This function is necessary, because TrafficLightSupervisor requires HdMapUtils. - * However, TrafficLightSupervisor is constructed in API and in that scope HdMapUtils is available + * This function is necessary, because TrafficLights requires HdMapUtils. + * However, TrafficLights is constructed in API and in that scope HdMapUtils is available * only after EntityManager is constructed. - * This is why TrafficLightSupervisor has to be initialized in API after EntityManager and thus - * TrafficLightSupervisor needs to be passed to EntityManager after initialization. + * This is why TrafficLights has to be initialized in API after EntityManager and thus + * TrafficLights needs to be passed to EntityManager after initialization. */ - auto setTrafficLightSupervisor( - const std::shared_ptr & traffic_light_supervisor) + auto setTrafficLights(const std::shared_ptr & traffic_lights) -> void { - traffic_light_supervisor_ptr_ = traffic_light_supervisor; + traffic_lights_ptr_ = traffic_lights; } template @@ -397,8 +396,7 @@ class EntityManager std::forward(xs)...)); success) { // FIXME: this ignores V2I traffic lights - iter->second->setTrafficLightManager( - traffic_light_supervisor_ptr_->getConventionalTrafficLightManager()); + iter->second->setTrafficLights(traffic_lights_ptr_->getConventionalTrafficLights()); return success; } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " is already exists."); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index c152248997e..47044327e5c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -80,8 +80,7 @@ class PedestrianEntity : public EntityBase void cancelRequest() override; - void setTrafficLightManager( - const std::shared_ptr & ptr) override; + void setTrafficLights(const std::shared_ptr & ptr) override; auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index a8ee76dd248..ce669bb28e3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -121,8 +121,7 @@ class VehicleEntity : public EntityBase void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override; - void setTrafficLightManager( - const std::shared_ptr &) override; + void setTrafficLights(const std::shared_ptr &) override; const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp index 695511cebfc..9c93f01a518 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp @@ -39,9 +39,9 @@ class ConfigurableRateUpdater { } - auto createTimer(double update_rate) -> void; + auto startTimer(double update_rate) -> void; - auto resetUpdateRate(double update_rate) -> void; + auto resetTimer(double update_rate) -> void; auto getUpdateRate() const -> double { return update_rate_; } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp deleted file mode 100644 index c43e9e940a6..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp +++ /dev/null @@ -1,61 +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 TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MANAGER_BASE_HPP_ -#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MANAGER_BASE_HPP_ - -#include -#include -#include -#include -#include // std::out_of_range -#include -#include -#include -#include -#include -#include -#include - -namespace traffic_simulator -{ -class TrafficLightManager -{ -protected: - using TrafficLightMap = std::unordered_map; - - TrafficLightMap traffic_lights_; - - const std::shared_ptr hdmap_; - -public: - explicit TrafficLightManager(const std::shared_ptr & hdmap); - - auto getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight &; - - auto getTrafficLightIds() const -> const lanelet::Ids; - - auto getTrafficLights() const -> const TrafficLightMap &; - - auto getTrafficLights() -> TrafficLightMap &; - - auto getTrafficLights(const lanelet::Id lanelet_id) - -> std::vector>; - - auto hasAnyLightChanged() -> bool; - - auto generateUpdateTrafficLightsRequest() -> simulation_api_schema::UpdateTrafficLightsRequest; -}; -} // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MANAGER_BASE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp index 9e2524c7077..91841715179 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp @@ -15,35 +15,46 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP -#include +#include namespace traffic_simulator { class TrafficLightMarkerPublisher { - const rclcpp::Publisher::SharedPtr marker_pub_; - const std::string map_frame_; - const rclcpp::Clock::SharedPtr clock_ptr_; - const std::shared_ptr traffic_light_manager_; - - auto deleteAllMarkers() const -> void; - auto drawMarkers() const -> void; - public: - template + template explicit TrafficLightMarkerPublisher( - const std::shared_ptr & traffic_light_manager, const NodePointer & node, - const std::string & map_frame = "map") - : marker_pub_(rclcpp::create_publisher( - node, "traffic_light/marker", rclcpp::QoS(1).transient_local())), - map_frame_(map_frame), - clock_ptr_(node->get_clock()), - traffic_light_manager_(traffic_light_manager) + const NodeTypePointer & node_ptr, const std::string & frame = "map") + : frame_(frame), + clock_ptr_(node_ptr->get_clock()), + publisher_(rclcpp::create_publisher( + node_ptr, "traffic_light/marker", rclcpp::QoS(1).transient_local())) { } - auto publish() -> void; -}; + auto deleteMarkers() const -> void + { + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array.markers.push_back(marker); + publisher_->publish(marker_array); + } + auto drawMarkers(const std::unordered_map & traffic_lights_map) const + -> void + { + visualization_msgs::msg::MarkerArray marker_array; + for (const auto & [id, traffic_light] : traffic_lights_map) { + traffic_light.draw(marker_array.markers, clock_ptr_->now(), frame_); + } + publisher_->publish(marker_array); + } + +private: + const std::string frame_; + const rclcpp::Clock::SharedPtr clock_ptr_; + const rclcpp::Publisher::SharedPtr publisher_; +}; } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp deleted file mode 100644 index bf6e4b9db02..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_supervisor.hpp +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright 2024 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 TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_SUPERVISOR_HPP_ -#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_SUPERVISOR_HPP_ - -#include -#include -#include -#include -#include // std::out_of_range -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace traffic_simulator -{ -class TrafficLightSupervisor -{ -public: - template - explicit TrafficLightSupervisor( - const NodePointerT & node, const std::shared_ptr & hdmap_utils, - const std::string & architecture_type) - : clock_ptr_(node->get_clock()), - conventional_traffic_light_manager_ptr_(std::make_shared(hdmap_utils)), - conventional_traffic_light_marker_publisher_ptr_( - std::make_shared(conventional_traffic_light_manager_ptr_, node)), - v2i_traffic_light_manager_ptr_(std::make_shared(hdmap_utils)), - v2i_traffic_light_marker_publisher_ptr_( - std::make_shared(v2i_traffic_light_manager_ptr_, node)), - v2i_traffic_light_legacy_topic_publisher_ptr_( - makeV2ITrafficLightPublisher(architecture_type, "/v2x/traffic_signals", node, hdmap_utils)), - v2i_traffic_light_publisher_ptr_(makeV2ITrafficLightPublisher( - architecture_type, "/perception/traffic_light_recognition/external/traffic_signals", node, - hdmap_utils)), - v2i_traffic_light_updater_( - node, - [this]() { - v2i_traffic_light_marker_publisher_ptr_->publish(); - v2i_traffic_light_publisher_ptr_->publish( - clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); - v2i_traffic_light_legacy_topic_publisher_ptr_->publish( - clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); - }), - conventional_traffic_light_updater_( - node, [this]() { conventional_traffic_light_marker_publisher_ptr_->publish(); }) - { - } - - auto getConventionalTrafficLightManager() const -> std::shared_ptr - { - return conventional_traffic_light_manager_ptr_; - } - - auto getV2ITrafficLightManager() const -> std::shared_ptr - { - return v2i_traffic_light_manager_ptr_; - } - - auto resetConventionalTrafficLightPublishRate(double rate) -> void - { - conventional_traffic_light_updater_.resetUpdateRate(rate); - } - - auto resetV2ITrafficLightPublishRate(double rate) -> void - { - 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().confidence = confidence; - } - } - - auto trafficLightsChanged() -> bool - { - return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or - v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); - } - - auto createConventionalTimer(double update_rate) -> void - { - conventional_traffic_light_updater_.createTimer(update_rate); - } - - auto createV2ITimer(double update_rate) -> void - { - v2i_traffic_light_updater_.createTimer(update_rate); - } - -protected: - template - auto makeV2ITrafficLightPublisher(const std::string & architecture_type, Ts &&... xs) - -> std::shared_ptr - { - if (architecture_type.find("awf/universe") != std::string::npos) { - return std::make_shared< - TrafficLightPublisher>( - std::forward(xs)...); - } else { - throw common::SemanticError( - "Unexpected architecture_type ", std::quoted(architecture_type), - " given for V2I traffic lights simulation."); - } - } - -protected: - const rclcpp::Clock::SharedPtr clock_ptr_; - - const std::shared_ptr conventional_traffic_light_manager_ptr_; - const std::shared_ptr - conventional_traffic_light_marker_publisher_ptr_; - - const std::shared_ptr v2i_traffic_light_manager_ptr_; - const std::shared_ptr v2i_traffic_light_marker_publisher_ptr_; - const std::shared_ptr v2i_traffic_light_legacy_topic_publisher_ptr_; - const std::shared_ptr v2i_traffic_light_publisher_ptr_; - - ConfigurableRateUpdater v2i_traffic_light_updater_, conventional_traffic_light_updater_; -}; -} // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_SUPERVISOR_HPP_ diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 0b3024d1474..b36884104e6 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -269,9 +269,9 @@ bool API::updateTimeInSim() bool API::updateTrafficLightsInSim() { - if (traffic_light_supervisor_ptr_->trafficLightsChanged()) { - auto req = traffic_light_supervisor_ptr_->getConventionalTrafficLightManager() - ->generateUpdateTrafficLightsRequest(); + if (traffic_lights_ptr_->isAnyTrafficLightChanged()) { + auto req = + traffic_lights_ptr_->getConventionalTrafficLights()->generateUpdateTrafficLightsRequest(); return zeromq_client_.call(req).result().success(); } /// @todo handle response diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 1e307e3bbaf..e0f9cdecef2 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -536,10 +536,10 @@ auto EntityBase::setLinearAcceleration(const double linear_acceleration) -> void status_.setLinearAcceleration(linear_acceleration); } -void EntityBase::setTrafficLightManager( - const std::shared_ptr & traffic_light_manager) +void EntityBase::setTrafficLights( + const std::shared_ptr & traffic_lights) { - traffic_light_manager_ = traffic_light_manager; + traffic_lights_ = traffic_lights; } auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 1bf0c987965..490edad772b 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -385,9 +385,9 @@ void EntityManager::update(const double current_time, const double step_time) "EntityManager::update", configuration.verbose); setVerbose(configuration.verbose); if (npc_logic_started_) { - traffic_light_supervisor_ptr_->createConventionalTimer( - configuration.conventional_traffic_light_publish_rate); - traffic_light_supervisor_ptr_->createV2ITimer(configuration.v2i_traffic_light_publish_rate); + traffic_lights_ptr_->startTrafficLightsUpdate( + configuration.conventional_traffic_light_publish_rate, + configuration.v2i_traffic_light_publish_rate); } std::unordered_map all_status; for (auto && [name, entity] : entities_) { diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 317b5fd533c..7f27c77d39e 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -174,11 +174,11 @@ auto PedestrianEntity::getEntityTypename() const -> const std::string & return result; } -void PedestrianEntity::setTrafficLightManager( - const std::shared_ptr & ptr) +void PedestrianEntity::setTrafficLights( + const std::shared_ptr & ptr) { - EntityBase::setTrafficLightManager(ptr); - behavior_plugin_ptr_->setTrafficLightManager(traffic_light_manager_); + EntityBase::setTrafficLights(ptr); + behavior_plugin_ptr_->setTrafficLights(traffic_lights_); } auto PedestrianEntity::getBehaviorParameter() const diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index c34e39fdcd6..8f1ac5047f0 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -313,11 +313,11 @@ void VehicleEntity::setBehaviorParameter( behavior_plugin_ptr_->setBehaviorParameter(parameter); } -void VehicleEntity::setTrafficLightManager( - const std::shared_ptr & ptr) +void VehicleEntity::setTrafficLights( + const std::shared_ptr & ptr) { - EntityBase::setTrafficLightManager(ptr); - behavior_plugin_ptr_->setTrafficLightManager(traffic_light_manager_); + EntityBase::setTrafficLights(ptr); + behavior_plugin_ptr_->setTrafficLights(traffic_lights_); } } // namespace entity diff --git a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp index fd1f41e87a3..0118b245422 100644 --- a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp @@ -16,7 +16,7 @@ namespace traffic_simulator { -auto ConfigurableRateUpdater::createTimer(double update_rate) -> void +auto ConfigurableRateUpdater::startTimer(double update_rate) -> void { if (!timer_) { update_rate_ = update_rate; @@ -27,7 +27,7 @@ auto ConfigurableRateUpdater::createTimer(double update_rate) -> void } } -auto ConfigurableRateUpdater::resetUpdateRate(double update_rate) -> void +auto ConfigurableRateUpdater::resetTimer(double update_rate) -> void { if (update_rate_ != update_rate) { update_rate_ = update_rate; diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp deleted file mode 100644 index fe983240de8..00000000000 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp +++ /dev/null @@ -1,99 +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 -#include -#include -#include -#include - -namespace traffic_simulator -{ -TrafficLightManager::TrafficLightManager(const std::shared_ptr & hdmap) -: hdmap_(hdmap) -{ -} - -auto TrafficLightManager::hasAnyLightChanged() -> bool -{ - return true; - // return std::any_of( - // std::begin(getTrafficLights()), std::end(getTrafficLights()), [](auto && id_and_traffic_light) { - // return id_and_traffic_light.second.colorChanged() or - // id_and_traffic_light.second.arrowChanged(); - // }); -} - -auto TrafficLightManager::getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight & -{ - if (auto iter = traffic_lights_.find(traffic_light_id); iter != std::end(traffic_lights_)) { - return iter->second; - } else { - traffic_lights_.emplace( - std::piecewise_construct, std::forward_as_tuple(traffic_light_id), - std::forward_as_tuple(traffic_light_id, *hdmap_)); - return traffic_lights_.at(traffic_light_id); - } -} - -auto TrafficLightManager::getTrafficLightIds() const -> const lanelet::Ids -{ - lanelet::Ids traffic_light_ids; - for (const auto & traffic_light_ : getTrafficLights()) { - traffic_light_ids.push_back(traffic_light_.first); - } - return traffic_light_ids; -} - -auto TrafficLightManager::getTrafficLights() const -> const TrafficLightMap & -{ - return traffic_lights_; -} - -auto TrafficLightManager::getTrafficLights() -> TrafficLightMap & { return traffic_lights_; } - -auto TrafficLightManager::getTrafficLights(const lanelet::Id lanelet_id) - -> std::vector> -{ - std::vector> traffic_lights; - - if (hdmap_->isTrafficLightRegulatoryElement(lanelet_id)) { - for (auto && traffic_light : - hdmap_->getTrafficLightRegulatoryElement(lanelet_id)->trafficLights()) { - traffic_lights.emplace_back(getTrafficLight(traffic_light.id())); - } - } else if (hdmap_->isTrafficLight(lanelet_id)) { - traffic_lights.emplace_back(getTrafficLight(lanelet_id)); - } else { - throw common::scenario_simulator_exception::Error( - "Given lanelet ID ", lanelet_id, " is neither a traffic light ID not a traffic relation ID."); - } - - return traffic_lights; -} - -auto TrafficLightManager::generateUpdateTrafficLightsRequest() - -> simulation_api_schema::UpdateTrafficLightsRequest -{ - simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; - for (auto && [lanelet_id, traffic_light] : traffic_lights_) { - *update_traffic_lights_request.add_states() = - static_cast(traffic_light); - } - return update_traffic_lights_request; -} - -} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp index 3cf16db030f..ef56735992f 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp @@ -17,38 +17,4 @@ namespace traffic_simulator { -auto TrafficLightMarkerPublisher::deleteAllMarkers() const -> void -{ - visualization_msgs::msg::MarkerArray message; - { - visualization_msgs::msg::Marker marker; - marker.action = marker.DELETEALL; - message.markers.push_back(marker); - } - - marker_pub_->publish(message); -} - -auto TrafficLightMarkerPublisher::drawMarkers() const -> void -{ - visualization_msgs::msg::MarkerArray marker_array; - - const auto now = clock_ptr_->now(); - - for (const auto & [id, traffic_light] : traffic_light_manager_->getTrafficLights()) { - traffic_light.draw(marker_array.markers, now, map_frame_); - } - - marker_pub_->publish(marker_array); -} - -auto TrafficLightMarkerPublisher::publish() -> void -{ - if (traffic_light_manager_->hasAnyLightChanged()) { - deleteAllMarkers(); - } - - drawMarkers(); -} - } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index fa6b3aad1ad..d8927125897 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -16,79 +16,79 @@ #include #include -#include +#include -TEST(TrafficLightManager, getIds) -{ - const auto node = std::make_shared("getIds"); - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - const auto hdmap_utils_ptr = std::make_shared(path, origin); - traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); - manager.getTrafficLight(34836); - EXPECT_FALSE(manager.getTrafficLights().find(34836) == std::end(manager.getTrafficLights())); - manager.getTrafficLight(34802); - EXPECT_FALSE(manager.getTrafficLights().find(34802) == std::end(manager.getTrafficLights())); - EXPECT_EQ(manager.getTrafficLights().size(), static_cast(2)); -} +// TEST(TrafficLightManager, getIds) +// { +// const auto node = std::make_shared("getIds"); +// std::string path = +// ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; +// geographic_msgs::msg::GeoPoint origin; +// origin.latitude = 35.61836750154; +// origin.longitude = 139.78066608243; +// const auto hdmap_utils_ptr = std::make_shared(path, origin); +// traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); +// manager.getTrafficLight(34836); +// EXPECT_FALSE(manager.getTrafficLights().find(34836) == std::end(manager.getTrafficLights())); +// manager.getTrafficLight(34802); +// EXPECT_FALSE(manager.getTrafficLights().find(34802) == std::end(manager.getTrafficLights())); +// EXPECT_EQ(manager.getTrafficLights().size(), static_cast(2)); +// } -TEST(TrafficLightManager, setColor) -{ - const auto node = std::make_shared("setColor"); - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - const auto hdmap_utils_ptr = std::make_shared(path, origin); - traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); - for (const auto & [id, traffic_light] : manager.getTrafficLights()) { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green); - EXPECT_TRUE( - manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::circle)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::yellow); - EXPECT_TRUE( - manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::circle)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::red); - EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::red, Status::solid_on, Shape::circle)); - } -} +// TEST(TrafficLightManager, setColor) +// { +// const auto node = std::make_shared("setColor"); +// std::string path = +// ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; +// geographic_msgs::msg::GeoPoint origin; +// origin.latitude = 35.61836750154; +// origin.longitude = 139.78066608243; +// const auto hdmap_utils_ptr = std::make_shared(path, origin); +// traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); +// for (const auto & [id, traffic_light] : manager.getTrafficLights()) { +// using Color = traffic_simulator::TrafficLight::Color; +// using Status = traffic_simulator::TrafficLight::Status; +// using Shape = traffic_simulator::TrafficLight::Shape; +// manager.getTrafficLight(id).clear(); +// manager.getTrafficLight(id).emplace(Color::green); +// EXPECT_TRUE( +// manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::circle)); +// manager.getTrafficLight(id).clear(); +// manager.getTrafficLight(id).emplace(Color::yellow); +// EXPECT_TRUE( +// manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::circle)); +// manager.getTrafficLight(id).clear(); +// manager.getTrafficLight(id).emplace(Color::red); +// EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::red, Status::solid_on, Shape::circle)); +// } +// } -TEST(TrafficLightManager, setArrow) -{ - const auto node = std::make_shared("setArrow"); - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - const auto hdmap_utils_ptr = std::make_shared(path, origin); - traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); - for (const auto & [id, traffic_light] : manager.getTrafficLights()) { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::left); - EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::left)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::right); - EXPECT_TRUE( - manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::right)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::up); - EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::up)); - } -} +// TEST(TrafficLightManager, setArrow) +// { +// const auto node = std::make_shared("setArrow"); +// std::string path = +// ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; +// geographic_msgs::msg::GeoPoint origin; +// origin.latitude = 35.61836750154; +// origin.longitude = 139.78066608243; +// const auto hdmap_utils_ptr = std::make_shared(path, origin); +// traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); +// for (const auto & [id, traffic_light] : manager.getTrafficLights()) { +// using Color = traffic_simulator::TrafficLight::Color; +// using Status = traffic_simulator::TrafficLight::Status; +// using Shape = traffic_simulator::TrafficLight::Shape; +// manager.getTrafficLight(id).clear(); +// manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::left); +// EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::left)); +// manager.getTrafficLight(id).clear(); +// manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::right); +// EXPECT_TRUE( +// manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::right)); +// manager.getTrafficLight(id).clear(); +// manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::up); +// EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::up)); +// } +// } int main(int argc, char ** argv) { From 4af5eec4943a79337beab1f8781685caac240e95 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 20 Jun 2024 12:14:04 +0200 Subject: [PATCH 04/45] ref(traffic_simulator/traffic_lights): refactor all classes insead of TrafficLight --- .../configurable_rate_updater.hpp | 26 ++-- .../traffic_lights/traffic_light.hpp | 4 +- .../traffic_light_marker_publisher.hpp | 21 +-- .../traffic_light_publisher.hpp | 18 +-- .../traffic_lights/traffic_lights.hpp | 32 ++-- .../traffic_lights/traffic_lights_base.hpp | 127 ++-------------- .../configurable_rate_updater.cpp | 29 ++-- .../src/traffic_lights/traffic_light.cpp | 19 +-- .../traffic_light_marker_publisher.cpp | 20 ++- .../traffic_light_publisher.cpp | 56 ++++--- .../src/traffic_lights/traffic_lights.cpp | 28 +++- .../traffic_lights/traffic_lights_base.cpp | 139 +++++++++++++++++- 12 files changed, 286 insertions(+), 233 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp index 9c93f01a518..b6604f45e63 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -22,28 +22,30 @@ namespace traffic_simulator { class ConfigurableRateUpdater { - rclcpp::TimerBase::SharedPtr timer_ = nullptr; - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; - const rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_interface_; - double update_rate_ = 0.0; - const std::function thunk_; - const rclcpp::Clock::SharedPtr clock_ptr_; - public: template ConfigurableRateUpdater(const NodePointer & node, std::function thunk) : node_base_interface_(node->get_node_base_interface()), node_timers_interface_(node->get_node_timers_interface()), - thunk_(thunk), - clock_ptr_(node->get_clock()) + clock_ptr_(node->get_clock()), + thunk_(std::move(thunk)) { } - auto startTimer(double update_rate) -> void; + auto startTimer(const double update_rate) -> void; - auto resetTimer(double update_rate) -> void; + auto resetTimer(const double update_rate) -> void; auto getUpdateRate() const -> double { return update_rate_; } + +private: + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; + const rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_interface_; + const rclcpp::Clock::SharedPtr clock_ptr_; + + rclcpp::TimerBase::SharedPtr timer_{nullptr}; + const std::function thunk_; + double update_rate_ = 0.0; }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index 88fed84ff8a..1ab50e53dfd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -320,6 +320,8 @@ struct TrafficLight } }; + explicit TrafficLight(const lanelet::Id, const hdmap_utils::HdMapUtils &); + const lanelet::Id way_id; double confidence = 1.0; @@ -328,8 +330,6 @@ struct TrafficLight const std::map> positions; - explicit TrafficLight(const lanelet::Id, hdmap_utils::HdMapUtils &); - auto clear() { bulbs.clear(); } auto contains(const Bulb & bulb) const { return bulbs.find(bulb) != std::end(bulbs); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp index 91841715179..b19d3fd6333 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -15,6 +15,7 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP +#include #include namespace traffic_simulator @@ -32,24 +33,10 @@ class TrafficLightMarkerPublisher { } - auto deleteMarkers() const -> void - { - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array.markers.push_back(marker); - publisher_->publish(marker_array); - } + auto deleteMarkers() const -> void; auto drawMarkers(const std::unordered_map & traffic_lights_map) const - -> void - { - visualization_msgs::msg::MarkerArray marker_array; - for (const auto & [id, traffic_light] : traffic_lights_map) { - traffic_light.draw(marker_array.markers, clock_ptr_->now(), frame_); - } - publisher_->publish(marker_array); - } + -> void; private: const std::string frame_; 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 d292f01582c..e26d0626970 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 @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -28,20 +28,16 @@ class TrafficLightPublisherBase public: virtual auto publish( const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void = 0; + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void = 0; }; template class TrafficLightPublisher : public TrafficLightPublisherBase { - const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; - - const std::shared_ptr hdmap_utils_; - public: - template + template explicit TrafficLightPublisher( - const std::string & topic_name, const NodePointer & node, + const std::string & topic_name, const NodeTypePointer & node, const std::shared_ptr & hdmap_utils = nullptr) : TrafficLightPublisherBase(), traffic_light_state_array_publisher_( @@ -52,7 +48,11 @@ class TrafficLightPublisher : public TrafficLightPublisherBase auto publish( const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void override; + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void override; + +private: + const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; + const std::shared_ptr hdmap_utils_; }; } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ 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 c6d88f94377..64cc420e362 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 @@ -44,23 +44,25 @@ class V2ITrafficLights : public TrafficLightsBase publisher_ptr_(makePublisher( architecture_type, "/perception/traffic_light_recognition/external/traffic_signals", node_ptr, hdmap_utils)), - legacy_publisher_ptr_( + legacy_topic_publisher_ptr_( makePublisher(architecture_type, "/v2x/traffic_signals", node_ptr, hdmap_utils)) { } private: - auto update() -> void + auto update() const -> void override { TrafficLightsBase::update(); publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); - legacy_publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); + legacy_topic_publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); } template auto makePublisher(const std::string & architecture_type, Ts &&... xs) -> std::unique_ptr { + /// here autoware_perception_msgs is used for all awf/universe/**. + /// @note perhaps autoware_auto_perception_msgs should be used for >= "awf/universe/20230906"? if (architecture_type.find("awf/universe") != std::string::npos) { return std::make_unique< TrafficLightPublisher>( @@ -73,7 +75,7 @@ class V2ITrafficLights : public TrafficLightsBase } const std::unique_ptr publisher_ptr_; - const std::unique_ptr legacy_publisher_ptr_; + const std::unique_ptr legacy_topic_publisher_ptr_; }; class TrafficLights @@ -90,29 +92,15 @@ class TrafficLights { } - auto isAnyTrafficLightChanged() -> bool - { - return conventional_traffic_lights_->isAnyTrafficLightChanged() or - v2i_traffic_lights_->isAnyTrafficLightChanged(); - } + auto isAnyTrafficLightChanged() -> bool; auto startTrafficLightsUpdate( const double conventional_traffic_light_update_rate, - const double v2i_traffic_lights_update_rate) - { - conventional_traffic_lights_->startUpdate(conventional_traffic_light_update_rate); - v2i_traffic_lights_->startUpdate(v2i_traffic_lights_update_rate); - } + const double v2i_traffic_lights_update_rate) -> void; - auto getConventionalTrafficLights() const -> std::shared_ptr - { - return conventional_traffic_lights_; - } + auto getConventionalTrafficLights() const -> std::shared_ptr; - auto getV2ITrafficLights() const -> std::shared_ptr - { - return v2i_traffic_lights_; - } + auto getV2ITrafficLights() const -> std::shared_ptr; private: const std::shared_ptr conventional_traffic_lights_; 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 3fe0410b864..3a0bfc9644c 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 @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -47,135 +47,40 @@ class TrafficLightsBase } // update - auto startUpdate(const double update_rate) -> void { rate_updater_.startTimer(update_rate); } + auto startUpdate(const double update_rate) -> void; - auto resetUpdate(const double update_rate) -> void { rate_updater_.resetTimer(update_rate); } + auto resetUpdate(const double update_rate) -> void; // checks, setters, getters - auto isAnyTrafficLightChanged() const -> bool { return true; } + auto isAnyTrafficLightChanged() const -> bool; - auto isRequiredStopTrafficLightState(const lanelet::Id traffic_light_id) -> bool - { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; - const auto & traffic_light = getTrafficLight(traffic_light_id); - return ( - traffic_light.contains(Color::red, Status::solid_on, Shape::circle) or - traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)); - } + auto isRequiredStopTrafficLightState(const lanelet::Id traffic_light_id) -> bool; - auto compareTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) - { - if (const auto & considered_traffic_lights = getTrafficLights(lanelet_id); - state.empty() || state == "none") { - return std::all_of( - std::begin(considered_traffic_lights), std::end(considered_traffic_lights), - [](const auto & considered_traffic_light) { - return considered_traffic_light.get().empty(); - }); - } else { - return std::all_of( - std::begin(considered_traffic_lights), std::end(considered_traffic_lights), - [&state](const auto & considered_traffic_light) { - return considered_traffic_light.get().contains(state); - }); - } - } + auto compareTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> bool; auto setTrafficLightsColor( - const lanelet::Id lanelet_id, const traffic_simulator::TrafficLight::Color & color) -> void - { - for (const auto & traffic_light : getTrafficLights(lanelet_id)) { - traffic_light.get().emplace(color); - } - } + const lanelet::Id lanelet_id, const traffic_simulator::TrafficLight::Color & color) -> void; - auto setTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> void - { - for (const auto & traffic_light : getTrafficLights(lanelet_id)) { - traffic_light.get().clear(); - traffic_light.get().set(state); - } - } + auto setTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> void; - auto setTrafficLightsConfidence(const lanelet::Id lanelet_id, const double confidence) -> void - { - for (auto & traffic_light : getTrafficLights(lanelet_id)) { - traffic_light.get().confidence = confidence; - } - } + auto setTrafficLightsConfidence(const lanelet::Id lanelet_id, const double confidence) -> void; - auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string - { - std::stringstream ss; - std::string separator = ""; - for (const auto & traffic_light : getTrafficLights(lanelet_id)) { - ss << separator << traffic_light; - separator = "; "; - } - return ss.str(); - } + auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string; auto generateUpdateTrafficLightsRequest() const - -> simulation_api_schema::UpdateTrafficLightsRequest - { - simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; - for (auto && [lanelet_id, traffic_light] : traffic_lights_map_) { - *update_traffic_lights_request.add_states() = - static_cast(traffic_light); - } - return update_traffic_lights_request; - } + -> simulation_api_schema::UpdateTrafficLightsRequest; protected: - virtual auto update() const -> void - { - if (isAnyTrafficLightChanged()) { - marker_publisher_ptr_->deleteMarkers(); - } - marker_publisher_ptr_->drawMarkers(traffic_lights_map_); - } + virtual auto update() const -> void; - auto isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool - { - return traffic_lights_map_.find(traffic_light_id) != traffic_lights_map_.end(); - } + auto isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool; - auto addTrafficLight(const lanelet::Id traffic_light_id) -> void - { - // emplace will not modify the map if the key already exists - traffic_lights_map_.emplace( - std::piecewise_construct, std::forward_as_tuple(traffic_light_id), - std::forward_as_tuple(traffic_light_id, *hdmap_utils_)); - } + auto addTrafficLight(const lanelet::Id traffic_light_id) -> void; - auto getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight & - { - addTrafficLight(traffic_light_id); - return traffic_lights_map_.at(traffic_light_id); - } + auto getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight &; auto getTrafficLights(const lanelet::Id lanelet_id) - -> std::vector> - { - // if passed id is regulatory element containing traffic_lights, add all of them - // if passed id is single traffic_light - add it, return all added traffic_lights - std::vector> traffic_lights; - if (hdmap_utils_->isTrafficLightRegulatoryElement(lanelet_id)) { - const auto & regulatory_element = hdmap_utils_->getTrafficLightRegulatoryElement(lanelet_id); - for (auto && traffic_light : regulatory_element->trafficLights()) { - traffic_lights.emplace_back(getTrafficLight(traffic_light.id())); - } - } else if (hdmap_utils_->isTrafficLight(lanelet_id)) { - traffic_lights.emplace_back(getTrafficLight(lanelet_id)); - } else { - throw common::scenario_simulator_exception::Error( - "Given lanelet ID ", lanelet_id, - " is neither a traffic light ID not a traffic relation ID."); - } - return traffic_lights; - } + -> std::vector>; const std::shared_ptr hdmap_utils_; const rclcpp::Clock::SharedPtr clock_ptr_; diff --git a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp index 0118b245422..6301d5cb79e 100644 --- a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -12,33 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include namespace traffic_simulator { -auto ConfigurableRateUpdater::startTimer(double update_rate) -> void +auto ConfigurableRateUpdater::startTimer(const double update_rate) -> void { if (!timer_) { update_rate_ = update_rate; - using namespace std::chrono_literals; + const auto period = std::chrono::duration(1.0 / update_rate_); timer_ = rclcpp::create_timer( - node_base_interface_, node_timers_interface_, clock_ptr_, 1s / update_rate_, - [this]() -> void { thunk_(); }); + node_base_interface_, node_timers_interface_, clock_ptr_, period, [this]() { thunk_(); }); + } else { + throw std::logic_error( + "ConfigurableRateUpdater was started earlier, cannot be started again - reset instead."); } } -auto ConfigurableRateUpdater::resetTimer(double update_rate) -> void +auto ConfigurableRateUpdater::resetTimer(const double update_rate) -> void { - if (update_rate_ != update_rate) { - update_rate_ = update_rate; - if (timer_ && not timer_->is_canceled()) { - timer_->cancel(); - } - - using namespace std::chrono_literals; - timer_ = rclcpp::create_timer( - node_base_interface_, node_timers_interface_, clock_ptr_, 1s / update_rate_, - [this]() -> void { thunk_(); }); + if (timer_ && !timer_->is_canceled()) { + timer_->cancel(); + timer_.reset(); } + startTimer(update_rate); } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp index 4501ba7ae0d..ef745269fa1 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp @@ -12,14 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include -#include -#include -#include namespace traffic_simulator { @@ -166,13 +160,14 @@ auto operator<<(std::ostream & os, const TrafficLight::Bulb & bulb) -> std::ostr << std::get(bulb.value); } -TrafficLight::TrafficLight(const lanelet::Id lanelet_id, hdmap_utils::HdMapUtils & map_manager) +TrafficLight::TrafficLight( + const lanelet::Id lanelet_id, const hdmap_utils::HdMapUtils & hdmap_utils) : way_id([&]() { - if (map_manager.isTrafficLight(lanelet_id)) { + if (hdmap_utils.isTrafficLight(lanelet_id)) { return lanelet_id; } else { // lanelet::RoleName::Refers - if (auto traffic_light_members = map_manager.getTrafficLightRegulatoryElement(lanelet_id) + if (auto traffic_light_members = hdmap_utils.getTrafficLightRegulatoryElement(lanelet_id) ->getParameters("refers"); traffic_light_members.size() > 0) { // Note: If `lanelet_id` is a relation id, it is okay to use only one of the referred way ids. @@ -187,13 +182,13 @@ TrafficLight::TrafficLight(const lanelet::Id lanelet_id, hdmap_utils::HdMapUtils positions{ std::make_pair( Bulb(Color::green, Status::solid_on, Shape::circle).hash(), - map_manager.getTrafficLightBulbPosition(way_id, "green")), + hdmap_utils.getTrafficLightBulbPosition(way_id, "green")), std::make_pair( Bulb(Color::yellow, Status::solid_on, Shape::circle).hash(), - map_manager.getTrafficLightBulbPosition(way_id, "yellow")), + hdmap_utils.getTrafficLightBulbPosition(way_id, "yellow")), std::make_pair( Bulb(Color::red, Status::solid_on, Shape::circle).hash(), - map_manager.getTrafficLightBulbPosition(way_id, "red")), + hdmap_utils.getTrafficLightBulbPosition(way_id, "red")), } { } diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp index ef56735992f..225008624f7 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -17,4 +17,22 @@ namespace traffic_simulator { +auto TrafficLightMarkerPublisher::deleteMarkers() const -> void +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array.markers.push_back(marker); + publisher_->publish(marker_array); +} + +auto TrafficLightMarkerPublisher::drawMarkers( + const std::unordered_map & traffic_lights_map) const -> void +{ + visualization_msgs::msg::MarkerArray marker_array; + for (const auto & [id, traffic_light] : traffic_lights_map) { + traffic_light.draw(marker_array.markers, clock_ptr_->now(), frame_); + } + publisher_->publish(marker_array); +} } // namespace traffic_simulator 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 64f6b8bd3e7..356c3f69283 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -21,57 +21,55 @@ namespace traffic_simulator 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_auto_perception_msgs::msg::TrafficSignalArray message; using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; - message.header.frame_id = "camera_link"; // DIRTY HACK!!! - message.header.stamp = current_ros_time; + 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()) { - TrafficLightType traffic_light_message; - traffic_light_message.map_primitive_id = traffic_light.id(); - for (auto bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; + // 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); - traffic_light_message.lights.push_back(light_bulb_message); + single_traffic_light_message.lights.push_back(light_bulb_message); } - message.signals.push_back(traffic_light_message); + traffic_lights_message.signals.push_back(single_traffic_light_message); } - traffic_light_state_array_publisher_->publish(message); + traffic_light_state_array_publisher_->publish(traffic_lights_message); } 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 { - assert(hdmap_utils_ != nullptr); + using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; + using TrafficLightBulbType = TrafficLightType::_elements_type::value_type; - autoware_perception_msgs::msg::TrafficSignalArray message; - message.stamp = current_ros_time; + autoware_perception_msgs::msg::TrafficSignalArray traffic_lights_message; + traffic_lights_message.stamp = current_ros_time; for (const auto & traffic_light : request.states()) { - auto relation_ids = + const auto relation_ids = hdmap_utils_->getTrafficLightRegulatoryElementIDsFromTrafficLight(traffic_light.id()); - - for (auto relation_id : relation_ids) { + for (const auto & relation_id : relation_ids) { // skip if the traffic light has no bulbs if (not traffic_light.traffic_light_status().empty()) { - using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; - TrafficLightType traffic_light_message; - traffic_light_message.traffic_signal_id = relation_id; - - for (auto bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = - autoware_perception_msgs::msg::TrafficSignal::_elements_type::value_type; + 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); - traffic_light_message.elements.push_back(light_bulb_message); + single_traffic_light_message.elements.push_back(light_bulb_message); } - message.signals.push_back(traffic_light_message); + traffic_lights_message.signals.push_back(single_traffic_light_message); } } } - traffic_light_state_array_publisher_->publish(message); + traffic_light_state_array_publisher_->publish(traffic_lights_message); } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp index 09fbb074905..b233cce9158 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -12,6 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + namespace traffic_simulator { +auto TrafficLights::isAnyTrafficLightChanged() -> bool +{ + return conventional_traffic_lights_->isAnyTrafficLightChanged() or + v2i_traffic_lights_->isAnyTrafficLightChanged(); +} + +auto TrafficLights::startTrafficLightsUpdate( + const double conventional_traffic_light_update_rate, const double v2i_traffic_lights_update_rate) + -> void +{ + conventional_traffic_lights_->startUpdate(conventional_traffic_light_update_rate); + v2i_traffic_lights_->startUpdate(v2i_traffic_lights_update_rate); +} + +auto TrafficLights::getConventionalTrafficLights() const + -> std::shared_ptr +{ + return conventional_traffic_lights_; +} + +auto TrafficLights::getV2ITrafficLights() const -> std::shared_ptr +{ + return v2i_traffic_lights_; +} } // namespace traffic_simulator 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 09fbb074905..f513c703e73 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -12,6 +12,143 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + namespace traffic_simulator { +auto TrafficLightsBase::startUpdate(const double update_rate) -> void +{ + rate_updater_.startTimer(update_rate); +} + +auto TrafficLightsBase::resetUpdate(const double update_rate) -> void +{ + rate_updater_.resetTimer(update_rate); +} + +// checks, setters, getters +auto TrafficLightsBase::isAnyTrafficLightChanged() const -> bool { return true; } + +auto TrafficLightsBase::isRequiredStopTrafficLightState(const lanelet::Id traffic_light_id) -> bool +{ + using Color = traffic_simulator::TrafficLight::Color; + using Status = traffic_simulator::TrafficLight::Status; + using Shape = traffic_simulator::TrafficLight::Shape; + const auto & traffic_light = getTrafficLight(traffic_light_id); + return ( + traffic_light.contains(Color::red, Status::solid_on, Shape::circle) or + traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)); +} + +auto TrafficLightsBase::compareTrafficLightsState( + const lanelet::Id lanelet_id, const std::string & state) -> bool +{ + if (const auto & considered_traffic_lights = getTrafficLights(lanelet_id); + state.empty() || state == "none") { + return std::all_of( + std::begin(considered_traffic_lights), std::end(considered_traffic_lights), + [](const auto & considered_traffic_light) { return considered_traffic_light.get().empty(); }); + } else { + return std::all_of( + std::begin(considered_traffic_lights), std::end(considered_traffic_lights), + [&state](const auto & considered_traffic_light) { + return considered_traffic_light.get().contains(state); + }); + } +} + +auto TrafficLightsBase::setTrafficLightsColor( + const lanelet::Id lanelet_id, const traffic_simulator::TrafficLight::Color & color) -> void +{ + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().emplace(color); + } +} + +auto TrafficLightsBase::setTrafficLightsState( + const lanelet::Id lanelet_id, const std::string & state) -> void +{ + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().clear(); + traffic_light.get().set(state); + } +} + +auto TrafficLightsBase::setTrafficLightsConfidence( + const lanelet::Id lanelet_id, const double confidence) -> void +{ + for (auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().confidence = confidence; + } +} + +auto TrafficLightsBase::getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string +{ + std::stringstream ss; + std::string separator = ""; + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + ss << separator << traffic_light; + separator = "; "; + } + return ss.str(); +} + +auto TrafficLightsBase::generateUpdateTrafficLightsRequest() const + -> simulation_api_schema::UpdateTrafficLightsRequest +{ + simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; + for (auto && [lanelet_id, traffic_light] : traffic_lights_map_) { + *update_traffic_lights_request.add_states() = + static_cast(traffic_light); + } + return update_traffic_lights_request; +} + +// private +auto TrafficLightsBase::update() const -> void +{ + if (isAnyTrafficLightChanged()) { + marker_publisher_ptr_->deleteMarkers(); + } + marker_publisher_ptr_->drawMarkers(traffic_lights_map_); +} + +auto TrafficLightsBase::isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool +{ + return traffic_lights_map_.find(traffic_light_id) != traffic_lights_map_.end(); +} + +auto TrafficLightsBase::addTrafficLight(const lanelet::Id traffic_light_id) -> void +{ + // emplace will not modify the map if the key already exists + traffic_lights_map_.emplace( + std::piecewise_construct, std::forward_as_tuple(traffic_light_id), + std::forward_as_tuple(traffic_light_id, *hdmap_utils_)); +} + +auto TrafficLightsBase::getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight & +{ + addTrafficLight(traffic_light_id); + return traffic_lights_map_.at(traffic_light_id); +} + +auto TrafficLightsBase::getTrafficLights(const lanelet::Id lanelet_id) + -> std::vector> +{ + // if passed id is regulatory element containing traffic_lights, add all of them + // if passed id is single traffic_light - add it, return all added traffic_lights + std::vector> traffic_lights; + if (hdmap_utils_->isTrafficLightRegulatoryElement(lanelet_id)) { + const auto & regulatory_element = hdmap_utils_->getTrafficLightRegulatoryElement(lanelet_id); + for (auto && traffic_light : regulatory_element->trafficLights()) { + traffic_lights.emplace_back(getTrafficLight(traffic_light.id())); + } + } else if (hdmap_utils_->isTrafficLight(lanelet_id)) { + traffic_lights.emplace_back(getTrafficLight(lanelet_id)); + } else { + throw common::scenario_simulator_exception::Error( + "Given lanelet ID ", lanelet_id, " is neither a traffic light ID not a traffic relation ID."); + } + return traffic_lights; +} } // namespace traffic_simulator From 2a260eee0a9be7a40aa0afac538c7854f6fbbcd6 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 20 Jun 2024 14:10:07 +0200 Subject: [PATCH 05/45] feat(scenario_sensor_simulator/traffic_lights_detector): remove dependencies on hdmap_utils, improve creation --- .../sensor_simulation/sensor_simulation.hpp | 20 ++-------- .../traffic_lights_detector.hpp | 37 +++++++++++++++---- .../src/simple_sensor_simulator.cpp | 2 +- .../proto/simulation_api_schema.proto | 1 + .../traffic_lights/traffic_light.hpp | 5 +++ .../traffic_light_publisher.hpp | 14 +++---- .../traffic_lights/traffic_lights.hpp | 26 ++++++++----- .../src/traffic_lights/traffic_light.cpp | 1 + .../traffic_light_publisher.cpp | 4 +- 9 files changed, 63 insertions(+), 47 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 6927a364d4f..4dd0a6a3572 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 @@ -94,24 +94,10 @@ class SensorSimulation auto attachPseudoTrafficLightsDetector( const double /*current_simulation_time*/, const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration, - rclcpp::Node & node, std::shared_ptr 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( - std::make_shared>( - "/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( - std::make_shared>( - "/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( + node, configuration.architecture_type())); } auto updateSensorFrame( 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 b9d4f9155df..9a9e93d8eca 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 @@ -1,4 +1,4 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. +// Copyright 2024 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. @@ -32,12 +32,10 @@ namespace traffic_lights */ class TrafficLightsDetector { - const std::shared_ptr publisher_; - public: - explicit TrafficLightsDetector( - const std::shared_ptr & publisher) - : publisher_(publisher) + template + explicit TrafficLightsDetector(NodeType & node, const std::string & architecture_type) + : publisher_ptr_(makePublisher(node, architecture_type)) { } @@ -45,8 +43,33 @@ class TrafficLightsDetector 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 + auto makePublisher(NodeType & node, const std::string & architecture_type) + -> std::unique_ptr + { + /* + V2ITrafficLights in TrafficSimulator publishes using topics "/v2x/traffic_signals" and + "/perception/traffic_light_recognition/external/traffic_signals" for all "awf/universe/..." + */ + if (architecture_type == "awf/universe") { + 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") { + using Message = autoware_perception_msgs::msg::TrafficSignalArray; + return std::make_unique>( + &node, "/perception/traffic_light_recognition/internal/traffic_signals"); + } else { + throw common::SemanticError( + "Unexpected architecture_type ", std::quoted(architecture_type), " given."); + } + }; + + const std::unique_ptr publisher_ptr_; }; } // namespace traffic_lights } // namespace simple_sensor_simulator diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index 4f3c1130989..43756c7579a 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -344,7 +344,7 @@ auto ScenarioSimulator::attachPseudoTrafficLightDetector( { auto response = simulation_api_schema::AttachPseudoTrafficLightDetectorResponse(); sensor_sim_.attachPseudoTrafficLightsDetector( - current_simulation_time_, req.configuration(), *this, hdmap_utils_); + current_simulation_time_, req.configuration(), *this); response.mutable_result()->set_success(true); return response; } diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto index b2b4b409d44..1fac7fef33e 100644 --- a/simulation/simulation_interface/proto/simulation_api_schema.proto +++ b/simulation/simulation_interface/proto/simulation_api_schema.proto @@ -294,6 +294,7 @@ message TrafficLight { message TrafficSignal { int32 id = 1; repeated TrafficLight traffic_light_status = 2; + repeated int32 relation_ids = 3; // TrafficLightRegulatoryElement ids based on TrafficLight id } /** diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index 1ab50e53dfd..f7824e62cd1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -324,6 +324,8 @@ struct TrafficLight const lanelet::Id way_id; + const lanelet::Ids regulatory_elements_ids; + double confidence = 1.0; std::set bulbs; @@ -390,6 +392,9 @@ struct TrafficLight simulation_api_schema::TrafficSignal traffic_signal_proto; traffic_signal_proto.set_id(way_id); + for (const auto relation_id : regulatory_elements_ids) { + traffic_signal_proto.add_relation_ids(relation_id); + } for (const auto & bulb : bulbs) { auto traffic_light_bulb_proto = static_cast(bulb); traffic_light_bulb_proto.set_confidence(confidence); 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 e26d0626970..0c8fb4a9e79 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 @@ -23,6 +23,7 @@ namespace traffic_simulator { + class TrafficLightPublisherBase { public: @@ -30,19 +31,15 @@ class TrafficLightPublisherBase const rclcpp::Time & current_ros_time, const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void = 0; }; - -template +template class TrafficLightPublisher : public TrafficLightPublisherBase { public: template - explicit TrafficLightPublisher( - const std::string & topic_name, const NodeTypePointer & node, - const std::shared_ptr & hdmap_utils = nullptr) + explicit TrafficLightPublisher(const NodeTypePointer & node, const std::string & topic_name) : TrafficLightPublisherBase(), traffic_light_state_array_publisher_( - rclcpp::create_publisher(node, topic_name, rclcpp::QoS(10).transient_local())), - hdmap_utils_(hdmap_utils) + rclcpp::create_publisher(node, topic_name, rclcpp::QoS(10).transient_local())) { } @@ -51,8 +48,7 @@ class TrafficLightPublisher : public TrafficLightPublisherBase const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void override; private: - const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; - const std::shared_ptr hdmap_utils_; + const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; }; } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ 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 64cc420e362..f1af7615053 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 @@ -42,10 +42,9 @@ class V2ITrafficLights : public TrafficLightsBase const std::string & architecture_type) : TrafficLightsBase(node_ptr, hdmap_utils), publisher_ptr_(makePublisher( - architecture_type, "/perception/traffic_light_recognition/external/traffic_signals", node_ptr, - hdmap_utils)), - legacy_topic_publisher_ptr_( - makePublisher(architecture_type, "/v2x/traffic_signals", node_ptr, hdmap_utils)) + node_ptr, architecture_type, + "/perception/traffic_light_recognition/external/traffic_signals")), + legacy_topic_publisher_ptr_(makePublisher(node_ptr, architecture_type, "/v2x/traffic_signals")) { } @@ -57,16 +56,23 @@ class V2ITrafficLights : public TrafficLightsBase legacy_topic_publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); } - template - auto makePublisher(const std::string & architecture_type, Ts &&... xs) - -> std::unique_ptr + template + auto makePublisher( + const NodeTypePointer & node_ptr, const std::string & architecture_type, + const std::string & topic_name) -> std::unique_ptr { - /// here autoware_perception_msgs is used for all awf/universe/**. - /// @note perhaps autoware_auto_perception_msgs should be used for >= "awf/universe/20230906"? + /* + Here autoware_perception_msgs is used for all awf/universe/.... + Perhaps autoware_auto_perception_msgs should be used for >= "awf/universe/20230906"? + + PseudoTrafficLightsDetector in SimpleSensorSimulator publishes using topic + "/perception/traffic_light_recognition/traffic_signals" for >= "awf/universe/20230906" + otherwise uses "/perception/traffic_light_recognition/internal/traffic_signals". + */ if (architecture_type.find("awf/universe") != std::string::npos) { return std::make_unique< TrafficLightPublisher>( - std::forward(xs)...); + node_ptr, topic_name); } else { throw common::SemanticError( "Unexpected architecture_type ", std::quoted(architecture_type), diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp index ef745269fa1..f7cc4e8079a 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp @@ -179,6 +179,7 @@ TrafficLight::TrafficLight( } } }()), + regulatory_elements_ids(hdmap_utils.getTrafficLightRegulatoryElementIDsFromTrafficLight(way_id)), positions{ std::make_pair( Bulb(Color::green, Status::solid_on, Shape::circle).hash(), 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 356c3f69283..cd261dcaed0 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -54,9 +54,7 @@ auto TrafficLightPublisher::p autoware_perception_msgs::msg::TrafficSignalArray traffic_lights_message; traffic_lights_message.stamp = current_ros_time; for (const auto & traffic_light : request.states()) { - const auto relation_ids = - hdmap_utils_->getTrafficLightRegulatoryElementIDsFromTrafficLight(traffic_light.id()); - for (const auto & relation_id : relation_ids) { + 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; From ef3818f5c3ae6fa883673318d87d9cedf5fcbc99 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 20 Jun 2024 18:42:20 +0200 Subject: [PATCH 06/45] feat(traffic_lights): develop cast for autoware msgs --- .../traffic_lights/traffic_light.hpp | 167 ++++++++++++++++++ 1 file changed, 167 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index f7824e62cd1..430ebd49cdd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -15,6 +15,9 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_ +#include +#include +#include #include #include #include @@ -318,6 +321,138 @@ struct TrafficLight return traffic_light_bulb_proto; } + + explicit operator autoware_auto_perception_msgs::msg::TrafficLight() const + { + auto color = [this]() { + switch (std::get(value).value) { + case Color::green: + return autoware_auto_perception_msgs::msg::TrafficLight::GREEN; + case Color::yellow: + return autoware_auto_perception_msgs::msg::TrafficLight::AMBER; + case Color::red: + return autoware_auto_perception_msgs::msg::TrafficLight::RED; + case Color::white: + return autoware_auto_perception_msgs::msg::TrafficLight::WHITE; + default: + return autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN; + } + }; + + auto status = [this]() { + switch (std::get(value).value) { + case Status::solid_on: + return autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON; + case Status::solid_off: + return autoware_auto_perception_msgs::msg::TrafficLight::SOLID_OFF; + case Status::flashing: + return autoware_auto_perception_msgs::msg::TrafficLight::FLASHING; + default: + return autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN; + } + }; + + auto shape = [this]() { + switch (std::get(value).value) { + case Shape::circle: + return autoware_auto_perception_msgs::msg::TrafficLight::CIRCLE; + case Shape::cross: + return autoware_auto_perception_msgs::msg::TrafficLight::CROSS; + case Shape::left: + return autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW; + case Shape::down: + return autoware_auto_perception_msgs::msg::TrafficLight::DOWN_ARROW; + case Shape::up: + return autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW; + case Shape::right: + return autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW; + case Shape::lower_left: + return autoware_auto_perception_msgs::msg::TrafficLight::DOWN_LEFT_ARROW; + case Shape::lower_right: + return autoware_auto_perception_msgs::msg::TrafficLight::DOWN_RIGHT_ARROW; + case Shape::upper_left: + return autoware_auto_perception_msgs::msg::TrafficLight::UP_LEFT_ARROW; + case Shape::upper_right: + return autoware_auto_perception_msgs::msg::TrafficLight::UP_RIGHT_ARROW; + default: + return autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN; + }; + }; + + autoware_auto_perception_msgs::msg::TrafficLight msg; + msg.color = color(); + msg.status = status(); + msg.shape = shape(); + // NOTE: confidence will be overwritten + msg.confidence = 1.0; + return msg; + } + + explicit operator autoware_perception_msgs::msg::TrafficSignalElement() const + { + auto color = [this]() { + switch (std::get(value).value) { + case Color::green: + return autoware_perception_msgs::msg::TrafficSignalElement::GREEN; + case Color::yellow: + return autoware_perception_msgs::msg::TrafficSignalElement::AMBER; + case Color::red: + return autoware_perception_msgs::msg::TrafficSignalElement::RED; + case Color::white: + return autoware_perception_msgs::msg::TrafficSignalElement::WHITE; + default: + throw common::SyntaxError(std::get(value), " is not supported color."); + } + }; + + auto status = [this]() { + switch (std::get(value).value) { + case Status::solid_on: + return autoware_perception_msgs::msg::TrafficSignalElement::SOLID_ON; + case Status::solid_off: + return autoware_perception_msgs::msg::TrafficSignalElement::SOLID_OFF; + case Status::flashing: + return autoware_perception_msgs::msg::TrafficSignalElement::FLASHING; + default: + throw common::SyntaxError(std::get(value), " is not supported color."); + } + }; + + auto shape = [this]() { + switch (std::get(value).value) { + case Shape::circle: + return autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE; + case Shape::cross: + return autoware_perception_msgs::msg::TrafficSignalElement::CROSS; + case Shape::left: + return autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW; + case Shape::down: + return autoware_perception_msgs::msg::TrafficSignalElement::DOWN_ARROW; + case Shape::up: + return autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW; + case Shape::right: + return autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW; + case Shape::lower_left: + return autoware_perception_msgs::msg::TrafficSignalElement::DOWN_LEFT_ARROW; + case Shape::lower_right: + return autoware_perception_msgs::msg::TrafficSignalElement::DOWN_RIGHT_ARROW; + case Shape::upper_left: + return autoware_perception_msgs::msg::TrafficSignalElement::UP_LEFT_ARROW; + case Shape::upper_right: + return autoware_perception_msgs::msg::TrafficSignalElement::UP_RIGHT_ARROW; + default: + throw common::SyntaxError(std::get(value), " is not supported color."); + }; + }; + + autoware_perception_msgs::msg::TrafficSignalElement msg; + msg.color = color(); + msg.status = status(); + msg.shape = shape(); + // NOTE: confidence will be overwritten + msg.confidence = 1.0; + return msg; + } }; explicit TrafficLight(const lanelet::Id, const hdmap_utils::HdMapUtils &); @@ -402,6 +537,38 @@ struct TrafficLight } return traffic_signal_proto; } + + explicit operator autoware_auto_perception_msgs::msg::TrafficSignal() const + { + autoware_auto_perception_msgs::msg::TrafficSignal traffic_signal; + traffic_signal.map_primitive_id = way_id; + for (const auto & bulb : bulbs) { + auto traffic_light_bulb = static_cast(bulb); + traffic_light_bulb.confidence = confidence; + traffic_signal.lights.push_back(traffic_light_bulb); + } + return traffic_signal; + } + + explicit operator std::vector() const + { + // skip if the traffic light has no bulbs + if (bulbs.empty()) { + return {}; + } else { + std::vector traffic_signals; + for (const auto & regulatory_element : regulatory_elements_ids) { + autoware_perception_msgs::msg::TrafficSignal traffic_signal; + traffic_signal.traffic_signal_id = regulatory_element; + for (const auto & bulb : bulbs) { + traffic_signal.elements.push_back( + static_cast(bulb)); + traffic_signals.push_back(traffic_signal); + } + } + return traffic_signals; + } + } }; } // namespace traffic_simulator From 2ac527fd0c58154b3de32085923c6b0994aa8e59 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 20 Jun 2024 18:43:53 +0200 Subject: [PATCH 07/45] feat(traffic_lights): develop individual traffic_lights_publisher for simple_sensor_simulator --- .../simple_sensor_simulator/CMakeLists.txt | 6 +- .../traffic_lights_detector.hpp | 18 ++--- .../traffic_lights_publisher.hpp | 56 ++++++++++++++ .../traffic_lights_publisher.cpp | 76 +++++++++++++++++++ 4 files changed, 145 insertions(+), 11 deletions(-) create mode 100644 simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp create mode 100644 simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index df6f898805f..100648bb1d0 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -36,12 +36,14 @@ ament_auto_add_library(simple_sensor_simulator_component SHARED src/sensor_simulation/detection_sensor/detection_sensor.cpp src/sensor_simulation/lidar/lidar_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/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 9a9e93d8eca..7b76b22aa14 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,10 +16,9 @@ #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_ #include +#include #include #include -#include -#include namespace simple_sensor_simulator { @@ -49,7 +48,7 @@ class TrafficLightsDetector private: template auto makePublisher(NodeType & node, const std::string & architecture_type) - -> std::unique_ptr + -> std::unique_ptr { /* V2ITrafficLights in TrafficSimulator publishes using topics "/v2x/traffic_signals" and @@ -57,19 +56,20 @@ 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"); } else { - throw common::SemanticError( - "Unexpected architecture_type ", std::quoted(architecture_type), " given."); + std::stringstream ss; + ss << "Unexpected architecture_type " << std::quoted(architecture_type) << " given."; + 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 new file mode 100644 index 00000000000..f79a38e4c7b --- /dev/null +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp @@ -0,0 +1,56 @@ +// Copyright 2024 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__TTRAFFIC_LIGHT_PUBLISHER_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TTRAFFIC_LIGHT_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__TTRAFFIC_LIGHT_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 new file mode 100644 index 00000000000..5cab1aa4c0f --- /dev/null +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp @@ -0,0 +1,76 @@ +// Copyright 2024 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 980781b79bf7cf92585426a8dea1869ee6859773 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 20 Jun 2024 18:54:59 +0200 Subject: [PATCH 08/45] feat(traffic_lights): improve traffic_lights_publisher in traffic_simulator --- .../traffic_light_publisher.hpp | 27 ++++---- .../traffic_lights/traffic_lights.hpp | 26 +++++--- .../traffic_lights/traffic_lights_base.hpp | 2 +- .../traffic_light_publisher.cpp | 61 +++++++------------ .../traffic_lights/traffic_lights_base.cpp | 8 --- 5 files changed, 58 insertions(+), 66 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 0c8fb4a9e79..27377af6ce2 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 @@ -20,34 +20,39 @@ #include #include #include +#include namespace traffic_simulator { - -class TrafficLightPublisherBase +class TrafficLightsPublisherBase { public: virtual auto publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void = 0; + const std::unordered_map & traffic_lights_map) const -> void = 0; }; + template -class TrafficLightPublisher : public TrafficLightPublisherBase +class TrafficLightsPublisher : public TrafficLightsPublisherBase { public: template - explicit TrafficLightPublisher(const NodeTypePointer & node, const std::string & topic_name) - : TrafficLightPublisherBase(), + explicit TrafficLightsPublisher( + const NodeTypePointer & node_ptr, const std::string & topic_name, + const std::string & frame = "camera_link") + : TrafficLightsPublisherBase(), + frame_(frame), + clock_ptr_(node_ptr->get_clock()), traffic_light_state_array_publisher_( - rclcpp::create_publisher(node, topic_name, rclcpp::QoS(10).transient_local())) + rclcpp::create_publisher(node_ptr, topic_name, rclcpp::QoS(10).transient_local())) { } - auto publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void override; + auto publish(const std::unordered_map & traffic_lights_map) const + -> void override; private: + const std::string frame_; + const rclcpp::Clock::SharedPtr clock_ptr_; const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; }; } // namespace traffic_simulator 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 f1af7615053..b359431466a 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 @@ -31,6 +31,15 @@ class ConventionalTrafficLights : public TrafficLightsBase : TrafficLightsBase(node_ptr, hdmap_utils) { } + +private: + auto update() const -> void + { + if (isAnyTrafficLightChanged()) { + marker_publisher_ptr_->deleteMarkers(); + } + marker_publisher_ptr_->drawMarkers(traffic_lights_map_); + } }; class V2ITrafficLights : public TrafficLightsBase @@ -51,15 +60,18 @@ class V2ITrafficLights : public TrafficLightsBase private: auto update() const -> void override { - TrafficLightsBase::update(); - publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); - legacy_topic_publisher_ptr_->publish(clock_ptr_->now(), generateUpdateTrafficLightsRequest()); + publisher_ptr_->publish(traffic_lights_map_); + legacy_topic_publisher_ptr_->publish(traffic_lights_map_); + if (isAnyTrafficLightChanged()) { + marker_publisher_ptr_->deleteMarkers(); + } + marker_publisher_ptr_->drawMarkers(traffic_lights_map_); } template auto makePublisher( const NodeTypePointer & node_ptr, const std::string & architecture_type, - const std::string & topic_name) -> std::unique_ptr + const std::string & topic_name) -> std::unique_ptr { /* Here autoware_perception_msgs is used for all awf/universe/.... @@ -71,7 +83,7 @@ class V2ITrafficLights : public TrafficLightsBase */ if (architecture_type.find("awf/universe") != std::string::npos) { return std::make_unique< - TrafficLightPublisher>( + TrafficLightsPublisher>( node_ptr, topic_name); } else { throw common::SemanticError( @@ -80,8 +92,8 @@ class V2ITrafficLights : public TrafficLightsBase } } - const std::unique_ptr publisher_ptr_; - const std::unique_ptr legacy_topic_publisher_ptr_; + const std::unique_ptr publisher_ptr_; + const std::unique_ptr legacy_topic_publisher_ptr_; }; class TrafficLights 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 3a0bfc9644c..cfb850a53df 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 @@ -71,7 +71,7 @@ class TrafficLightsBase -> simulation_api_schema::UpdateTrafficLightsRequest; protected: - virtual auto update() const -> void; + virtual auto update() const -> void = 0; auto isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool; 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 cd261dcaed0..0c9e06e0742 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -19,54 +19,37 @@ namespace traffic_simulator { template <> -auto TrafficLightPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void +auto TrafficLightsPublisher::publish( + const std::unordered_map & traffic_lights_map) 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_lights_message.header.frame_id = frame_; + traffic_lights_message.header.stamp = clock_ptr_->now(); + for (const auto & [id, traffic_light] : traffic_lights_map) { + traffic_lights_message.signals.push_back( + static_cast(traffic_light)); } traffic_light_state_array_publisher_->publish(traffic_lights_message); } template <> -auto TrafficLightPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void +auto TrafficLightsPublisher::publish( + const std::unordered_map & traffic_lights_map) 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_lights_message.stamp = clock_ptr_->now(); + + size_t total_number_of_signals{0}; + for (const auto & [id, traffic_light] : traffic_lights_map) { + total_number_of_signals += traffic_light.regulatory_elements_ids.size(); + } + traffic_lights_message.signals.reserve(total_number_of_signals); + + for (const auto & [id, traffic_light] : traffic_lights_map) { + const auto traffic_signals = + static_cast>(traffic_light); + traffic_lights_message.signals.insert( + traffic_lights_message.signals.end(), traffic_signals.begin(), traffic_signals.end()); } traffic_light_state_array_publisher_->publish(traffic_lights_message); } 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 f513c703e73..721a7dd68a2 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -105,14 +105,6 @@ auto TrafficLightsBase::generateUpdateTrafficLightsRequest() const } // private -auto TrafficLightsBase::update() const -> void -{ - if (isAnyTrafficLightChanged()) { - marker_publisher_ptr_->deleteMarkers(); - } - marker_publisher_ptr_->drawMarkers(traffic_lights_map_); -} - auto TrafficLightsBase::isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool { return traffic_lights_map_.find(traffic_light_id) != traffic_lights_map_.end(); From e74b3646e8d07ee4a173a64fa29c0fa3cc670919 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 20 Jun 2024 19:51:42 +0200 Subject: [PATCH 09/45] feat(traffic_lights): add converions for autoware_auto_perception_msgs --- .../simulation_interface/conversions.hpp | 18 ++++ simulation/simulation_interface/package.xml | 1 + .../simulation_interface/src/conversions.cpp | 94 +++++++++++++++++++ .../traffic_lights/traffic_light.hpp | 6 ++ .../traffic_light_publisher.hpp | 4 +- .../traffic_lights/traffic_lights_base.hpp | 3 + 6 files changed, 124 insertions(+), 2 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4ec8320616b..4d1110b6fcd 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -23,6 +23,9 @@ #include #include +#include +#include +#include #include #include #include @@ -185,6 +188,21 @@ auto toProto( autoware_auto_vehicle_msgs::msg::GearCommand> &, traffic_simulator_msgs::VehicleCommand &) -> void; +// now unused since autoware_perception_msgs::msg::TrafficSignal is still supported +auto toProto( + const autoware_auto_perception_msgs::msg::TrafficLight & message, + simulation_api_schema::TrafficLight & proto) -> void; + +// now unused since autoware_perception_msgs::msg::TrafficSignal is still supported +auto toProto( + const autoware_auto_perception_msgs::msg::TrafficSignal & message, + simulation_api_schema::TrafficSignal & proto) -> void; + +// now unused since autoware_perception_msgs::msg::TrafficSignal is still supported +auto toProto( + const autoware_auto_perception_msgs::msg::TrafficSignalArray & message, + simulation_api_schema::UpdateTrafficLightsRequest & proto) -> void; + template auto toMsg( const simulation_api_schema::TrafficLight & proto, diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 7cab5a458e3..32fb9ec17ef 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -12,6 +12,7 @@ autoware_auto_control_msgs autoware_auto_vehicle_msgs + autoware_auto_perception_msgs boost builtin_interfaces geometry_msgs diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index 1804eb11c1a..1590c176feb 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -603,6 +603,100 @@ auto toProto( toProto(std::get<1>(message), *proto.mutable_gear_command()); } +auto toProto( + const autoware_auto_perception_msgs::msg::TrafficLight & message, + simulation_api_schema::TrafficLight & proto) -> void +{ + auto color = [&]() { + switch (message.color) { + case autoware_auto_perception_msgs::msg::TrafficLight::GREEN: + return simulation_api_schema::TrafficLight_Color_GREEN; + case autoware_auto_perception_msgs::msg::TrafficLight::AMBER: + return simulation_api_schema::TrafficLight_Color_AMBER; + case autoware_auto_perception_msgs::msg::TrafficLight::RED: + return simulation_api_schema::TrafficLight_Color_RED; + case autoware_auto_perception_msgs::msg::TrafficLight::WHITE: + return simulation_api_schema::TrafficLight_Color_WHITE; + default: + return simulation_api_schema::TrafficLight_Color_UNKNOWN_COLOR; + // + // throw THROW_SIMULATION_ERROR(message.color, " is not supported as a status."); + } + }; + + auto status = [&]() { + switch (message.status) { + case autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON: + return simulation_api_schema::TrafficLight_Status_SOLID_ON; + case autoware_auto_perception_msgs::msg::TrafficLight::SOLID_OFF: + return simulation_api_schema::TrafficLight_Status_SOLID_OFF; + case autoware_auto_perception_msgs::msg::TrafficLight::FLASHING: + return simulation_api_schema::TrafficLight_Status_FLASHING; + default: + return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; + // default: + // throw THROW_SIMULATION_ERROR(message.status, " is not supported as a status."); + } + }; + + auto shape = [&]() { + switch (message.shape) { + case autoware_auto_perception_msgs::msg::TrafficLight::CIRCLE: + return simulation_api_schema::TrafficLight_Shape_CIRCLE; + case autoware_auto_perception_msgs::msg::TrafficLight::CROSS: + return simulation_api_schema::TrafficLight_Shape_CROSS; + case autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW: + return simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; + case autoware_auto_perception_msgs::msg::TrafficLight::DOWN_ARROW: + return simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; + case autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW: + return simulation_api_schema::TrafficLight_Shape_UP_ARROW; + case autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW: + return simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; + case autoware_auto_perception_msgs::msg::TrafficLight::DOWN_LEFT_ARROW: + return simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; + case autoware_auto_perception_msgs::msg::TrafficLight::DOWN_RIGHT_ARROW: + return simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; + case autoware_auto_perception_msgs::msg::TrafficLight::UP_LEFT_ARROW: + return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; + case autoware_auto_perception_msgs::msg::TrafficLight::UP_RIGHT_ARROW: + return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; + default: + return simulation_api_schema::TrafficLight_Shape_UNKNOWN_SHAPE; + } + }; + + proto.set_status(status()); + proto.set_shape(shape()); + proto.set_color(color()); + proto.set_confidence(message.confidence); +} + +auto toProto( + const autoware_auto_perception_msgs::msg::TrafficSignal & message, + simulation_api_schema::TrafficSignal & proto) -> void +{ + // here is a lack of information in autoware_auto_perception_msgs::msg + // to complete relation_ids it in simulation_api_schema::TrafficSignal + proto.set_id(message.map_primitive_id); + for (const auto & traffic_light : message.lights) { + simulation_api_schema::TrafficLight traffic_light_proto; + toProto(traffic_light, traffic_light_proto); + *proto.add_traffic_light_status() = traffic_light_proto; + } +} + +auto toProto( + const autoware_auto_perception_msgs::msg::TrafficSignalArray & message, + simulation_api_schema::UpdateTrafficLightsRequest & proto) -> void +{ + for (const auto & traffic_signal : message.signals) { + simulation_api_schema::TrafficSignal traffic_signal_proto; + toProto(traffic_signal, traffic_signal_proto); + *proto.add_states() = traffic_signal_proto; + } +} + auto toProtobufMessage(const traffic_simulator_msgs::msg::Vertex & message) -> traffic_simulator_msgs::Vertex { diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index 430ebd49cdd..5ed9af398c0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -253,6 +253,7 @@ struct TrafficLight friend auto operator<<(std::ostream & os, const Bulb & bulb) -> std::ostream &; + // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported explicit operator simulation_api_schema::TrafficLight() const { auto color = [this]() { @@ -388,6 +389,7 @@ struct TrafficLight return msg; } + // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported explicit operator autoware_perception_msgs::msg::TrafficSignalElement() const { auto color = [this]() { @@ -522,6 +524,9 @@ struct TrafficLight friend auto operator<<(std::ostream & os, const TrafficLight & traffic_light) -> std::ostream &; + // simulation_api_schema should not occur here, but it is necessary to transfer + // "relation_ids" in proto - which is not needed when autoware_auto_perception_msgs::msg::TrafficSignal is used + // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported explicit operator simulation_api_schema::TrafficSignal() const { simulation_api_schema::TrafficSignal traffic_signal_proto; @@ -550,6 +555,7 @@ struct TrafficLight return traffic_signal; } + // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported explicit operator std::vector() const { // skip if the traffic light has no bulbs 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 27377af6ce2..8e52eee4f4f 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 @@ -42,8 +42,8 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase : TrafficLightsPublisherBase(), 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())) + traffic_light_state_array_publisher_(rclcpp::create_publisher( + node_ptr, topic_name, rclcpp::QoS(10).transient_local())) { } 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 cfb850a53df..9fbb2e9e93f 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 @@ -67,6 +67,9 @@ class TrafficLightsBase auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string; + // simulation_api_schema should not occur here, but it is necessary to transfer + // "relation_ids" in proto - which is not needed when autoware_auto_perception_msgs::msg::TrafficSignal is used + // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported auto generateUpdateTrafficLightsRequest() const -> simulation_api_schema::UpdateTrafficLightsRequest; From 87bcb80e204684c476e64c9db6fa5a2bb78e6313 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 20 Jun 2024 20:03:36 +0200 Subject: [PATCH 10/45] ref(traffic_lights): rename rest of TrafficLight**->TrafficLights** --- simulation/traffic_simulator/CMakeLists.txt | 14 +++++++------- .../traffic_simulator/entity/entity_manager.hpp | 4 ++-- .../traffic_lights/traffic_lights.hpp | 2 +- .../traffic_lights/traffic_lights_base.hpp | 6 +++--- ...her.hpp => traffic_lights_marker_publisher.hpp} | 10 +++++----- ..._publisher.hpp => traffic_lights_publisher.hpp} | 6 +++--- ...her.cpp => traffic_lights_marker_publisher.cpp} | 6 +++--- ..._publisher.cpp => traffic_lights_publisher.cpp} | 2 +- 8 files changed, 25 insertions(+), 25 deletions(-) rename simulation/traffic_simulator/include/traffic_simulator/traffic_lights/{traffic_light_marker_publisher.hpp => traffic_lights_marker_publisher.hpp} (81%) rename simulation/traffic_simulator/include/traffic_simulator/traffic_lights/{traffic_light_publisher.hpp => traffic_lights_publisher.hpp} (89%) rename simulation/traffic_simulator/src/traffic_lights/{traffic_light_marker_publisher.cpp => traffic_lights_marker_publisher.cpp} (87%) rename simulation/traffic_simulator/src/traffic_lights/{traffic_light_publisher.cpp => traffic_lights_publisher.cpp} (97%) diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 638ddd73fcf..6b9e1b9b441 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -45,18 +45,18 @@ ament_auto_add_library(traffic_simulator SHARED src/entity/vehicle_entity.cpp src/hdmap_utils/hdmap_utils.cpp src/helper/helper.cpp - src/job/job_list.cpp src/job/job.cpp + src/job/job_list.cpp src/simulation_clock/simulation_clock.cpp - src/traffic_lights/configurable_rate_updater.cpp - src/traffic_lights/traffic_light_marker_publisher.cpp - src/traffic_lights/traffic_light_publisher.cpp - src/traffic_lights/traffic_light.cpp - src/traffic_lights/traffic_lights_base.cpp - src/traffic_lights/traffic_lights.cpp src/traffic/traffic_controller.cpp src/traffic/traffic_sink.cpp src/traffic/traffic_source.cpp + src/traffic_lights/configurable_rate_updater.cpp + src/traffic_lights/traffic_light.cpp + src/traffic_lights/traffic_lights.cpp + src/traffic_lights/traffic_lights_base.cpp + src/traffic_lights/traffic_lights_marker_publisher.cpp + src/traffic_lights/traffic_lights_publisher.cpp src/utils/distance.cpp src/utils/pose.cpp ) 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 2f344a4af8d..33acbb1995b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -40,9 +40,9 @@ #include #include #include -#include -#include #include +#include +#include #include #include #include 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 b359431466a..963da3aaf0a 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,8 +17,8 @@ #include #include -#include #include +#include namespace traffic_simulator { 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 9fbb2e9e93f..4798e32a897 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 @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include @@ -42,7 +42,7 @@ class TrafficLightsBase : hdmap_utils_(hdmap_utils), clock_ptr_(node_ptr->get_clock()), rate_updater_(node_ptr, [this]() { update(); }), - marker_publisher_ptr_(std::make_unique(node_ptr)) + marker_publisher_ptr_(std::make_unique(node_ptr)) { } @@ -89,7 +89,7 @@ class TrafficLightsBase const rclcpp::Clock::SharedPtr clock_ptr_; std::unordered_map traffic_lights_map_; - const std::unique_ptr marker_publisher_ptr_; + const std::unique_ptr marker_publisher_ptr_; ConfigurableRateUpdater rate_updater_; }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_marker_publisher.hpp similarity index 81% rename from simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp rename to simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_marker_publisher.hpp index b19d3fd6333..6ae4708cf3f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_marker_publisher.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP -#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP +#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_MARKER_PUBLISHER_HPP +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_MARKER_PUBLISHER_HPP #include #include namespace traffic_simulator { -class TrafficLightMarkerPublisher +class TrafficLightsMarkerPublisher { public: template - explicit TrafficLightMarkerPublisher( + explicit TrafficLightsMarkerPublisher( const NodeTypePointer & node_ptr, const std::string & frame = "map") : frame_(frame), clock_ptr_(node_ptr->get_clock()), @@ -44,4 +44,4 @@ class TrafficLightMarkerPublisher const rclcpp::Publisher::SharedPtr publisher_; }; } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_MARKER_PUBLISHER_HPP 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_lights_publisher.hpp similarity index 89% rename from simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp rename to simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp index 8e52eee4f4f..f5a8adde9a4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ -#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ +#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ #include #include @@ -56,4 +56,4 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; }; } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_marker_publisher.cpp similarity index 87% rename from simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp rename to simulation/traffic_simulator/src/traffic_lights/traffic_lights_marker_publisher.cpp index 225008624f7..41e5d10a44b 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_marker_publisher.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include namespace traffic_simulator { -auto TrafficLightMarkerPublisher::deleteMarkers() const -> void +auto TrafficLightsMarkerPublisher::deleteMarkers() const -> void { visualization_msgs::msg::MarkerArray marker_array; visualization_msgs::msg::Marker marker; @@ -26,7 +26,7 @@ auto TrafficLightMarkerPublisher::deleteMarkers() const -> void publisher_->publish(marker_array); } -auto TrafficLightMarkerPublisher::drawMarkers( +auto TrafficLightsMarkerPublisher::drawMarkers( const std::unordered_map & traffic_lights_map) const -> void { visualization_msgs::msg::MarkerArray marker_array; diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp similarity index 97% rename from simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp rename to simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp index 0c9e06e0742..77b1d485e2f 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include namespace traffic_simulator { From 2ed3d2a21b27d30104ac9f1d0bfe77a8188447d4 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Fri, 21 Jun 2024 10:00:22 +0200 Subject: [PATCH 11/45] Remove compile error - reorder Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/traffic_lights/traffic_lights_base.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 4798e32a897..f848e2d1385 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 @@ -41,8 +41,8 @@ class TrafficLightsBase const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) : hdmap_utils_(hdmap_utils), clock_ptr_(node_ptr->get_clock()), - rate_updater_(node_ptr, [this]() { update(); }), - marker_publisher_ptr_(std::make_unique(node_ptr)) + marker_publisher_ptr_(std::make_unique(node_ptr)), + rate_updater_(node_ptr, [this]() { update(); }) { } From 95a1b7ae629192ff0623b9009b26c758556ee18d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Fri, 21 Jun 2024 10:00:49 +0200 Subject: [PATCH 12/45] Remove unused variables Signed-off-by: Mateusz Palczuk --- simulation/behavior_tree_plugin/src/action_node.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 8ca0574c26f..c5a6220f5d8 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -195,9 +195,6 @@ auto ActionNode::getDistanceToTrafficLightStopLine( !traffic_light_ids.empty()) { std::set collision_points = {}; for (const auto traffic_light_id : traffic_light_ids) { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; if (traffic_lights->isRequiredStopTrafficLightState(traffic_light_id)) { if ( const auto collision_point = From b0aca76a102c5b04dff5c13016d3d35d2e2527df Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 21 Jun 2024 12:21:16 +0200 Subject: [PATCH 13/45] feat(traffic_lights): use conversions instead operator() overlaod, remove overloads, improve publisher --- simulation/do_nothing_plugin/src/plugin.cpp | 2 + .../simulation_interface/conversions.hpp | 3 - .../simulation_interface/src/conversions.cpp | 2 +- .../traffic_lights/traffic_light.hpp | 90 ------------------- .../traffic_lights/traffic_lights.hpp | 4 +- .../traffic_lights/traffic_lights_base.hpp | 11 +-- .../traffic_lights_publisher.hpp | 8 +- simulation/traffic_simulator/src/api/api.cpp | 18 +++- .../src/entity/entity_manager.cpp | 1 + .../traffic_lights/traffic_lights_base.cpp | 37 ++++++-- .../traffic_lights_publisher.cpp | 30 +------ 11 files changed, 64 insertions(+), 142 deletions(-) diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp index 9f5597e3f7d..278d43de5d1 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -16,6 +16,8 @@ #include #include #include +#include +#include namespace entity_behavior { diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4d1110b6fcd..15f95868e72 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -188,17 +188,14 @@ auto toProto( autoware_auto_vehicle_msgs::msg::GearCommand> &, traffic_simulator_msgs::VehicleCommand &) -> void; -// now unused since autoware_perception_msgs::msg::TrafficSignal is still supported auto toProto( const autoware_auto_perception_msgs::msg::TrafficLight & message, simulation_api_schema::TrafficLight & proto) -> void; -// now unused since autoware_perception_msgs::msg::TrafficSignal is still supported auto toProto( const autoware_auto_perception_msgs::msg::TrafficSignal & message, simulation_api_schema::TrafficSignal & proto) -> void; -// now unused since autoware_perception_msgs::msg::TrafficSignal is still supported auto toProto( const autoware_auto_perception_msgs::msg::TrafficSignalArray & message, simulation_api_schema::UpdateTrafficLightsRequest & proto) -> void; diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index 1590c176feb..7e4249e6bb6 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -677,7 +677,7 @@ auto toProto( simulation_api_schema::TrafficSignal & proto) -> void { // here is a lack of information in autoware_auto_perception_msgs::msg - // to complete relation_ids it in simulation_api_schema::TrafficSignal + // to complete relation_ids in simulation_api_schema::TrafficSignal proto.set_id(message.map_primitive_id); for (const auto & traffic_light : message.lights) { simulation_api_schema::TrafficLight traffic_light_proto; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index 5ed9af398c0..ff65d6c1820 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -28,7 +28,6 @@ #include #include #include -#include #include #include #include @@ -253,76 +252,6 @@ struct TrafficLight friend auto operator<<(std::ostream & os, const Bulb & bulb) -> std::ostream &; - // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported - explicit operator simulation_api_schema::TrafficLight() const - { - auto color = [this]() { - switch (std::get(value).value) { - case Color::green: - return simulation_api_schema::TrafficLight_Color_GREEN; - case Color::yellow: - return simulation_api_schema::TrafficLight_Color_AMBER; - case Color::red: - return simulation_api_schema::TrafficLight_Color_RED; - case Color::white: - return simulation_api_schema::TrafficLight_Color_WHITE; - default: - throw common::SyntaxError(std::get(value), " is not supported color."); - } - }; - - auto status = [this]() { - switch (std::get(value).value) { - case Status::solid_on: - return simulation_api_schema::TrafficLight_Status_SOLID_ON; - case Status::solid_off: - return simulation_api_schema::TrafficLight_Status_SOLID_OFF; - case Status::flashing: - return simulation_api_schema::TrafficLight_Status_FLASHING; - case Status::unknown: - return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; - default: - throw common::SyntaxError(std::get(value), " is not supported as a status."); - } - }; - - auto shape = [this]() { - switch (std::get(value).value) { - case Shape::circle: - return simulation_api_schema::TrafficLight_Shape_CIRCLE; - case Shape::cross: - return simulation_api_schema::TrafficLight_Shape_CROSS; - case Shape::left: - return simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; - case Shape::down: - return simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; - case Shape::up: - return simulation_api_schema::TrafficLight_Shape_UP_ARROW; - case Shape::right: - return simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; - case Shape::lower_left: - return simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; - case Shape::lower_right: - return simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; - case Shape::upper_left: - return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; - case Shape::upper_right: - return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; - default: - throw common::SyntaxError(std::get(value), " is not supported as a shape."); - } - }; - - simulation_api_schema::TrafficLight traffic_light_bulb_proto; - traffic_light_bulb_proto.set_status(status()); - traffic_light_bulb_proto.set_shape(shape()); - traffic_light_bulb_proto.set_color(color()); - // NOTE: confidence will be overwritten in TrafficLight::operator simulation_api_schema::TrafficSignal() - traffic_light_bulb_proto.set_confidence(1.0); - - return traffic_light_bulb_proto; - } - explicit operator autoware_auto_perception_msgs::msg::TrafficLight() const { auto color = [this]() { @@ -524,25 +453,6 @@ struct TrafficLight friend auto operator<<(std::ostream & os, const TrafficLight & traffic_light) -> std::ostream &; - // simulation_api_schema should not occur here, but it is necessary to transfer - // "relation_ids" in proto - which is not needed when autoware_auto_perception_msgs::msg::TrafficSignal is used - // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported - explicit operator simulation_api_schema::TrafficSignal() const - { - simulation_api_schema::TrafficSignal traffic_signal_proto; - - traffic_signal_proto.set_id(way_id); - for (const auto relation_id : regulatory_elements_ids) { - traffic_signal_proto.add_relation_ids(relation_id); - } - for (const auto & bulb : bulbs) { - auto traffic_light_bulb_proto = static_cast(bulb); - traffic_light_bulb_proto.set_confidence(confidence); - *traffic_signal_proto.add_traffic_light_status() = traffic_light_bulb_proto; - } - return traffic_signal_proto; - } - explicit operator autoware_auto_perception_msgs::msg::TrafficSignal() const { autoware_auto_perception_msgs::msg::TrafficSignal traffic_signal; 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 963da3aaf0a..2075a8140ea 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 @@ -60,8 +60,8 @@ class V2ITrafficLights : public TrafficLightsBase private: auto update() const -> void override { - publisher_ptr_->publish(traffic_lights_map_); - legacy_topic_publisher_ptr_->publish(traffic_lights_map_); + publisher_ptr_->publish(*this); + legacy_topic_publisher_ptr_->publish(*this); 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 4798e32a897..b2055283a42 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 @@ -15,6 +15,8 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ +#include +#include #include #include #include @@ -67,11 +69,10 @@ class TrafficLightsBase auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string; - // simulation_api_schema should not occur here, but it is necessary to transfer - // "relation_ids" in proto - which is not needed when autoware_auto_perception_msgs::msg::TrafficSignal is used - // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported - auto generateUpdateTrafficLightsRequest() const - -> simulation_api_schema::UpdateTrafficLightsRequest; + auto generateAutowarePerceptionMsg() const -> autoware_perception_msgs::msg::TrafficSignalArray; + + auto generateAutowareAutoPerceptionMsg() const + -> autoware_auto_perception_msgs::msg::TrafficSignalArray; protected: virtual auto update() const -> void = 0; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp index f5a8adde9a4..b3005735a99 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp @@ -17,18 +17,17 @@ #include #include -#include #include #include #include +#include namespace traffic_simulator { class TrafficLightsPublisherBase { public: - virtual auto publish( - const std::unordered_map & traffic_lights_map) const -> void = 0; + virtual auto publish(const TrafficLightsBase & traffic_lights) const -> void = 0; }; template @@ -47,8 +46,7 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase { } - auto publish(const std::unordered_map & traffic_lights_map) const - -> void override; + auto publish(const TrafficLightsBase & traffic_lights) const -> void override; private: const std::string frame_; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index b36884104e6..f4ccaa728a8 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -270,9 +270,21 @@ bool API::updateTimeInSim() bool API::updateTrafficLightsInSim() { if (traffic_lights_ptr_->isAnyTrafficLightChanged()) { - auto req = - traffic_lights_ptr_->getConventionalTrafficLights()->generateUpdateTrafficLightsRequest(); - return zeromq_client_.call(req).result().success(); + simulation_api_schema::UpdateTrafficLightsRequest request; + const auto traffic_lights_msg = + traffic_lights_ptr_->getConventionalTrafficLights()->generateAutowareAutoPerceptionMsg(); + simulation_interface::toProto(traffic_lights_msg, request); + // here is a lack of information in autoware_auto_perception_msgs::msg + // to complete the relation_ids, so complete it manually here + for (auto & traffic_signal : *request.mutable_states()) { + const auto relation_ids = + entity_manager_ptr_->getHdmapUtils()->getTrafficLightRegulatoryElementIDsFromTrafficLight( + traffic_signal.id()); + for (const auto & relation_id : relation_ids) { + traffic_signal.add_relation_ids(relation_id); + } + } + return zeromq_client_.call(request).result().success(); } /// @todo handle response return simulation_api_schema::UpdateTrafficLightsResponse().result().success(); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 490edad772b..5e18b732247 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include 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 721a7dd68a2..836bbad60d3 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -93,15 +93,38 @@ auto TrafficLightsBase::getTrafficLightsComposedState(const lanelet::Id lanelet_ return ss.str(); } -auto TrafficLightsBase::generateUpdateTrafficLightsRequest() const - -> simulation_api_schema::UpdateTrafficLightsRequest +auto TrafficLightsBase::generateAutowarePerceptionMsg() const + -> autoware_perception_msgs::msg::TrafficSignalArray { - simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; - for (auto && [lanelet_id, traffic_light] : traffic_lights_map_) { - *update_traffic_lights_request.add_states() = - static_cast(traffic_light); + autoware_perception_msgs::msg::TrafficSignalArray traffic_lights_message; + traffic_lights_message.stamp = clock_ptr_->now(); + + size_t total_number_of_signals{0}; + for (const auto & [id, traffic_light] : traffic_lights_map_) { + total_number_of_signals += traffic_light.regulatory_elements_ids.size(); + } + traffic_lights_message.signals.reserve(total_number_of_signals); + + for (const auto & [id, traffic_light] : traffic_lights_map_) { + const auto traffic_signals = + static_cast>(traffic_light); + traffic_lights_message.signals.insert( + traffic_lights_message.signals.end(), traffic_signals.begin(), traffic_signals.end()); + } + return traffic_lights_message; +} + +auto TrafficLightsBase::generateAutowareAutoPerceptionMsg() const + -> autoware_auto_perception_msgs::msg::TrafficSignalArray +{ + autoware_auto_perception_msgs::msg::TrafficSignalArray traffic_lights_message; + traffic_lights_message.header.frame_id = "camera_link"; /// DIRTY HACK! + traffic_lights_message.header.stamp = clock_ptr_->now(); + for (const auto & [id, traffic_light] : traffic_lights_map_) { + traffic_lights_message.signals.push_back( + static_cast(traffic_light)); } - return update_traffic_lights_request; + return traffic_lights_message; } // private diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp index 77b1d485e2f..1dc8de58cd5 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp @@ -20,37 +20,15 @@ namespace traffic_simulator { template <> auto TrafficLightsPublisher::publish( - const std::unordered_map & traffic_lights_map) const -> void + const TrafficLightsBase & traffic_lights) const -> void { - autoware_auto_perception_msgs::msg::TrafficSignalArray traffic_lights_message; - traffic_lights_message.header.frame_id = frame_; - traffic_lights_message.header.stamp = clock_ptr_->now(); - for (const auto & [id, traffic_light] : traffic_lights_map) { - traffic_lights_message.signals.push_back( - static_cast(traffic_light)); - } - traffic_light_state_array_publisher_->publish(traffic_lights_message); + traffic_light_state_array_publisher_->publish(traffic_lights.generateAutowareAutoPerceptionMsg()); } template <> auto TrafficLightsPublisher::publish( - const std::unordered_map & traffic_lights_map) const -> void + const TrafficLightsBase & traffic_lights) const -> void { - autoware_perception_msgs::msg::TrafficSignalArray traffic_lights_message; - traffic_lights_message.stamp = clock_ptr_->now(); - - size_t total_number_of_signals{0}; - for (const auto & [id, traffic_light] : traffic_lights_map) { - total_number_of_signals += traffic_light.regulatory_elements_ids.size(); - } - traffic_lights_message.signals.reserve(total_number_of_signals); - - for (const auto & [id, traffic_light] : traffic_lights_map) { - const auto traffic_signals = - static_cast>(traffic_light); - traffic_lights_message.signals.insert( - traffic_lights_message.signals.end(), traffic_signals.begin(), traffic_signals.end()); - } - traffic_light_state_array_publisher_->publish(traffic_lights_message); + traffic_light_state_array_publisher_->publish(traffic_lights.generateAutowarePerceptionMsg()); } } // namespace traffic_simulator From fe5cd70d7e8b3c38330ef427a5a15ede8b78c03b Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 21 Jun 2024 12:26:36 +0200 Subject: [PATCH 14/45] ref(conversions): remove comments --- simulation/simulation_interface/src/conversions.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index 7e4249e6bb6..7b40b973ff6 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -619,8 +619,6 @@ auto toProto( return simulation_api_schema::TrafficLight_Color_WHITE; default: return simulation_api_schema::TrafficLight_Color_UNKNOWN_COLOR; - // - // throw THROW_SIMULATION_ERROR(message.color, " is not supported as a status."); } }; @@ -634,8 +632,6 @@ auto toProto( return simulation_api_schema::TrafficLight_Status_FLASHING; default: return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; - // default: - // throw THROW_SIMULATION_ERROR(message.status, " is not supported as a status."); } }; From 149d0a823888cdb076194885257f9ec9e729e1d2 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 21 Jun 2024 12:59:42 +0200 Subject: [PATCH 15/45] feat(traffic_lights): last code improvements --- .../include/traffic_simulator/api/api.hpp | 24 ++++--------------- .../entity/entity_manager.hpp | 14 +++++------ .../traffic_lights/traffic_light.hpp | 8 ++++--- 3 files changed, 16 insertions(+), 30 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index ceb4229e627..a4aea9f045b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -328,26 +328,12 @@ class API const bool allow_spawn_outside_lane = false, const bool require_footprint_fitting = false, const bool random_orientation = false, std::optional random_seed = std::nullopt) -> void; - // clang-format off -#define FORWARD_TO_TRAFFIC_LIGHTS(NAME) \ - /*! \ - @brief Forward to arguments to the TrafficLights::NAME function. \ - @return return value of the TrafficLights::NAME function. \ - @note This function was defined by FORWARD_TO_TRAFFIC_LIGHTS macro. \ - */ \ - template \ - decltype(auto) NAME(Ts &&... xs) \ - { \ - assert(traffic_lights_ptr_); \ - return (*traffic_lights_ptr_).NAME(std::forward(xs)...); \ - } \ - static_assert(true, "") - // clang-format on + auto getV2ITrafficLights() { return traffic_lights_ptr_->getV2ITrafficLights(); } - FORWARD_TO_TRAFFIC_LIGHTS(getV2ITrafficLights); - FORWARD_TO_TRAFFIC_LIGHTS(getConventionalTrafficLights); - -#undef FORWARD_TO_TRAFFIC_LIGHTS + auto getConventionalTrafficLights() + { + return traffic_lights_ptr_->getConventionalTrafficLights(); + } // clang-format off #define FORWARD_TO_ENTITY_MANAGER(NAME) \ 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 33acbb1995b..108ced95e6a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -112,16 +112,14 @@ class EntityManager public: /** - * This function is necessary, because TrafficLights requires HdMapUtils. - * However, TrafficLights is constructed in API and in that scope HdMapUtils is available - * only after EntityManager is constructed. - * This is why TrafficLights has to be initialized in API after EntityManager and thus - * TrafficLights needs to be passed to EntityManager after initialization. + This function is necessary because the TrafficLights object is created after the EntityManager, + so it can be assigned during the call of the EntityManager constructor. + TrafficLights cannot be created before the EntityManager due to the dependency on HdMapUtils. */ - auto setTrafficLights(const std::shared_ptr & traffic_lights) - -> void + auto setTrafficLights( + const std::shared_ptr & traffic_lights_ptr) -> void { - traffic_lights_ptr_ = traffic_lights; + traffic_lights_ptr_ = traffic_lights_ptr; } template diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index ff65d6c1820..1132fa6db04 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -477,10 +477,12 @@ struct TrafficLight autoware_perception_msgs::msg::TrafficSignal traffic_signal; traffic_signal.traffic_signal_id = regulatory_element; for (const auto & bulb : bulbs) { - traffic_signal.elements.push_back( - static_cast(bulb)); - traffic_signals.push_back(traffic_signal); + auto traffic_light_bulb = + static_cast(bulb); + traffic_light_bulb.confidence = confidence; + traffic_signal.elements.push_back(traffic_light_bulb); } + traffic_signals.push_back(traffic_signal); } return traffic_signals; } From 1fe2648a86090f4519407ecf261e9d281dfb7b81 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Fri, 21 Jun 2024 15:34:28 +0200 Subject: [PATCH 16/45] Add conventional traffic lights tests Signed-off-by: Mateusz Palczuk --- .../test/src/expect_eq_macros.hpp | 18 + .../test_traffic_light_manager.cpp | 381 ++++++++++++++---- 2 files changed, 328 insertions(+), 71 deletions(-) diff --git a/simulation/traffic_simulator/test/src/expect_eq_macros.hpp b/simulation/traffic_simulator/test/src/expect_eq_macros.hpp index c321e9b1a7d..42adee91cbe 100644 --- a/simulation/traffic_simulator/test/src/expect_eq_macros.hpp +++ b/simulation/traffic_simulator/test/src/expect_eq_macros.hpp @@ -59,6 +59,12 @@ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); +#define EXPECT_QUATERNION_EQ_STREAM(DATA0, DATA1, STREAM_MESSAGE) \ + EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x) << STREAM_MESSAGE; \ + EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y) << STREAM_MESSAGE; \ + EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z) << STREAM_MESSAGE; \ + EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w) << STREAM_MESSAGE; + #define EXPECT_QUATERNION_NEAR(DATA0, DATA1, EPS) \ EXPECT_NEAR(DATA0.x, DATA1.x, EPS); \ EXPECT_NEAR(DATA0.y, DATA1.y, EPS); \ @@ -130,4 +136,16 @@ EXPECT_EQ(DATA0.trajectory_shape, DATA1.trajectory_shape); \ EXPECT_LANE_CHANGE_CONSTRAINT_EQ(DATA0.constraint, DATA1.constraint); +#define EXPECT_COLOR_RGBA_EQ_STREAM(DATA0, DATA1, STREAM_MESSAGE) \ + EXPECT_DOUBLE_EQ(DATA0.r, DATA1.r) << STREAM_MESSAGE; \ + EXPECT_DOUBLE_EQ(DATA0.g, DATA1.g) << STREAM_MESSAGE; \ + EXPECT_DOUBLE_EQ(DATA0.b, DATA1.b) << STREAM_MESSAGE; \ + EXPECT_DOUBLE_EQ(DATA0.a, DATA1.a) << STREAM_MESSAGE; + +#define EXPECT_COLOR_RGBA_NEAR_STREAM(DATA0, DATA1, EPS, STREAM_MESSAGE) \ + EXPECT_NEAR(DATA0.r, DATA1.r, EPS) << STREAM_MESSAGE; \ + EXPECT_NEAR(DATA0.g, DATA1.g, EPS) << STREAM_MESSAGE; \ + EXPECT_NEAR(DATA0.b, DATA1.b, EPS) << STREAM_MESSAGE; \ + EXPECT_NEAR(DATA0.a, DATA1.a, EPS) << STREAM_MESSAGE; + #endif // TRAFFIC_SIMULATOR__TEST__EXPECT_EQ_MACROS_HPP_ diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index d8927125897..25de92087b7 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -16,79 +16,318 @@ #include #include +#include #include -// TEST(TrafficLightManager, getIds) -// { -// const auto node = std::make_shared("getIds"); -// std::string path = -// ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; -// geographic_msgs::msg::GeoPoint origin; -// origin.latitude = 35.61836750154; -// origin.longitude = 139.78066608243; -// const auto hdmap_utils_ptr = std::make_shared(path, origin); -// traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); -// manager.getTrafficLight(34836); -// EXPECT_FALSE(manager.getTrafficLights().find(34836) == std::end(manager.getTrafficLights())); -// manager.getTrafficLight(34802); -// EXPECT_FALSE(manager.getTrafficLights().find(34802) == std::end(manager.getTrafficLights())); -// EXPECT_EQ(manager.getTrafficLights().size(), static_cast(2)); -// } - -// TEST(TrafficLightManager, setColor) -// { -// const auto node = std::make_shared("setColor"); -// std::string path = -// ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; -// geographic_msgs::msg::GeoPoint origin; -// origin.latitude = 35.61836750154; -// origin.longitude = 139.78066608243; -// const auto hdmap_utils_ptr = std::make_shared(path, origin); -// traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); -// for (const auto & [id, traffic_light] : manager.getTrafficLights()) { -// using Color = traffic_simulator::TrafficLight::Color; -// using Status = traffic_simulator::TrafficLight::Status; -// using Shape = traffic_simulator::TrafficLight::Shape; -// manager.getTrafficLight(id).clear(); -// manager.getTrafficLight(id).emplace(Color::green); -// EXPECT_TRUE( -// manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::circle)); -// manager.getTrafficLight(id).clear(); -// manager.getTrafficLight(id).emplace(Color::yellow); -// EXPECT_TRUE( -// manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::circle)); -// manager.getTrafficLight(id).clear(); -// manager.getTrafficLight(id).emplace(Color::red); -// EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::red, Status::solid_on, Shape::circle)); -// } -// } - -// TEST(TrafficLightManager, setArrow) -// { -// const auto node = std::make_shared("setArrow"); -// std::string path = -// ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; -// geographic_msgs::msg::GeoPoint origin; -// origin.latitude = 35.61836750154; -// origin.longitude = 139.78066608243; -// const auto hdmap_utils_ptr = std::make_shared(path, origin); -// traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); -// for (const auto & [id, traffic_light] : manager.getTrafficLights()) { -// using Color = traffic_simulator::TrafficLight::Color; -// using Status = traffic_simulator::TrafficLight::Status; -// using Shape = traffic_simulator::TrafficLight::Shape; -// manager.getTrafficLight(id).clear(); -// manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::left); -// EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::left)); -// manager.getTrafficLight(id).clear(); -// manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::right); -// EXPECT_TRUE( -// manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::right)); -// manager.getTrafficLight(id).clear(); -// manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::up); -// EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::up)); -// } -// } +#include "../expect_eq_macros.hpp" + +/// Helper functions +// clang-format off +auto stateFromColor(const std::string & color) -> std::string { return color + " solidOn circle"; } +auto stateFromStatus(const std::string & status) -> std::string { return "green " + status + " circle"; } +auto stateFromShape(const std::string & shape) -> std::string { return "green solidOn " + shape; } +// clang-format on + +class ConventionalTrafficLightsTest : public testing::Test +{ +protected: + const lanelet::Id id = 34836; + + const rclcpp::Node::SharedPtr node_ptr = + std::make_shared("ConventionalTrafficLightsTest"); + + const std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + + const std::shared_ptr hdmap_utils_ptr = + std::make_shared( + path, geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)); + + traffic_simulator::ConventionalTrafficLights lights = + traffic_simulator::ConventionalTrafficLights(node_ptr, hdmap_utils_ptr); +}; + +TEST_F(ConventionalTrafficLightsTest, setTrafficLightsColor) +{ + using Color = traffic_simulator::TrafficLight::Color; + + lights.setTrafficLightsColor(id, Color::green); + EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("green") == std::string::npos); + + lights.setTrafficLightsColor(id, Color::yellow); + EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("yellow") == std::string::npos); + + lights.setTrafficLightsColor(id, Color::red); + EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("red") == std::string::npos); + + lights.setTrafficLightsColor(id, Color::white); + EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("white") == std::string::npos); +} + +TEST_F(ConventionalTrafficLightsTest, setTrafficLightsState_color) +{ + // green + lights.setTrafficLightsState(id, stateFromColor("green")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("green")); + + lights.setTrafficLightsState(id, stateFromColor("Green")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("green")); + + // red + lights.setTrafficLightsState(id, stateFromColor("red")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("red")); + + lights.setTrafficLightsState(id, stateFromColor("Red")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("red")); + + // yellow + lights.setTrafficLightsState(id, stateFromColor("yellow")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("yellow")); + + lights.setTrafficLightsState(id, stateFromColor("Yellow")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("yellow")); + + lights.setTrafficLightsState(id, stateFromColor("amber")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("yellow")); + + // white + lights.setTrafficLightsState(id, stateFromColor("white")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("white")); +} + +TEST_F(ConventionalTrafficLightsTest, setTrafficLightsState_status) +{ + // solid on + lights.setTrafficLightsState(id, stateFromStatus("solidOn")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOn")); + + // solid off + lights.setTrafficLightsState(id, stateFromStatus("solidOff")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOff")); + + lights.setTrafficLightsState(id, stateFromStatus("Blank")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOff")); + + lights.setTrafficLightsState(id, stateFromStatus("none")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOff")); + + // flashing + lights.setTrafficLightsState(id, stateFromStatus("flashing")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("flashing")); + + // unknown + lights.setTrafficLightsState(id, stateFromStatus("unknown")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("unknown")); +} + +TEST_F(ConventionalTrafficLightsTest, setTrafficLightsState_shape) +{ + lights.setTrafficLightsState(id, stateFromShape("circle")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("circle")); + + lights.setTrafficLightsState(id, stateFromShape("cross")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("cross")); + + lights.setTrafficLightsState(id, stateFromShape("left")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("left")); + + lights.setTrafficLightsState(id, stateFromShape("down")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("down")); + + lights.setTrafficLightsState(id, stateFromShape("up")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("up")); + + lights.setTrafficLightsState(id, stateFromShape("right")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("right")); + + lights.setTrafficLightsState(id, stateFromShape("lowerLeft")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("lowerLeft")); + + lights.setTrafficLightsState(id, stateFromShape("upperLeft")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("upperLeft")); + + lights.setTrafficLightsState(id, stateFromShape("lowerRight")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("lowerRight")); + + lights.setTrafficLightsState(id, stateFromShape("upperRight")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("upperRight")); + + lights.setTrafficLightsState(id, stateFromShape("straight")); + EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("up")); +} + +TEST_F(ConventionalTrafficLightsTest, isAnyTrafficLightChanged) +{ + EXPECT_TRUE(lights.isAnyTrafficLightChanged()); +} + +TEST_F(ConventionalTrafficLightsTest, isRequiredStopTrafficLightState) +{ + lights.setTrafficLightsState(id, stateFromColor("green")); + EXPECT_FALSE(lights.isRequiredStopTrafficLightState(id)); + + lights.setTrafficLightsState(id, stateFromColor("yellow")); + EXPECT_TRUE(lights.isRequiredStopTrafficLightState(id)); + + lights.setTrafficLightsState(id, "yellow flashing circle"); + EXPECT_FALSE(lights.isRequiredStopTrafficLightState(id)); + + lights.setTrafficLightsState(id, stateFromColor("red")); + EXPECT_TRUE(lights.isRequiredStopTrafficLightState(id)); +} + +TEST_F(ConventionalTrafficLightsTest, compareTrafficLightsState) +{ + lights.setTrafficLightsState(id, stateFromColor("green")); + EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromColor("green"))); + EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromColor("Green"))); + EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("yellow"))); + EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("red"))); + EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("amber"))); + EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("white"))); + + lights.setTrafficLightsState(id, stateFromStatus("Blank")); + EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromStatus("solidOff"))); + EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromStatus("Blank"))); + EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromStatus("none"))); +} + +TEST_F(ConventionalTrafficLightsTest, startUpdate) +{ + lights.setTrafficLightsState(id, stateFromColor("red")); + + std::vector markers; + + lights.startUpdate(20.0); + + rclcpp::Subscription::SharedPtr subscriber = + node_ptr->create_subscription( + "traffic_light/marker", 10, + [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { + markers.push_back(*msg_in); + }); + + // spin for 1 second + const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + + // verify lambdas + const auto verify_delete_marker = + [](const visualization_msgs::msg::Marker & marker, const auto & info = "") { + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETEALL) << info; + }; + + const auto verify_add_marker = []( + const visualization_msgs::msg::Marker & marker, + const auto info = "") { + EXPECT_EQ(marker.ns, "bulb") << info; + EXPECT_EQ(marker.id, 34836) << info; + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE) << info; + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD) << info; + + EXPECT_POINT_NEAR_STREAM( + marker.pose.position, + geometry_msgs::build().x(3770.02).y(73738.34).z(5.80), 0.01, info); + + EXPECT_QUATERNION_EQ_STREAM(marker.pose.orientation, geometry_msgs::msg::Quaternion(), info); + + EXPECT_POINT_EQ_STREAM( + marker.scale, geometry_msgs::build().x(0.3).y(0.3).z(0.3), info); + + EXPECT_COLOR_RGBA_EQ_STREAM( + marker.color, std_msgs::build().r(1.0).g(0.0).b(0.0).a(1.0), info); + }; + + // verify + EXPECT_EQ(markers.size(), static_cast(40)); + for (std::size_t i = 0; i < markers.size(); i += 2) { + { + const auto & one_marker = markers[i].markers; + EXPECT_EQ(one_marker.size(), static_cast(1)); + verify_delete_marker(one_marker[0], "marker " + std::to_string(i)); + } + { + const auto & one_marker = markers[i + 1].markers; + EXPECT_EQ(one_marker.size(), static_cast(1)); + verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); + } + } +} + +TEST_F(ConventionalTrafficLightsTest, resetUpdate) +{ + lights.setTrafficLightsState(id, stateFromColor("green")); + + std::vector markers; + + lights.startUpdate(20.0); + + rclcpp::Subscription::SharedPtr subscriber = + node_ptr->create_subscription( + "traffic_light/marker", 10, + [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { + markers.push_back(*msg_in); + }); + + // spin for 1 second + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + lights.resetUpdate(4.0); + end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + + // verify lambdas + const auto verify_delete_marker = + [](const visualization_msgs::msg::Marker & marker, const auto & info = "") { + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETEALL) << info; + }; + + const auto verify_add_marker = []( + const visualization_msgs::msg::Marker & marker, + const auto info = "") { + EXPECT_EQ(marker.ns, "bulb") << info; + EXPECT_EQ(marker.id, 34836) << info; + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE) << info; + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD) << info; + + EXPECT_POINT_NEAR_STREAM( + marker.pose.position, + geometry_msgs::build().x(3770.02).y(73738.34).z(5.80), 0.01, info); + + EXPECT_QUATERNION_EQ_STREAM(marker.pose.orientation, geometry_msgs::msg::Quaternion(), info); + + EXPECT_POINT_EQ_STREAM( + marker.scale, geometry_msgs::build().x(0.3).y(0.3).z(0.3), info); + + EXPECT_COLOR_RGBA_NEAR_STREAM( + marker.color, std_msgs::build().r(0.0).g(0.5).b(0.0).a(1.0), 0.01, + info); + }; + + // verify + EXPECT_EQ(markers.size(), static_cast(24)); + for (std::size_t i = 0; i < markers.size(); i += 2) { + { + const auto & one_marker = markers[i].markers; + EXPECT_EQ(one_marker.size(), static_cast(1)); + verify_delete_marker(one_marker[0], "marker " + std::to_string(i)); + } + { + const auto & one_marker = markers[i + 1].markers; + EXPECT_EQ(one_marker.size(), static_cast(1)); + verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); + } + } +} int main(int argc, char ** argv) { From 30b5bd29803c5085f2c94f3b00ced9a2d496b44d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Mon, 24 Jun 2024 11:52:08 +0200 Subject: [PATCH 17/45] Add message timing verification in tests Signed-off-by: Mateusz Palczuk --- .../test_traffic_light_manager.cpp | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index 25de92087b7..eec2fe7dd31 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -27,6 +27,12 @@ auto stateFromColor(const std::string & color) -> std::string { return color + auto stateFromStatus(const std::string & status) -> std::string { return "green " + status + " circle"; } auto stateFromShape(const std::string & shape) -> std::string { return "green solidOn " + shape; } // clang-format on +auto getTime(const std_msgs::msg::Header & header) -> unsigned int +{ + static constexpr unsigned int nanosecond_multiplier = static_cast(1e+9); + return static_cast(header.stamp.sec) * nanosecond_multiplier + + static_cast(header.stamp.nanosec); +} class ConventionalTrafficLightsTest : public testing::Test { @@ -243,6 +249,8 @@ TEST_F(ConventionalTrafficLightsTest, startUpdate) marker.color, std_msgs::build().r(1.0).g(0.0).b(0.0).a(1.0), info); }; + std::vector headers; + // verify EXPECT_EQ(markers.size(), static_cast(40)); for (std::size_t i = 0; i < markers.size(); i += 2) { @@ -255,8 +263,18 @@ TEST_F(ConventionalTrafficLightsTest, startUpdate) const auto & one_marker = markers[i + 1].markers; EXPECT_EQ(one_marker.size(), static_cast(1)); verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); + + headers.push_back(one_marker[0].header); } } + + // verify message timing + const double expected_time = 1.0 / 20.0; + const double actual_time = + (static_cast(getTime(headers.back()) - getTime(headers.front())) / + static_cast(headers.size() - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); } TEST_F(ConventionalTrafficLightsTest, resetUpdate) @@ -313,6 +331,8 @@ TEST_F(ConventionalTrafficLightsTest, resetUpdate) info); }; + std::vector headers; + // verify EXPECT_EQ(markers.size(), static_cast(24)); for (std::size_t i = 0; i < markers.size(); i += 2) { @@ -325,8 +345,28 @@ TEST_F(ConventionalTrafficLightsTest, resetUpdate) const auto & one_marker = markers[i + 1].markers; EXPECT_EQ(one_marker.size(), static_cast(1)); verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); + + headers.push_back(one_marker[0].header); } } + + // verify message timing + { + const double expected_time = 1.0 / 20.0; + const double actual_time = + (static_cast(getTime(headers.at(9)) - getTime(headers.front())) / + static_cast(10 - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + } + { + const double expected_time = 1.0 / 4.0; + const double actual_time = + (static_cast(getTime(headers.back()) - getTime(*(headers.rbegin() + 1))) / + static_cast(headers.size() - 10 - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + } } int main(int argc, char ** argv) From 852a5eb549e863b89c2d4ec879cf8af395a98a19 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Mon, 24 Jun 2024 16:50:57 +0200 Subject: [PATCH 18/45] Add ROS message generation tests for TrafficLights Signed-off-by: Mateusz Palczuk --- .../test_traffic_light_manager.cpp | 72 +++++++++++++++++-- 1 file changed, 68 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index eec2fe7dd31..bde771ce1e6 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -27,17 +27,22 @@ auto stateFromColor(const std::string & color) -> std::string { return color + auto stateFromStatus(const std::string & status) -> std::string { return "green " + status + " circle"; } auto stateFromShape(const std::string & shape) -> std::string { return "green solidOn " + shape; } // clang-format on -auto getTime(const std_msgs::msg::Header & header) -> unsigned int + +/// Returns time in nanoseconds +auto getTime(const std_msgs::msg::Header & header) -> int { - static constexpr unsigned int nanosecond_multiplier = static_cast(1e+9); - return static_cast(header.stamp.sec) * nanosecond_multiplier + - static_cast(header.stamp.nanosec); + static constexpr int nanosecond_multiplier = static_cast(1e+9); + return static_cast(header.stamp.sec) * nanosecond_multiplier + + static_cast(header.stamp.nanosec); } +/// Returns time in nanoseconds +auto getTime(const rclcpp::Time & time) -> int { return static_cast(time.nanoseconds()); } class ConventionalTrafficLightsTest : public testing::Test { protected: const lanelet::Id id = 34836; + const lanelet::Id signal_id = 34806; const rclcpp::Node::SharedPtr node_ptr = std::make_shared("ConventionalTrafficLightsTest"); @@ -369,6 +374,65 @@ TEST_F(ConventionalTrafficLightsTest, resetUpdate) } } +TEST_F(ConventionalTrafficLightsTest, generateAutowarePerceptionMsg) +{ + lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); + lights.setTrafficLightsConfidence(id, 0.7); + + const auto msg = lights.generateAutowarePerceptionMsg(); + + const double expected_time = static_cast(getTime(node_ptr->get_clock()->now())) * 1e-9; + const double actual_time = static_cast(getTime(msg.stamp)) * 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + + EXPECT_EQ(msg.signals.size(), static_cast(1)); + EXPECT_EQ(msg.signals.front().elements.size(), static_cast(2)); + + EXPECT_EQ(msg.signals[0].traffic_signal_id, signal_id); + + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + // signals are parsed in reverse order + EXPECT_EQ(msg.signals[0].elements[0].color, TrafficSignalElement::AMBER); + EXPECT_EQ(msg.signals[0].elements[0].status, TrafficSignalElement::FLASHING); + EXPECT_EQ(msg.signals[0].elements[0].shape, TrafficSignalElement::CIRCLE); + EXPECT_NEAR(msg.signals[0].elements[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(msg.signals[0].elements[1].color, TrafficSignalElement::RED); + EXPECT_EQ(msg.signals[0].elements[1].status, TrafficSignalElement::SOLID_ON); + EXPECT_EQ(msg.signals[0].elements[1].shape, TrafficSignalElement::CIRCLE); + EXPECT_NEAR(msg.signals[0].elements[1].confidence, 0.7, 1e-6); +} + +TEST_F(ConventionalTrafficLightsTest, generateAutowareAutoPerceptionMsg) +{ + lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); + lights.setTrafficLightsConfidence(id, 0.7); + + const auto msg = lights.generateAutowareAutoPerceptionMsg(); + + const double expected_time = static_cast(getTime(node_ptr->get_clock()->now())) * 1e-9; + const double actual_time = static_cast(getTime(msg.header)) * 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + + EXPECT_EQ(msg.signals.size(), static_cast(1)); + EXPECT_EQ(msg.signals.front().lights.size(), static_cast(2)); + + EXPECT_EQ(msg.header.frame_id, "camera_link"); + EXPECT_EQ(msg.signals[0].map_primitive_id, id); + + using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; + // signals are parsed in reverse order + EXPECT_EQ(msg.signals[0].lights[0].color, TrafficLight::AMBER); + EXPECT_EQ(msg.signals[0].lights[0].status, TrafficLight::FLASHING); + EXPECT_EQ(msg.signals[0].lights[0].shape, TrafficLight::CIRCLE); + EXPECT_NEAR(msg.signals[0].lights[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(msg.signals[0].lights[1].color, TrafficLight::RED); + EXPECT_EQ(msg.signals[0].lights[1].status, TrafficLight::SOLID_ON); + EXPECT_EQ(msg.signals[0].lights[1].shape, TrafficLight::CIRCLE); + EXPECT_NEAR(msg.signals[0].lights[1].confidence, 0.7, 1e-6); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 36dd3f030bb7bc879efc7e6d64f4708a3c5e27de Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 25 Jun 2024 11:31:14 +0200 Subject: [PATCH 19/45] Add publishing tests for V2I traffic lights Signed-off-by: Mateusz Palczuk --- .../test_traffic_light_manager.cpp | 290 +++++++++++++++++- 1 file changed, 287 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index bde771ce1e6..f756902c3ab 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -29,12 +30,15 @@ auto stateFromShape(const std::string & shape) -> std::string { return "green // clang-format on /// Returns time in nanoseconds -auto getTime(const std_msgs::msg::Header & header) -> int +auto getTime(const builtin_interfaces::msg::Time & time) -> int { static constexpr int nanosecond_multiplier = static_cast(1e+9); - return static_cast(header.stamp.sec) * nanosecond_multiplier + - static_cast(header.stamp.nanosec); + return static_cast(time.sec) * nanosecond_multiplier + static_cast(time.nanosec); } + +/// Returns time in nanoseconds +auto getTime(const std_msgs::msg::Header & header) -> int { return getTime(header.stamp); } + /// Returns time in nanoseconds auto getTime(const rclcpp::Time & time) -> int { return static_cast(time.nanoseconds()); } @@ -61,6 +65,28 @@ class ConventionalTrafficLightsTest : public testing::Test traffic_simulator::ConventionalTrafficLights(node_ptr, hdmap_utils_ptr); }; +class V2ITrafficLightsTest : public testing::Test +{ +protected: + const lanelet::Id id = 34836; + const lanelet::Id signal_id = 34806; + + const rclcpp::Node::SharedPtr node_ptr = std::make_shared("V2ITrafficLightsTest"); + + const std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + + const std::shared_ptr hdmap_utils_ptr = + std::make_shared( + path, geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)); + + traffic_simulator::V2ITrafficLights lights = + traffic_simulator::V2ITrafficLights(node_ptr, hdmap_utils_ptr, "awf/universe"); +}; + TEST_F(ConventionalTrafficLightsTest, setTrafficLightsColor) { using Color = traffic_simulator::TrafficLight::Color; @@ -433,6 +459,264 @@ TEST_F(ConventionalTrafficLightsTest, generateAutowareAutoPerceptionMsg) EXPECT_NEAR(msg.signals[0].lights[1].confidence, 0.7, 1e-6); } +TEST_F(V2ITrafficLightsTest, startUpdate) +{ + using namespace autoware_perception_msgs::msg; + + lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); + lights.setTrafficLightsConfidence(id, 0.7); + + std::vector signals; + + lights.startUpdate(20.0); + + rclcpp::Subscription::SharedPtr subscriber = + node_ptr->create_subscription( + "/perception/traffic_light_recognition/external/traffic_signals", 10, + [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + + // spin for 1 second + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + + EXPECT_EQ(signals.size(), static_cast(20)); + + std::vector stamps; + + for (std::size_t i = 0; i < signals.size(); ++i) { + stamps.push_back(signals[i].stamp); + + const auto & one_message = signals[i].signals; + std::string info = "signals message number " + std::to_string(i); + + EXPECT_EQ(one_message.size(), static_cast(1)) << info; + EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; + + EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; + + EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; + EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; + EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; + EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; + EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); + } + + // verify message timing + const double expected_time = 1.0 / 20.0; + const double actual_time = + (static_cast(getTime(stamps.back()) - getTime(stamps.front())) / + static_cast(stamps.size() - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); +} + +TEST_F(V2ITrafficLightsTest, startUpdate_legacy) +{ + using namespace autoware_perception_msgs::msg; + + lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); + lights.setTrafficLightsConfidence(id, 0.7); + + std::vector signals; + + lights.startUpdate(20.0); + + rclcpp::Subscription::SharedPtr subscriber = + node_ptr->create_subscription( + "/v2x/traffic_signals", 10, + [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + + // spin for 1 second + const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + + EXPECT_EQ(signals.size(), static_cast(20)); + + std::vector stamps; + + for (std::size_t i = 0; i < signals.size(); ++i) { + stamps.push_back(signals[i].stamp); + + const auto & one_message = signals[i].signals; + std::string info = "signals message number " + std::to_string(i); + + EXPECT_EQ(one_message.size(), static_cast(1)) << info; + EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; + + EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; + + EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; + EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; + EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; + EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; + EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); + } + + // verify message timing + const double expected_time = 1.0 / 20.0; + const double actual_time = + (static_cast(getTime(stamps.back()) - getTime(stamps.front())) / + static_cast(stamps.size() - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); +} + +TEST_F(V2ITrafficLightsTest, resetUpdate) +{ + using namespace autoware_perception_msgs::msg; + + lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); + lights.setTrafficLightsConfidence(id, 0.7); + + std::vector signals; + + lights.startUpdate(20.0); + + rclcpp::Subscription::SharedPtr subscriber = + node_ptr->create_subscription( + "/perception/traffic_light_recognition/external/traffic_signals", 10, + [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + + // spin for 1 second + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + lights.resetUpdate(4.0); + end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + + EXPECT_EQ(signals.size(), static_cast(12)); + + std::vector stamps; + + for (std::size_t i = 0; i < signals.size(); ++i) { + stamps.push_back(signals[i].stamp); + + const auto & one_message = signals[i].signals; + std::string info = "signals message number " + std::to_string(i); + + EXPECT_EQ(one_message.size(), static_cast(1)) << info; + EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; + + EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; + + EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; + EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; + EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; + EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; + EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); + } + + // verify message timing + { + const double expected_time = 1.0 / 20.0; + const double actual_time = + (static_cast(getTime(stamps.at(9)) - getTime(stamps.front())) / + static_cast(10 - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + } + { + const double expected_time = 1.0 / 4.0; + const double actual_time = + (static_cast(getTime(stamps.back()) - getTime(*(stamps.rbegin() + 1))) / + static_cast(stamps.size() - 10 - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + } +} + +TEST_F(V2ITrafficLightsTest, resetUpdate_legacy) +{ + using namespace autoware_perception_msgs::msg; + + lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); + lights.setTrafficLightsConfidence(id, 0.7); + + std::vector signals; + + lights.startUpdate(20.0); + + rclcpp::Subscription::SharedPtr subscriber = + node_ptr->create_subscription( + "/v2x/traffic_signals", 10, + [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + + // spin for 1 second + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + lights.resetUpdate(4.0); + end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(node_ptr); + } + + EXPECT_EQ(signals.size(), static_cast(12)); + + std::vector stamps; + + for (std::size_t i = 0; i < signals.size(); ++i) { + stamps.push_back(signals[i].stamp); + + const auto & one_message = signals[i].signals; + std::string info = "signals message number " + std::to_string(i); + + EXPECT_EQ(one_message.size(), static_cast(1)) << info; + EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; + + EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; + + EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; + EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; + EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; + EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; + EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); + } + + // verify message timing + { + const double expected_time = 1.0 / 20.0; + const double actual_time = + (static_cast(getTime(stamps.at(9)) - getTime(stamps.front())) / + static_cast(10 - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + } + { + const double expected_time = 1.0 / 4.0; + const double actual_time = + (static_cast(getTime(stamps.back()) - getTime(*(stamps.rbegin() + 1))) / + static_cast(stamps.size() - 10 - 1)) * + 1e-9; + EXPECT_NEAR(actual_time, expected_time, 1e-4); + } +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 4408e7c44f4239b4271db0f333fae83997c801fd Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 25 Jun 2024 15:19:41 +0200 Subject: [PATCH 20/45] Use typed tests in traffic lights Signed-off-by: Mateusz Palczuk --- .../test_traffic_light_manager.cpp | 345 ++++++++++-------- 1 file changed, 185 insertions(+), 160 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index f756902c3ab..866506f32ae 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -42,14 +42,14 @@ auto getTime(const std_msgs::msg::Header & header) -> int { return getTime(heade /// Returns time in nanoseconds auto getTime(const rclcpp::Time & time) -> int { return static_cast(time.nanoseconds()); } -class ConventionalTrafficLightsTest : public testing::Test +// Separate base class to enable different constructor argument number +class TrafficLightsTestBase : public testing::Test { -protected: +public: const lanelet::Id id = 34836; const lanelet::Id signal_id = 34806; - const rclcpp::Node::SharedPtr node_ptr = - std::make_shared("ConventionalTrafficLightsTest"); + const rclcpp::Node::SharedPtr node_ptr = std::make_shared("TrafficLightsTest"); const std::string path = ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; @@ -60,188 +60,211 @@ class ConventionalTrafficLightsTest : public testing::Test .latitude(35.61836750154) .longitude(139.78066608243) .altitude(0.0)); - - traffic_simulator::ConventionalTrafficLights lights = - traffic_simulator::ConventionalTrafficLights(node_ptr, hdmap_utils_ptr); }; -class V2ITrafficLightsTest : public testing::Test +template +class TrafficLightsTest : public TrafficLightsTestBase { -protected: - const lanelet::Id id = 34836; - const lanelet::Id signal_id = 34806; +public: + TrafficLightsT lights; - const rclcpp::Node::SharedPtr node_ptr = std::make_shared("V2ITrafficLightsTest"); + TrafficLightsTest() : lights(node_ptr, hdmap_utils_ptr) {} +}; - const std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; +// Add specialization to enable different constructor argument number +template <> +class TrafficLightsTest : public TrafficLightsTestBase +{ +public: + traffic_simulator::V2ITrafficLights lights; - const std::shared_ptr hdmap_utils_ptr = - std::make_shared( - path, geographic_msgs::build() - .latitude(35.61836750154) - .longitude(139.78066608243) - .altitude(0.0)); + TrafficLightsTest() : lights(node_ptr, hdmap_utils_ptr, "awf/universe"){}; +}; - traffic_simulator::V2ITrafficLights lights = - traffic_simulator::V2ITrafficLights(node_ptr, hdmap_utils_ptr, "awf/universe"); +// Alias for declaring types in typed tests +using TrafficLightsTypes = + testing::Types; + +// Test name generator +class TrafficLightsNameGenerator +{ +public: + template + static std::string GetName(int) + { + if constexpr (std::is_same_v) + return "ConventionalTrafficLights"; + if constexpr (std::is_same_v) + return "V2ITrafficLights"; + } }; -TEST_F(ConventionalTrafficLightsTest, setTrafficLightsColor) +TYPED_TEST_SUITE(TrafficLightsTest, TrafficLightsTypes, TrafficLightsNameGenerator); + +// Define V2I type for use in tests with V2I traffic lights only +using V2ITrafficLightsTest = TrafficLightsTest; + +TYPED_TEST(TrafficLightsTest, setTrafficLightsColor) { using Color = traffic_simulator::TrafficLight::Color; - lights.setTrafficLightsColor(id, Color::green); - EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("green") == std::string::npos); + this->lights.setTrafficLightsColor(this->id, Color::green); + EXPECT_FALSE( + this->lights.getTrafficLightsComposedState(this->id).find("green") == std::string::npos); - lights.setTrafficLightsColor(id, Color::yellow); - EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("yellow") == std::string::npos); + this->lights.setTrafficLightsColor(this->id, Color::yellow); + EXPECT_FALSE( + this->lights.getTrafficLightsComposedState(this->id).find("yellow") == std::string::npos); - lights.setTrafficLightsColor(id, Color::red); - EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("red") == std::string::npos); + this->lights.setTrafficLightsColor(this->id, Color::red); + EXPECT_FALSE( + this->lights.getTrafficLightsComposedState(this->id).find("red") == std::string::npos); - lights.setTrafficLightsColor(id, Color::white); - EXPECT_FALSE(lights.getTrafficLightsComposedState(id).find("white") == std::string::npos); + this->lights.setTrafficLightsColor(this->id, Color::white); + EXPECT_FALSE( + this->lights.getTrafficLightsComposedState(this->id).find("white") == std::string::npos); } -TEST_F(ConventionalTrafficLightsTest, setTrafficLightsState_color) +TYPED_TEST(TrafficLightsTest, setTrafficLightsState_color) { // green - lights.setTrafficLightsState(id, stateFromColor("green")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("green")); + this->lights.setTrafficLightsState(this->id, stateFromColor("green")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("green")); - lights.setTrafficLightsState(id, stateFromColor("Green")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("green")); + this->lights.setTrafficLightsState(this->id, stateFromColor("Green")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("green")); // red - lights.setTrafficLightsState(id, stateFromColor("red")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("red")); + this->lights.setTrafficLightsState(this->id, stateFromColor("red")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("red")); - lights.setTrafficLightsState(id, stateFromColor("Red")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("red")); + this->lights.setTrafficLightsState(this->id, stateFromColor("Red")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("red")); // yellow - lights.setTrafficLightsState(id, stateFromColor("yellow")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("yellow")); + this->lights.setTrafficLightsState(this->id, stateFromColor("yellow")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("yellow")); - lights.setTrafficLightsState(id, stateFromColor("Yellow")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("yellow")); + this->lights.setTrafficLightsState(this->id, stateFromColor("Yellow")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("yellow")); - lights.setTrafficLightsState(id, stateFromColor("amber")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("yellow")); + this->lights.setTrafficLightsState(this->id, stateFromColor("amber")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("yellow")); // white - lights.setTrafficLightsState(id, stateFromColor("white")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromColor("white")); + this->lights.setTrafficLightsState(this->id, stateFromColor("white")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("white")); } -TEST_F(ConventionalTrafficLightsTest, setTrafficLightsState_status) +TYPED_TEST(TrafficLightsTest, setTrafficLightsState_status) { // solid on - lights.setTrafficLightsState(id, stateFromStatus("solidOn")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOn")); + this->lights.setTrafficLightsState(this->id, stateFromStatus("solidOn")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOn")); // solid off - lights.setTrafficLightsState(id, stateFromStatus("solidOff")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOff")); + this->lights.setTrafficLightsState(this->id, stateFromStatus("solidOff")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); - lights.setTrafficLightsState(id, stateFromStatus("Blank")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOff")); + this->lights.setTrafficLightsState(this->id, stateFromStatus("Blank")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); - lights.setTrafficLightsState(id, stateFromStatus("none")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("solidOff")); + this->lights.setTrafficLightsState(this->id, stateFromStatus("none")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); // flashing - lights.setTrafficLightsState(id, stateFromStatus("flashing")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("flashing")); + this->lights.setTrafficLightsState(this->id, stateFromStatus("flashing")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("flashing")); // unknown - lights.setTrafficLightsState(id, stateFromStatus("unknown")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromStatus("unknown")); + this->lights.setTrafficLightsState(this->id, stateFromStatus("unknown")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("unknown")); } -TEST_F(ConventionalTrafficLightsTest, setTrafficLightsState_shape) +TYPED_TEST(TrafficLightsTest, setTrafficLightsState_shape) { - lights.setTrafficLightsState(id, stateFromShape("circle")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("circle")); + this->lights.setTrafficLightsState(this->id, stateFromShape("circle")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("circle")); - lights.setTrafficLightsState(id, stateFromShape("cross")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("cross")); + this->lights.setTrafficLightsState(this->id, stateFromShape("cross")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("cross")); - lights.setTrafficLightsState(id, stateFromShape("left")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("left")); + this->lights.setTrafficLightsState(this->id, stateFromShape("left")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("left")); - lights.setTrafficLightsState(id, stateFromShape("down")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("down")); + this->lights.setTrafficLightsState(this->id, stateFromShape("down")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("down")); - lights.setTrafficLightsState(id, stateFromShape("up")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("up")); + this->lights.setTrafficLightsState(this->id, stateFromShape("up")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("up")); - lights.setTrafficLightsState(id, stateFromShape("right")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("right")); + this->lights.setTrafficLightsState(this->id, stateFromShape("right")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("right")); - lights.setTrafficLightsState(id, stateFromShape("lowerLeft")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("lowerLeft")); + this->lights.setTrafficLightsState(this->id, stateFromShape("lowerLeft")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("lowerLeft")); - lights.setTrafficLightsState(id, stateFromShape("upperLeft")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("upperLeft")); + this->lights.setTrafficLightsState(this->id, stateFromShape("upperLeft")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("upperLeft")); - lights.setTrafficLightsState(id, stateFromShape("lowerRight")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("lowerRight")); + this->lights.setTrafficLightsState(this->id, stateFromShape("lowerRight")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("lowerRight")); - lights.setTrafficLightsState(id, stateFromShape("upperRight")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("upperRight")); + this->lights.setTrafficLightsState(this->id, stateFromShape("upperRight")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("upperRight")); - lights.setTrafficLightsState(id, stateFromShape("straight")); - EXPECT_EQ(lights.getTrafficLightsComposedState(id), stateFromShape("up")); + this->lights.setTrafficLightsState(this->id, stateFromShape("straight")); + EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("up")); } -TEST_F(ConventionalTrafficLightsTest, isAnyTrafficLightChanged) +TYPED_TEST(TrafficLightsTest, isAnyTrafficLightChanged) { - EXPECT_TRUE(lights.isAnyTrafficLightChanged()); + EXPECT_TRUE(this->lights.isAnyTrafficLightChanged()); } -TEST_F(ConventionalTrafficLightsTest, isRequiredStopTrafficLightState) +TYPED_TEST(TrafficLightsTest, isRequiredStopTrafficLightState) { - lights.setTrafficLightsState(id, stateFromColor("green")); - EXPECT_FALSE(lights.isRequiredStopTrafficLightState(id)); + this->lights.setTrafficLightsState(this->id, stateFromColor("green")); + EXPECT_FALSE(this->lights.isRequiredStopTrafficLightState(this->id)); - lights.setTrafficLightsState(id, stateFromColor("yellow")); - EXPECT_TRUE(lights.isRequiredStopTrafficLightState(id)); + this->lights.setTrafficLightsState(this->id, stateFromColor("yellow")); + EXPECT_TRUE(this->lights.isRequiredStopTrafficLightState(this->id)); - lights.setTrafficLightsState(id, "yellow flashing circle"); - EXPECT_FALSE(lights.isRequiredStopTrafficLightState(id)); + this->lights.setTrafficLightsState(this->id, "yellow flashing circle"); + EXPECT_FALSE(this->lights.isRequiredStopTrafficLightState(this->id)); - lights.setTrafficLightsState(id, stateFromColor("red")); - EXPECT_TRUE(lights.isRequiredStopTrafficLightState(id)); + this->lights.setTrafficLightsState(this->id, stateFromColor("red")); + EXPECT_TRUE(this->lights.isRequiredStopTrafficLightState(this->id)); } -TEST_F(ConventionalTrafficLightsTest, compareTrafficLightsState) +TYPED_TEST(TrafficLightsTest, compareTrafficLightsState) { - lights.setTrafficLightsState(id, stateFromColor("green")); - EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromColor("green"))); - EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromColor("Green"))); - EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("yellow"))); - EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("red"))); - EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("amber"))); - EXPECT_FALSE(lights.compareTrafficLightsState(id, stateFromColor("white"))); - - lights.setTrafficLightsState(id, stateFromStatus("Blank")); - EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromStatus("solidOff"))); - EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromStatus("Blank"))); - EXPECT_TRUE(lights.compareTrafficLightsState(id, stateFromStatus("none"))); + this->lights.setTrafficLightsState(this->id, stateFromColor("green")); + EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromColor("green"))); + EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromColor("Green"))); + EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("yellow"))); + EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("red"))); + EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("amber"))); + EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("white"))); + + this->lights.setTrafficLightsState(this->id, stateFromStatus("Blank")); + EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromStatus("solidOff"))); + EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromStatus("Blank"))); + EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromStatus("none"))); } -TEST_F(ConventionalTrafficLightsTest, startUpdate) +TYPED_TEST(TrafficLightsTest, startUpdate_publishMarkers) { - lights.setTrafficLightsState(id, stateFromColor("red")); + this->lights.setTrafficLightsState(this->id, stateFromColor("red")); std::vector markers; - lights.startUpdate(20.0); + this->lights.startUpdate(20.0); + + auto node_ptr = this->node_ptr; rclcpp::Subscription::SharedPtr subscriber = - node_ptr->create_subscription( + this->node_ptr->template create_subscription( "traffic_light/marker", 10, [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { markers.push_back(*msg_in); @@ -250,7 +273,7 @@ TEST_F(ConventionalTrafficLightsTest, startUpdate) // spin for 1 second const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } // verify lambdas @@ -308,16 +331,16 @@ TEST_F(ConventionalTrafficLightsTest, startUpdate) EXPECT_NEAR(actual_time, expected_time, 1e-4); } -TEST_F(ConventionalTrafficLightsTest, resetUpdate) +TYPED_TEST(TrafficLightsTest, resetUpdate_publishMarkers) { - lights.setTrafficLightsState(id, stateFromColor("green")); + this->lights.setTrafficLightsState(this->id, stateFromColor("green")); std::vector markers; - lights.startUpdate(20.0); + this->lights.startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = - node_ptr->create_subscription( + this->node_ptr->template create_subscription( "traffic_light/marker", 10, [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { markers.push_back(*msg_in); @@ -326,12 +349,12 @@ TEST_F(ConventionalTrafficLightsTest, resetUpdate) // spin for 1 second auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } - lights.resetUpdate(4.0); + this->lights.resetUpdate(4.0); end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } // verify lambdas @@ -400,21 +423,22 @@ TEST_F(ConventionalTrafficLightsTest, resetUpdate) } } -TEST_F(ConventionalTrafficLightsTest, generateAutowarePerceptionMsg) +TYPED_TEST(TrafficLightsTest, generateAutowarePerceptionMsg) { - lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); - lights.setTrafficLightsConfidence(id, 0.7); + this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights.setTrafficLightsConfidence(this->id, 0.7); - const auto msg = lights.generateAutowarePerceptionMsg(); + const auto msg = this->lights.generateAutowarePerceptionMsg(); - const double expected_time = static_cast(getTime(node_ptr->get_clock()->now())) * 1e-9; + const double expected_time = + static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; const double actual_time = static_cast(getTime(msg.stamp)) * 1e-9; EXPECT_NEAR(actual_time, expected_time, 1e-4); EXPECT_EQ(msg.signals.size(), static_cast(1)); EXPECT_EQ(msg.signals.front().elements.size(), static_cast(2)); - EXPECT_EQ(msg.signals[0].traffic_signal_id, signal_id); + EXPECT_EQ(msg.signals[0].traffic_signal_id, this->signal_id); using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; // signals are parsed in reverse order @@ -429,14 +453,15 @@ TEST_F(ConventionalTrafficLightsTest, generateAutowarePerceptionMsg) EXPECT_NEAR(msg.signals[0].elements[1].confidence, 0.7, 1e-6); } -TEST_F(ConventionalTrafficLightsTest, generateAutowareAutoPerceptionMsg) +TYPED_TEST(TrafficLightsTest, generateAutowareAutoPerceptionMsg) { - lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); - lights.setTrafficLightsConfidence(id, 0.7); + this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights.setTrafficLightsConfidence(this->id, 0.7); - const auto msg = lights.generateAutowareAutoPerceptionMsg(); + const auto msg = this->lights.generateAutowareAutoPerceptionMsg(); - const double expected_time = static_cast(getTime(node_ptr->get_clock()->now())) * 1e-9; + const double expected_time = + static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; const double actual_time = static_cast(getTime(msg.header)) * 1e-9; EXPECT_NEAR(actual_time, expected_time, 1e-4); @@ -444,7 +469,7 @@ TEST_F(ConventionalTrafficLightsTest, generateAutowareAutoPerceptionMsg) EXPECT_EQ(msg.signals.front().lights.size(), static_cast(2)); EXPECT_EQ(msg.header.frame_id, "camera_link"); - EXPECT_EQ(msg.signals[0].map_primitive_id, id); + EXPECT_EQ(msg.signals[0].map_primitive_id, this->id); using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; // signals are parsed in reverse order @@ -459,26 +484,26 @@ TEST_F(ConventionalTrafficLightsTest, generateAutowareAutoPerceptionMsg) EXPECT_NEAR(msg.signals[0].lights[1].confidence, 0.7, 1e-6); } -TEST_F(V2ITrafficLightsTest, startUpdate) +TEST_F(V2ITrafficLightsTest, startUpdate_publishSignals) { using namespace autoware_perception_msgs::msg; - lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); - lights.setTrafficLightsConfidence(id, 0.7); + this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights.setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - lights.startUpdate(20.0); + this->lights.startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = - node_ptr->create_subscription( + this->node_ptr->create_subscription( "/perception/traffic_light_recognition/external/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); // spin for 1 second auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } EXPECT_EQ(signals.size(), static_cast(20)); @@ -516,26 +541,26 @@ TEST_F(V2ITrafficLightsTest, startUpdate) EXPECT_NEAR(actual_time, expected_time, 1e-4); } -TEST_F(V2ITrafficLightsTest, startUpdate_legacy) +TEST_F(V2ITrafficLightsTest, startUpdate_publishSignalsLegacy) { using namespace autoware_perception_msgs::msg; - lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); - lights.setTrafficLightsConfidence(id, 0.7); + this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights.setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - lights.startUpdate(20.0); + this->lights.startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = - node_ptr->create_subscription( + this->node_ptr->create_subscription( "/v2x/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); // spin for 1 second const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } EXPECT_EQ(signals.size(), static_cast(20)); @@ -573,31 +598,31 @@ TEST_F(V2ITrafficLightsTest, startUpdate_legacy) EXPECT_NEAR(actual_time, expected_time, 1e-4); } -TEST_F(V2ITrafficLightsTest, resetUpdate) +TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignals) { using namespace autoware_perception_msgs::msg; - lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); - lights.setTrafficLightsConfidence(id, 0.7); + this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights.setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - lights.startUpdate(20.0); + this->lights.startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = - node_ptr->create_subscription( + this->node_ptr->create_subscription( "/perception/traffic_light_recognition/external/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); // spin for 1 second auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } - lights.resetUpdate(4.0); + this->lights.resetUpdate(4.0); end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } EXPECT_EQ(signals.size(), static_cast(12)); @@ -645,31 +670,31 @@ TEST_F(V2ITrafficLightsTest, resetUpdate) } } -TEST_F(V2ITrafficLightsTest, resetUpdate_legacy) +TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignalsLegacy) { using namespace autoware_perception_msgs::msg; - lights.setTrafficLightsState(id, "red solidOn circle, yellow flashing circle"); - lights.setTrafficLightsConfidence(id, 0.7); + this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights.setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - lights.startUpdate(20.0); + this->lights.startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = - node_ptr->create_subscription( + this->node_ptr->create_subscription( "/v2x/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); // spin for 1 second auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } - lights.resetUpdate(4.0); + this->lights.resetUpdate(4.0); end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(node_ptr); + rclcpp::spin_some(this->node_ptr); } EXPECT_EQ(signals.size(), static_cast(12)); From 5e0ef5fb35d8456df7d265a6814169f5ff7d7552 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 26 Jun 2024 10:18:55 +0200 Subject: [PATCH 21/45] Simplify traffic lights tests Use pointer for traffic lights Signed-off-by: Mateusz Palczuk --- .../test_traffic_light_manager.cpp | 246 +++++++++--------- 1 file changed, 121 insertions(+), 125 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index 866506f32ae..33fb07ea9e5 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -42,14 +42,14 @@ auto getTime(const std_msgs::msg::Header & header) -> int { return getTime(heade /// Returns time in nanoseconds auto getTime(const rclcpp::Time & time) -> int { return static_cast(time.nanoseconds()); } -// Separate base class to enable different constructor argument number -class TrafficLightsTestBase : public testing::Test +template +class TrafficLightsTest : public testing::Test { public: const lanelet::Id id = 34836; const lanelet::Id signal_id = 34806; - const rclcpp::Node::SharedPtr node_ptr = std::make_shared("TrafficLightsTest"); + const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsTest"); const std::string path = ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; @@ -60,25 +60,20 @@ class TrafficLightsTestBase : public testing::Test .latitude(35.61836750154) .longitude(139.78066608243) .altitude(0.0)); -}; -template -class TrafficLightsTest : public TrafficLightsTestBase -{ -public: - TrafficLightsT lights; - - TrafficLightsTest() : lights(node_ptr, hdmap_utils_ptr) {} -}; - -// Add specialization to enable different constructor argument number -template <> -class TrafficLightsTest : public TrafficLightsTestBase -{ -public: - traffic_simulator::V2ITrafficLights lights; - - TrafficLightsTest() : lights(node_ptr, hdmap_utils_ptr, "awf/universe"){}; + std::unique_ptr lights; + + TrafficLightsTest() + : lights([this] { + if constexpr (std::is_same_v) + return std::make_unique(node_ptr, hdmap_utils_ptr); + else if constexpr (std::is_same_v) + return std::make_unique(node_ptr, hdmap_utils_ptr, "awf/universe"); + else + throw std::runtime_error("Given TrafficLights type is not supported"); + }()) + { + } }; // Alias for declaring types in typed tests @@ -99,6 +94,7 @@ class TrafficLightsNameGenerator } }; +// Declare typed test suite TYPED_TEST_SUITE(TrafficLightsTest, TrafficLightsTypes, TrafficLightsNameGenerator); // Define V2I type for use in tests with V2I traffic lights only @@ -108,158 +104,158 @@ TYPED_TEST(TrafficLightsTest, setTrafficLightsColor) { using Color = traffic_simulator::TrafficLight::Color; - this->lights.setTrafficLightsColor(this->id, Color::green); + this->lights->setTrafficLightsColor(this->id, Color::green); EXPECT_FALSE( - this->lights.getTrafficLightsComposedState(this->id).find("green") == std::string::npos); + this->lights->getTrafficLightsComposedState(this->id).find("green") == std::string::npos); - this->lights.setTrafficLightsColor(this->id, Color::yellow); + this->lights->setTrafficLightsColor(this->id, Color::yellow); EXPECT_FALSE( - this->lights.getTrafficLightsComposedState(this->id).find("yellow") == std::string::npos); + this->lights->getTrafficLightsComposedState(this->id).find("yellow") == std::string::npos); - this->lights.setTrafficLightsColor(this->id, Color::red); + this->lights->setTrafficLightsColor(this->id, Color::red); EXPECT_FALSE( - this->lights.getTrafficLightsComposedState(this->id).find("red") == std::string::npos); + this->lights->getTrafficLightsComposedState(this->id).find("red") == std::string::npos); - this->lights.setTrafficLightsColor(this->id, Color::white); + this->lights->setTrafficLightsColor(this->id, Color::white); EXPECT_FALSE( - this->lights.getTrafficLightsComposedState(this->id).find("white") == std::string::npos); + this->lights->getTrafficLightsComposedState(this->id).find("white") == std::string::npos); } TYPED_TEST(TrafficLightsTest, setTrafficLightsState_color) { // green - this->lights.setTrafficLightsState(this->id, stateFromColor("green")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("green")); + this->lights->setTrafficLightsState(this->id, stateFromColor("green")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("green")); - this->lights.setTrafficLightsState(this->id, stateFromColor("Green")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("green")); + this->lights->setTrafficLightsState(this->id, stateFromColor("Green")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("green")); // red - this->lights.setTrafficLightsState(this->id, stateFromColor("red")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("red")); + this->lights->setTrafficLightsState(this->id, stateFromColor("red")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("red")); - this->lights.setTrafficLightsState(this->id, stateFromColor("Red")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("red")); + this->lights->setTrafficLightsState(this->id, stateFromColor("Red")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("red")); // yellow - this->lights.setTrafficLightsState(this->id, stateFromColor("yellow")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("yellow")); + this->lights->setTrafficLightsState(this->id, stateFromColor("yellow")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("yellow")); - this->lights.setTrafficLightsState(this->id, stateFromColor("Yellow")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("yellow")); + this->lights->setTrafficLightsState(this->id, stateFromColor("Yellow")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("yellow")); - this->lights.setTrafficLightsState(this->id, stateFromColor("amber")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("yellow")); + this->lights->setTrafficLightsState(this->id, stateFromColor("amber")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("yellow")); // white - this->lights.setTrafficLightsState(this->id, stateFromColor("white")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromColor("white")); + this->lights->setTrafficLightsState(this->id, stateFromColor("white")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("white")); } TYPED_TEST(TrafficLightsTest, setTrafficLightsState_status) { // solid on - this->lights.setTrafficLightsState(this->id, stateFromStatus("solidOn")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOn")); + this->lights->setTrafficLightsState(this->id, stateFromStatus("solidOn")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOn")); // solid off - this->lights.setTrafficLightsState(this->id, stateFromStatus("solidOff")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); + this->lights->setTrafficLightsState(this->id, stateFromStatus("solidOff")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); - this->lights.setTrafficLightsState(this->id, stateFromStatus("Blank")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); + this->lights->setTrafficLightsState(this->id, stateFromStatus("Blank")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); - this->lights.setTrafficLightsState(this->id, stateFromStatus("none")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); + this->lights->setTrafficLightsState(this->id, stateFromStatus("none")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); // flashing - this->lights.setTrafficLightsState(this->id, stateFromStatus("flashing")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("flashing")); + this->lights->setTrafficLightsState(this->id, stateFromStatus("flashing")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("flashing")); // unknown - this->lights.setTrafficLightsState(this->id, stateFromStatus("unknown")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromStatus("unknown")); + this->lights->setTrafficLightsState(this->id, stateFromStatus("unknown")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("unknown")); } TYPED_TEST(TrafficLightsTest, setTrafficLightsState_shape) { - this->lights.setTrafficLightsState(this->id, stateFromShape("circle")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("circle")); + this->lights->setTrafficLightsState(this->id, stateFromShape("circle")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("circle")); - this->lights.setTrafficLightsState(this->id, stateFromShape("cross")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("cross")); + this->lights->setTrafficLightsState(this->id, stateFromShape("cross")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("cross")); - this->lights.setTrafficLightsState(this->id, stateFromShape("left")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("left")); + this->lights->setTrafficLightsState(this->id, stateFromShape("left")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("left")); - this->lights.setTrafficLightsState(this->id, stateFromShape("down")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("down")); + this->lights->setTrafficLightsState(this->id, stateFromShape("down")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("down")); - this->lights.setTrafficLightsState(this->id, stateFromShape("up")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("up")); + this->lights->setTrafficLightsState(this->id, stateFromShape("up")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("up")); - this->lights.setTrafficLightsState(this->id, stateFromShape("right")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("right")); + this->lights->setTrafficLightsState(this->id, stateFromShape("right")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("right")); - this->lights.setTrafficLightsState(this->id, stateFromShape("lowerLeft")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("lowerLeft")); + this->lights->setTrafficLightsState(this->id, stateFromShape("lowerLeft")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("lowerLeft")); - this->lights.setTrafficLightsState(this->id, stateFromShape("upperLeft")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("upperLeft")); + this->lights->setTrafficLightsState(this->id, stateFromShape("upperLeft")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("upperLeft")); - this->lights.setTrafficLightsState(this->id, stateFromShape("lowerRight")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("lowerRight")); + this->lights->setTrafficLightsState(this->id, stateFromShape("lowerRight")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("lowerRight")); - this->lights.setTrafficLightsState(this->id, stateFromShape("upperRight")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("upperRight")); + this->lights->setTrafficLightsState(this->id, stateFromShape("upperRight")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("upperRight")); - this->lights.setTrafficLightsState(this->id, stateFromShape("straight")); - EXPECT_EQ(this->lights.getTrafficLightsComposedState(this->id), stateFromShape("up")); + this->lights->setTrafficLightsState(this->id, stateFromShape("straight")); + EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("up")); } TYPED_TEST(TrafficLightsTest, isAnyTrafficLightChanged) { - EXPECT_TRUE(this->lights.isAnyTrafficLightChanged()); + EXPECT_TRUE(this->lights->isAnyTrafficLightChanged()); } TYPED_TEST(TrafficLightsTest, isRequiredStopTrafficLightState) { - this->lights.setTrafficLightsState(this->id, stateFromColor("green")); - EXPECT_FALSE(this->lights.isRequiredStopTrafficLightState(this->id)); + this->lights->setTrafficLightsState(this->id, stateFromColor("green")); + EXPECT_FALSE(this->lights->isRequiredStopTrafficLightState(this->id)); - this->lights.setTrafficLightsState(this->id, stateFromColor("yellow")); - EXPECT_TRUE(this->lights.isRequiredStopTrafficLightState(this->id)); + this->lights->setTrafficLightsState(this->id, stateFromColor("yellow")); + EXPECT_TRUE(this->lights->isRequiredStopTrafficLightState(this->id)); - this->lights.setTrafficLightsState(this->id, "yellow flashing circle"); - EXPECT_FALSE(this->lights.isRequiredStopTrafficLightState(this->id)); + this->lights->setTrafficLightsState(this->id, "yellow flashing circle"); + EXPECT_FALSE(this->lights->isRequiredStopTrafficLightState(this->id)); - this->lights.setTrafficLightsState(this->id, stateFromColor("red")); - EXPECT_TRUE(this->lights.isRequiredStopTrafficLightState(this->id)); + this->lights->setTrafficLightsState(this->id, stateFromColor("red")); + EXPECT_TRUE(this->lights->isRequiredStopTrafficLightState(this->id)); } TYPED_TEST(TrafficLightsTest, compareTrafficLightsState) { - this->lights.setTrafficLightsState(this->id, stateFromColor("green")); - EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromColor("green"))); - EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromColor("Green"))); - EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("yellow"))); - EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("red"))); - EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("amber"))); - EXPECT_FALSE(this->lights.compareTrafficLightsState(this->id, stateFromColor("white"))); - - this->lights.setTrafficLightsState(this->id, stateFromStatus("Blank")); - EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromStatus("solidOff"))); - EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromStatus("Blank"))); - EXPECT_TRUE(this->lights.compareTrafficLightsState(this->id, stateFromStatus("none"))); + this->lights->setTrafficLightsState(this->id, stateFromColor("green")); + EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromColor("green"))); + EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromColor("Green"))); + EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("yellow"))); + EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("red"))); + EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("amber"))); + EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("white"))); + + this->lights->setTrafficLightsState(this->id, stateFromStatus("Blank")); + EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromStatus("solidOff"))); + EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromStatus("Blank"))); + EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromStatus("none"))); } TYPED_TEST(TrafficLightsTest, startUpdate_publishMarkers) { - this->lights.setTrafficLightsState(this->id, stateFromColor("red")); + this->lights->setTrafficLightsState(this->id, stateFromColor("red")); std::vector markers; - this->lights.startUpdate(20.0); + this->lights->startUpdate(20.0); auto node_ptr = this->node_ptr; @@ -333,11 +329,11 @@ TYPED_TEST(TrafficLightsTest, startUpdate_publishMarkers) TYPED_TEST(TrafficLightsTest, resetUpdate_publishMarkers) { - this->lights.setTrafficLightsState(this->id, stateFromColor("green")); + this->lights->setTrafficLightsState(this->id, stateFromColor("green")); std::vector markers; - this->lights.startUpdate(20.0); + this->lights->startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->template create_subscription( @@ -351,7 +347,7 @@ TYPED_TEST(TrafficLightsTest, resetUpdate_publishMarkers) while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - this->lights.resetUpdate(4.0); + this->lights->resetUpdate(4.0); end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); @@ -425,10 +421,10 @@ TYPED_TEST(TrafficLightsTest, resetUpdate_publishMarkers) TYPED_TEST(TrafficLightsTest, generateAutowarePerceptionMsg) { - this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights.setTrafficLightsConfidence(this->id, 0.7); + this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights->setTrafficLightsConfidence(this->id, 0.7); - const auto msg = this->lights.generateAutowarePerceptionMsg(); + const auto msg = this->lights->generateAutowarePerceptionMsg(); const double expected_time = static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; @@ -455,10 +451,10 @@ TYPED_TEST(TrafficLightsTest, generateAutowarePerceptionMsg) TYPED_TEST(TrafficLightsTest, generateAutowareAutoPerceptionMsg) { - this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights.setTrafficLightsConfidence(this->id, 0.7); + this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights->setTrafficLightsConfidence(this->id, 0.7); - const auto msg = this->lights.generateAutowareAutoPerceptionMsg(); + const auto msg = this->lights->generateAutowareAutoPerceptionMsg(); const double expected_time = static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; @@ -488,12 +484,12 @@ TEST_F(V2ITrafficLightsTest, startUpdate_publishSignals) { using namespace autoware_perception_msgs::msg; - this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights.setTrafficLightsConfidence(this->id, 0.7); + this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights->setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - this->lights.startUpdate(20.0); + this->lights->startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( @@ -545,12 +541,12 @@ TEST_F(V2ITrafficLightsTest, startUpdate_publishSignalsLegacy) { using namespace autoware_perception_msgs::msg; - this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights.setTrafficLightsConfidence(this->id, 0.7); + this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights->setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - this->lights.startUpdate(20.0); + this->lights->startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( @@ -602,12 +598,12 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignals) { using namespace autoware_perception_msgs::msg; - this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights.setTrafficLightsConfidence(this->id, 0.7); + this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights->setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - this->lights.startUpdate(20.0); + this->lights->startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( @@ -619,7 +615,7 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignals) while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - this->lights.resetUpdate(4.0); + this->lights->resetUpdate(4.0); end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); @@ -674,12 +670,12 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignalsLegacy) { using namespace autoware_perception_msgs::msg; - this->lights.setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights.setTrafficLightsConfidence(this->id, 0.7); + this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); + this->lights->setTrafficLightsConfidence(this->id, 0.7); std::vector signals; - this->lights.startUpdate(20.0); + this->lights->startUpdate(20.0); rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( @@ -691,7 +687,7 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignalsLegacy) while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - this->lights.resetUpdate(4.0); + this->lights->resetUpdate(4.0); end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); From ca83e193fb64fae326ab7c8439388caf53686551 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 26 Jun 2024 10:20:02 +0200 Subject: [PATCH 22/45] Resolve problems with resources not released Signed-off-by: Mateusz Palczuk --- .../traffic_lights/traffic_lights_publisher.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp index b3005735a99..18761700a08 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp @@ -28,6 +28,7 @@ class TrafficLightsPublisherBase { public: virtual auto publish(const TrafficLightsBase & traffic_lights) const -> void = 0; + virtual ~TrafficLightsPublisherBase() = default; }; template @@ -46,6 +47,8 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase { } + ~TrafficLightsPublisher() override = default; + auto publish(const TrafficLightsBase & traffic_lights) const -> void override; private: From 3591b86c8a45486df04409564b8bde8f16f900e1 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 26 Jun 2024 10:31:36 +0200 Subject: [PATCH 23/45] Rename traffic lights test file Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/test/src/traffic_lights/CMakeLists.txt | 4 ++-- ...test_traffic_light_manager.cpp => test_traffic_lights.cpp} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename simulation/traffic_simulator/test/src/traffic_lights/{test_traffic_light_manager.cpp => test_traffic_lights.cpp} (100%) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt index 23df37b3bd0..4143eb47235 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt @@ -1,5 +1,5 @@ ament_add_gtest(test_traffic_light test_traffic_light.cpp) target_link_libraries(test_traffic_light traffic_simulator) -ament_add_gtest(test_traffic_light_manager test_traffic_light_manager.cpp) -target_link_libraries(test_traffic_light_manager traffic_simulator) +ament_add_gtest(test_traffic_lights test_traffic_lights.cpp) +target_link_libraries(test_traffic_lights traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp similarity index 100% rename from simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp rename to simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp From 6c47410c9e75d073523d2c11f5d5bb1d9ce96905 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 26 Jun 2024 10:43:37 +0200 Subject: [PATCH 24/45] Add explicit destructors for traffic lights Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/traffic_lights/traffic_lights.hpp | 4 ++++ .../traffic_simulator/traffic_lights/traffic_lights_base.hpp | 2 ++ 2 files changed, 6 insertions(+) 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 2075a8140ea..122d5c1ea93 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 @@ -32,6 +32,8 @@ class ConventionalTrafficLights : public TrafficLightsBase { } + ~ConventionalTrafficLights() override = default; + private: auto update() const -> void { @@ -57,6 +59,8 @@ class V2ITrafficLights : public TrafficLightsBase { } + ~V2ITrafficLights() override = default; + private: auto update() const -> void override { 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 b28f1352522..7af7bea9cac 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 @@ -48,6 +48,8 @@ class TrafficLightsBase { } + virtual ~TrafficLightsBase() = default; + // update auto startUpdate(const double update_rate) -> void; From 5b28b6cf2295943cbb700132160e90dff875f0f6 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 26 Jun 2024 11:04:56 +0200 Subject: [PATCH 25/45] Rename internal traffic lights tests Signed-off-by: Mateusz Palczuk --- .../traffic_lights/test_traffic_lights.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp index 33fb07ea9e5..59abee3f301 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp @@ -43,13 +43,13 @@ auto getTime(const std_msgs::msg::Header & header) -> int { return getTime(heade auto getTime(const rclcpp::Time & time) -> int { return static_cast(time.nanoseconds()); } template -class TrafficLightsTest : public testing::Test +class TrafficLightsInternalTest : public testing::Test { public: const lanelet::Id id = 34836; const lanelet::Id signal_id = 34806; - const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsTest"); + const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsInternalTest"); const std::string path = ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; @@ -63,7 +63,7 @@ class TrafficLightsTest : public testing::Test std::unique_ptr lights; - TrafficLightsTest() + TrafficLightsInternalTest() : lights([this] { if constexpr (std::is_same_v) return std::make_unique(node_ptr, hdmap_utils_ptr); @@ -95,12 +95,12 @@ class TrafficLightsNameGenerator }; // Declare typed test suite -TYPED_TEST_SUITE(TrafficLightsTest, TrafficLightsTypes, TrafficLightsNameGenerator); +TYPED_TEST_SUITE(TrafficLightsInternalTest, TrafficLightsTypes, TrafficLightsNameGenerator); // Define V2I type for use in tests with V2I traffic lights only -using V2ITrafficLightsTest = TrafficLightsTest; +using V2ITrafficLightsTest = TrafficLightsInternalTest; -TYPED_TEST(TrafficLightsTest, setTrafficLightsColor) +TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsColor) { using Color = traffic_simulator::TrafficLight::Color; @@ -121,7 +121,7 @@ TYPED_TEST(TrafficLightsTest, setTrafficLightsColor) this->lights->getTrafficLightsComposedState(this->id).find("white") == std::string::npos); } -TYPED_TEST(TrafficLightsTest, setTrafficLightsState_color) +TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsState_color) { // green this->lights->setTrafficLightsState(this->id, stateFromColor("green")); @@ -152,7 +152,7 @@ TYPED_TEST(TrafficLightsTest, setTrafficLightsState_color) EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("white")); } -TYPED_TEST(TrafficLightsTest, setTrafficLightsState_status) +TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsState_status) { // solid on this->lights->setTrafficLightsState(this->id, stateFromStatus("solidOn")); @@ -177,7 +177,7 @@ TYPED_TEST(TrafficLightsTest, setTrafficLightsState_status) EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("unknown")); } -TYPED_TEST(TrafficLightsTest, setTrafficLightsState_shape) +TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsState_shape) { this->lights->setTrafficLightsState(this->id, stateFromShape("circle")); EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("circle")); @@ -213,12 +213,12 @@ TYPED_TEST(TrafficLightsTest, setTrafficLightsState_shape) EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("up")); } -TYPED_TEST(TrafficLightsTest, isAnyTrafficLightChanged) +TYPED_TEST(TrafficLightsInternalTest, isAnyTrafficLightChanged) { EXPECT_TRUE(this->lights->isAnyTrafficLightChanged()); } -TYPED_TEST(TrafficLightsTest, isRequiredStopTrafficLightState) +TYPED_TEST(TrafficLightsInternalTest, isRequiredStopTrafficLightState) { this->lights->setTrafficLightsState(this->id, stateFromColor("green")); EXPECT_FALSE(this->lights->isRequiredStopTrafficLightState(this->id)); @@ -233,7 +233,7 @@ TYPED_TEST(TrafficLightsTest, isRequiredStopTrafficLightState) EXPECT_TRUE(this->lights->isRequiredStopTrafficLightState(this->id)); } -TYPED_TEST(TrafficLightsTest, compareTrafficLightsState) +TYPED_TEST(TrafficLightsInternalTest, compareTrafficLightsState) { this->lights->setTrafficLightsState(this->id, stateFromColor("green")); EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromColor("green"))); @@ -249,7 +249,7 @@ TYPED_TEST(TrafficLightsTest, compareTrafficLightsState) EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromStatus("none"))); } -TYPED_TEST(TrafficLightsTest, startUpdate_publishMarkers) +TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers) { this->lights->setTrafficLightsState(this->id, stateFromColor("red")); @@ -327,7 +327,7 @@ TYPED_TEST(TrafficLightsTest, startUpdate_publishMarkers) EXPECT_NEAR(actual_time, expected_time, 1e-4); } -TYPED_TEST(TrafficLightsTest, resetUpdate_publishMarkers) +TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers) { this->lights->setTrafficLightsState(this->id, stateFromColor("green")); @@ -419,7 +419,7 @@ TYPED_TEST(TrafficLightsTest, resetUpdate_publishMarkers) } } -TYPED_TEST(TrafficLightsTest, generateAutowarePerceptionMsg) +TYPED_TEST(TrafficLightsInternalTest, generateAutowarePerceptionMsg) { this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); this->lights->setTrafficLightsConfidence(this->id, 0.7); @@ -449,7 +449,7 @@ TYPED_TEST(TrafficLightsTest, generateAutowarePerceptionMsg) EXPECT_NEAR(msg.signals[0].elements[1].confidence, 0.7, 1e-6); } -TYPED_TEST(TrafficLightsTest, generateAutowareAutoPerceptionMsg) +TYPED_TEST(TrafficLightsInternalTest, generateAutowareAutoPerceptionMsg) { this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); this->lights->setTrafficLightsConfidence(this->id, 0.7); From 44b458df42bfaa61b29be91cbec1da1e269005de Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 26 Jun 2024 11:07:16 +0200 Subject: [PATCH 26/45] Rename internal traffic lights tests file Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/test/src/traffic_lights/CMakeLists.txt | 4 ++-- ...st_traffic_lights.cpp => test_traffic_lights_internal.cpp} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename simulation/traffic_simulator/test/src/traffic_lights/{test_traffic_lights.cpp => test_traffic_lights_internal.cpp} (100%) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt index 4143eb47235..7d7e72ca022 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt @@ -1,5 +1,5 @@ ament_add_gtest(test_traffic_light test_traffic_light.cpp) target_link_libraries(test_traffic_light traffic_simulator) -ament_add_gtest(test_traffic_lights test_traffic_lights.cpp) -target_link_libraries(test_traffic_lights traffic_simulator) +ament_add_gtest(test_traffic_lights_internal test_traffic_lights_internal.cpp) +target_link_libraries(test_traffic_lights_internal traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp similarity index 100% rename from simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp rename to simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp From e8ef0c01255aaa05251f741ba98784e80bf717ef Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 26 Jun 2024 18:02:04 +0200 Subject: [PATCH 27/45] Add TrafficLights tests + clean up other tests and verify frequency Signed-off-by: Mateusz Palczuk --- .../test/src/traffic_lights/CMakeLists.txt | 7 +- .../test/src/traffic_lights/helper.hpp | 44 +++ .../traffic_lights/test_traffic_lights.cpp | 151 ++++++++++ .../test_traffic_lights_internal.cpp | 283 ++++++++++-------- 4 files changed, 359 insertions(+), 126 deletions(-) create mode 100644 simulation/traffic_simulator/test/src/traffic_lights/helper.hpp create mode 100644 simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp diff --git a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt index 7d7e72ca022..3ed41676649 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt @@ -1,5 +1,8 @@ ament_add_gtest(test_traffic_light test_traffic_light.cpp) target_link_libraries(test_traffic_light traffic_simulator) -ament_add_gtest(test_traffic_lights_internal test_traffic_lights_internal.cpp) -target_link_libraries(test_traffic_lights_internal traffic_simulator) +ament_add_gtest(test_traffic_lights + test_traffic_lights_internal.cpp + test_traffic_lights.cpp +) +target_link_libraries(test_traffic_lights traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp new file mode 100644 index 00000000000..a85c91a2c75 --- /dev/null +++ b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 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 TRAFFIC_SIMULATOR__TEST__TRAFFIC_LIGHTS__HELPER_HPP_ +#define TRAFFIC_SIMULATOR__TEST__TRAFFIC_LIGHTS__HELPER_HPP_ + +#include +#include + +/// Helper functions +// clang-format off +inline auto stateFromColor(const std::string & color) -> std::string { return color + " solidOn circle"; } +inline auto stateFromStatus(const std::string & status) -> std::string { return "green " + status + " circle"; } +inline auto stateFromShape(const std::string & shape) -> std::string { return "green solidOn " + shape; } +// clang-format on + +/// Returns time in nanoseconds +inline auto getTime(const builtin_interfaces::msg::Time & time) -> int +{ + static constexpr int nanosecond_multiplier = static_cast(1e+9); + return static_cast(time.sec) * nanosecond_multiplier + static_cast(time.nanosec); +} + +/// Returns time in nanoseconds +inline auto getTime(const std_msgs::msg::Header & header) -> int { return getTime(header.stamp); } + +/// Returns time in nanoseconds +inline auto getTime(const rclcpp::Time & time) -> int +{ + return static_cast(time.nanoseconds()); +} + +#endif // TRAFFIC_SIMULATOR__TEST__TRAFFIC_LIGHTS__HELPER_HPP_ diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp new file mode 100644 index 00000000000..86aea5345b0 --- /dev/null +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp @@ -0,0 +1,151 @@ +// Copyright 2024 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 +#include +#include +#include + +#include "../expect_eq_macros.hpp" +#include "helper.hpp" + +constexpr double timing_eps = 1e-3; +constexpr double frequency_eps = 0.5; + +class TrafficLightsTest : public testing::Test +{ +public: + const lanelet::Id id = 34836; + const lanelet::Id signal_id = 34806; + + const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsTest"); + + const std::string path = + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + + const std::shared_ptr hdmap_utils_ptr = + std::make_shared( + path, geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)); + + const std::string red_state = stateFromColor("red"); + const std::string yellow_state = "yellow flashing circle"; + + std::unique_ptr lights = + std::make_unique(node_ptr, hdmap_utils_ptr, "awf/universe"); +}; + +TEST_F(TrafficLightsTest, isAnyTrafficLightChanged) +{ + EXPECT_TRUE(lights->isAnyTrafficLightChanged()); +} + +TEST_F(TrafficLightsTest, getConventionalTrafficLights) +{ + { + this->lights->getConventionalTrafficLights()->setTrafficLightsState(this->id, this->red_state); + + const auto actual_state = + this->lights->getConventionalTrafficLights()->getTrafficLightsComposedState(this->id); + + EXPECT_EQ(actual_state, this->red_state); + } + { + this->lights->getConventionalTrafficLights()->setTrafficLightsState( + this->id, this->yellow_state); + + const auto actual_state = + this->lights->getConventionalTrafficLights()->getTrafficLightsComposedState(this->id); + + EXPECT_EQ(actual_state, this->yellow_state); + } +} + +TEST_F(TrafficLightsTest, getV2ITrafficLights) +{ + { + this->lights->getV2ITrafficLights()->setTrafficLightsState(this->id, this->red_state); + + const auto actual_state = + this->lights->getV2ITrafficLights()->getTrafficLightsComposedState(this->id); + + EXPECT_EQ(actual_state, this->red_state); + } + { + this->lights->getV2ITrafficLights()->setTrafficLightsState(this->id, this->yellow_state); + + const auto actual_state = + this->lights->getV2ITrafficLights()->getTrafficLightsComposedState(this->id); + + EXPECT_EQ(actual_state, this->yellow_state); + } +} + +TEST_F(TrafficLightsTest, startTrafficLightsUpdate) +{ + this->lights->getConventionalTrafficLights()->setTrafficLightsState(this->id, this->red_state); + this->lights->getV2ITrafficLights()->setTrafficLightsState(this->id, this->red_state); + + std::vector markers; + + rclcpp::Subscription::SharedPtr subscriber = + this->node_ptr->template create_subscription( + "traffic_light/marker", 10, + [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { + markers.push_back(*msg_in); + }); + + this->lights->startTrafficLightsUpdate(20.0, 10.0); + + // start time is required to be measured here and not from first message, because there are two publishers publishing to this topic at the same time + const auto start_time = node_ptr->now(); + + // spin for 1 second + const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1020); + while (std::chrono::system_clock::now() < end) { + rclcpp::spin_some(this->node_ptr); + } + + std::vector headers; + + // verify + for (std::size_t i = 0; i < markers.size(); ++i) { + const auto & one_marker = markers[i].markers; + EXPECT_EQ(one_marker.size(), static_cast(1)); + + if ( + one_marker.front().header.stamp.sec != 0 and one_marker.front().header.stamp.nanosec != 0u) { + headers.push_back(one_marker.front().header); + } + } + + // verify message timing + const double expected_frequency = 30.0; + const double actual_frequency = + static_cast(headers.size()) / + static_cast(getTime(headers.back()) - getTime(start_time)) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp index 59abee3f301..8872be966b1 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp @@ -21,26 +21,10 @@ #include #include "../expect_eq_macros.hpp" +#include "helper.hpp" -/// Helper functions -// clang-format off -auto stateFromColor(const std::string & color) -> std::string { return color + " solidOn circle"; } -auto stateFromStatus(const std::string & status) -> std::string { return "green " + status + " circle"; } -auto stateFromShape(const std::string & shape) -> std::string { return "green solidOn " + shape; } -// clang-format on - -/// Returns time in nanoseconds -auto getTime(const builtin_interfaces::msg::Time & time) -> int -{ - static constexpr int nanosecond_multiplier = static_cast(1e+9); - return static_cast(time.sec) * nanosecond_multiplier + static_cast(time.nanosec); -} - -/// Returns time in nanoseconds -auto getTime(const std_msgs::msg::Header & header) -> int { return getTime(header.stamp); } - -/// Returns time in nanoseconds -auto getTime(const rclcpp::Time & time) -> int { return static_cast(time.nanoseconds()); } +constexpr double timing_eps = 1e-3; +constexpr double frequency_eps = 0.5; template class TrafficLightsInternalTest : public testing::Test @@ -255,10 +239,6 @@ TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers) std::vector markers; - this->lights->startUpdate(20.0); - - auto node_ptr = this->node_ptr; - rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->template create_subscription( "traffic_light/marker", 10, @@ -266,8 +246,10 @@ TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers) markers.push_back(*msg_in); }); + this->lights->startUpdate(20.0); + // spin for 1 second - const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); + const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1005); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } @@ -302,7 +284,6 @@ TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers) std::vector headers; // verify - EXPECT_EQ(markers.size(), static_cast(40)); for (std::size_t i = 0; i < markers.size(); i += 2) { { const auto & one_marker = markers[i].markers; @@ -319,21 +300,18 @@ TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers) } // verify message timing - const double expected_time = 1.0 / 20.0; - const double actual_time = - (static_cast(getTime(headers.back()) - getTime(headers.front())) / - static_cast(headers.size() - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 20.0; + const double actual_frequency = + static_cast(headers.size() - 1) / + static_cast(getTime(headers.back()) - getTime(headers.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers) { this->lights->setTrafficLightsState(this->id, stateFromColor("green")); - std::vector markers; - - this->lights->startUpdate(20.0); + std::vector markers, markers_reset; rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->template create_subscription( @@ -342,13 +320,22 @@ TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers) markers.push_back(*msg_in); }); + this->lights->startUpdate(20.0); + // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - this->lights->resetUpdate(4.0); - end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + + subscriber = this->node_ptr->template create_subscription( + "traffic_light/marker", 10, + [&markers_reset](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { + markers_reset.push_back(*msg_in); + }); + + this->lights->resetUpdate(10.0); + end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } @@ -381,10 +368,9 @@ TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers) info); }; - std::vector headers; + std::vector headers, headers_reset; // verify - EXPECT_EQ(markers.size(), static_cast(24)); for (std::size_t i = 0; i < markers.size(); i += 2) { { const auto & one_marker = markers[i].markers; @@ -399,23 +385,35 @@ TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers) headers.push_back(one_marker[0].header); } } + for (std::size_t i = 0; i < markers_reset.size(); i += 2) { + { + const auto & one_marker = markers_reset[i].markers; + EXPECT_EQ(one_marker.size(), static_cast(1)); + verify_delete_marker(one_marker[0], "marker " + std::to_string(i)); + } + { + const auto & one_marker = markers_reset[i + 1].markers; + EXPECT_EQ(one_marker.size(), static_cast(1)); + verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); + + headers_reset.push_back(one_marker[0].header); + } + } // verify message timing { - const double expected_time = 1.0 / 20.0; - const double actual_time = - (static_cast(getTime(headers.at(9)) - getTime(headers.front())) / - static_cast(10 - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 20.0; + const double actual_frequency = + static_cast(headers.size() - 1) / + static_cast(getTime(headers.back()) - getTime(headers.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } { - const double expected_time = 1.0 / 4.0; - const double actual_time = - (static_cast(getTime(headers.back()) - getTime(*(headers.rbegin() + 1))) / - static_cast(headers.size() - 10 - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 10.0; + const double actual_frequency = + static_cast(headers_reset.size() - 1) / + static_cast(getTime(headers_reset.back()) - getTime(headers_reset.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } } @@ -429,7 +427,7 @@ TYPED_TEST(TrafficLightsInternalTest, generateAutowarePerceptionMsg) const double expected_time = static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; const double actual_time = static_cast(getTime(msg.stamp)) * 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + EXPECT_NEAR(actual_time, expected_time, timing_eps); EXPECT_EQ(msg.signals.size(), static_cast(1)); EXPECT_EQ(msg.signals.front().elements.size(), static_cast(2)); @@ -459,7 +457,7 @@ TYPED_TEST(TrafficLightsInternalTest, generateAutowareAutoPerceptionMsg) const double expected_time = static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; const double actual_time = static_cast(getTime(msg.header)) * 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + EXPECT_NEAR(actual_time, expected_time, timing_eps); EXPECT_EQ(msg.signals.size(), static_cast(1)); EXPECT_EQ(msg.signals.front().lights.size(), static_cast(2)); @@ -489,21 +487,19 @@ TEST_F(V2ITrafficLightsTest, startUpdate_publishSignals) std::vector signals; - this->lights->startUpdate(20.0); - rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( "/perception/traffic_light_recognition/external/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + this->lights->startUpdate(20.0); + // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1005); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - EXPECT_EQ(signals.size(), static_cast(20)); - std::vector stamps; for (std::size_t i = 0; i < signals.size(); ++i) { @@ -529,12 +525,12 @@ TEST_F(V2ITrafficLightsTest, startUpdate_publishSignals) } // verify message timing - const double expected_time = 1.0 / 20.0; - const double actual_time = - (static_cast(getTime(stamps.back()) - getTime(stamps.front())) / - static_cast(stamps.size() - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 20.0; + const double actual_frequency = + static_cast(stamps.size() - 1) / + static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; + + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } TEST_F(V2ITrafficLightsTest, startUpdate_publishSignalsLegacy) @@ -546,21 +542,19 @@ TEST_F(V2ITrafficLightsTest, startUpdate_publishSignalsLegacy) std::vector signals; - this->lights->startUpdate(20.0); - rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( "/v2x/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + this->lights->startUpdate(20.0); + // spin for 1 second - const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1001); + const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1005); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - EXPECT_EQ(signals.size(), static_cast(20)); - std::vector stamps; for (std::size_t i = 0; i < signals.size(); ++i) { @@ -586,12 +580,11 @@ TEST_F(V2ITrafficLightsTest, startUpdate_publishSignalsLegacy) } // verify message timing - const double expected_time = 1.0 / 20.0; - const double actual_time = - (static_cast(getTime(stamps.back()) - getTime(stamps.front())) / - static_cast(stamps.size() - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 20.0; + const double actual_frequency = + static_cast(stamps.size() - 1) / + static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignals) @@ -601,29 +594,34 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignals) this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); this->lights->setTrafficLightsConfidence(this->id, 0.7); - std::vector signals; - - this->lights->startUpdate(20.0); + std::vector signals, signals_reset; rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( "/perception/traffic_light_recognition/external/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + this->lights->startUpdate(20.0); + // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - this->lights->resetUpdate(4.0); - end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + + subscriber = this->node_ptr->create_subscription( + "/perception/traffic_light_recognition/external/traffic_signals", 10, + [&signals_reset](const TrafficSignalArray::SharedPtr msg_in) { + signals_reset.push_back(*msg_in); + }); + + this->lights->resetUpdate(10.0); + end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - EXPECT_EQ(signals.size(), static_cast(12)); - - std::vector stamps; + std::vector stamps, stamps_reset; for (std::size_t i = 0; i < signals.size(); ++i) { stamps.push_back(signals[i].stamp); @@ -647,22 +645,42 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignals) EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); } + for (std::size_t i = 0; i < signals_reset.size(); ++i) { + stamps_reset.push_back(signals_reset[i].stamp); + + const auto & one_message = signals_reset[i].signals; + std::string info = "signals message number " + std::to_string(i); + + EXPECT_EQ(one_message.size(), static_cast(1)) << info; + EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; + + EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; + + EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; + EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; + EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; + EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; + EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); + } + // verify message timing { - const double expected_time = 1.0 / 20.0; - const double actual_time = - (static_cast(getTime(stamps.at(9)) - getTime(stamps.front())) / - static_cast(10 - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 20.0; + const double actual_frequency = + static_cast(stamps.size() - 1) / + static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } { - const double expected_time = 1.0 / 4.0; - const double actual_time = - (static_cast(getTime(stamps.back()) - getTime(*(stamps.rbegin() + 1))) / - static_cast(stamps.size() - 10 - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 10.0; + const double actual_frequency = + static_cast(stamps_reset.size() - 1) / + static_cast(getTime(stamps_reset.back()) - getTime(stamps_reset.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } } @@ -673,29 +691,33 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignalsLegacy) this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); this->lights->setTrafficLightsConfidence(this->id, 0.7); - std::vector signals; - - this->lights->startUpdate(20.0); + std::vector signals, signals_reset; rclcpp::Subscription::SharedPtr subscriber = this->node_ptr->create_subscription( "/v2x/traffic_signals", 10, [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); + this->lights->startUpdate(20.0); + // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - this->lights->resetUpdate(4.0); - end = std::chrono::system_clock::now() + std::chrono::milliseconds(501); + + subscriber = this->node_ptr->create_subscription( + "/v2x/traffic_signals", 10, [&signals_reset](const TrafficSignalArray::SharedPtr msg_in) { + signals_reset.push_back(*msg_in); + }); + + this->lights->resetUpdate(10.0); + end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); while (std::chrono::system_clock::now() < end) { rclcpp::spin_some(this->node_ptr); } - EXPECT_EQ(signals.size(), static_cast(12)); - - std::vector stamps; + std::vector stamps, stamps_reset; for (std::size_t i = 0; i < signals.size(); ++i) { stamps.push_back(signals[i].stamp); @@ -719,28 +741,41 @@ TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignalsLegacy) EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); } + for (std::size_t i = 0; i < signals_reset.size(); ++i) { + stamps_reset.push_back(signals_reset[i].stamp); + + const auto & one_message = signals_reset[i].signals; + std::string info = "signals message number " + std::to_string(i); + + EXPECT_EQ(one_message.size(), static_cast(1)) << info; + EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; + + EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; + + EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; + EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; + EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); + + EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; + EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; + EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; + EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); + } + // verify message timing { - const double expected_time = 1.0 / 20.0; - const double actual_time = - (static_cast(getTime(stamps.at(9)) - getTime(stamps.front())) / - static_cast(10 - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 20.0; + const double actual_frequency = + static_cast(stamps.size() - 1) / + static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } { - const double expected_time = 1.0 / 4.0; - const double actual_time = - (static_cast(getTime(stamps.back()) - getTime(*(stamps.rbegin() + 1))) / - static_cast(stamps.size() - 10 - 1)) * - 1e-9; - EXPECT_NEAR(actual_time, expected_time, 1e-4); + const double expected_frequency = 10.0; + const double actual_frequency = + static_cast(stamps_reset.size() - 1) / + static_cast(getTime(stamps_reset.back()) - getTime(stamps_reset.front())) * 1e+9; + EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); } } - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - return RUN_ALL_TESTS(); -} From 371534ddcce93c37ba95c331fd3c6089b324d42b Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 27 Jun 2024 11:34:17 +0200 Subject: [PATCH 28/45] Remove exception throwing when starting a timer again in configurable rate updater Signed-off-by: Mateusz Palczuk --- .../src/traffic_lights/configurable_rate_updater.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp index 6301d5cb79e..b638b08f5a6 100644 --- a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp @@ -24,9 +24,6 @@ auto ConfigurableRateUpdater::startTimer(const double update_rate) -> void const auto period = std::chrono::duration(1.0 / update_rate_); timer_ = rclcpp::create_timer( node_base_interface_, node_timers_interface_, clock_ptr_, period, [this]() { thunk_(); }); - } else { - throw std::logic_error( - "ConfigurableRateUpdater was started earlier, cannot be started again - reset instead."); } } From 433019c50f796cfc3543922e78263fe7606b21f0 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 25 Jul 2024 16:46:00 +0200 Subject: [PATCH 29/45] Remove traffic lights tests Signed-off-by: Mateusz Palczuk --- .../test/src/expect_eq_macros.hpp | 18 - .../test/src/traffic_lights/CMakeLists.txt | 6 - .../test/src/traffic_lights/helper.hpp | 44 - .../traffic_lights/test_traffic_lights.cpp | 151 ---- .../test_traffic_lights_internal.cpp | 781 ------------------ 5 files changed, 1000 deletions(-) delete mode 100644 simulation/traffic_simulator/test/src/traffic_lights/helper.hpp delete mode 100644 simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp delete mode 100644 simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp diff --git a/simulation/traffic_simulator/test/src/expect_eq_macros.hpp b/simulation/traffic_simulator/test/src/expect_eq_macros.hpp index 42adee91cbe..c321e9b1a7d 100644 --- a/simulation/traffic_simulator/test/src/expect_eq_macros.hpp +++ b/simulation/traffic_simulator/test/src/expect_eq_macros.hpp @@ -59,12 +59,6 @@ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); -#define EXPECT_QUATERNION_EQ_STREAM(DATA0, DATA1, STREAM_MESSAGE) \ - EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x) << STREAM_MESSAGE; \ - EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y) << STREAM_MESSAGE; \ - EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z) << STREAM_MESSAGE; \ - EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w) << STREAM_MESSAGE; - #define EXPECT_QUATERNION_NEAR(DATA0, DATA1, EPS) \ EXPECT_NEAR(DATA0.x, DATA1.x, EPS); \ EXPECT_NEAR(DATA0.y, DATA1.y, EPS); \ @@ -136,16 +130,4 @@ EXPECT_EQ(DATA0.trajectory_shape, DATA1.trajectory_shape); \ EXPECT_LANE_CHANGE_CONSTRAINT_EQ(DATA0.constraint, DATA1.constraint); -#define EXPECT_COLOR_RGBA_EQ_STREAM(DATA0, DATA1, STREAM_MESSAGE) \ - EXPECT_DOUBLE_EQ(DATA0.r, DATA1.r) << STREAM_MESSAGE; \ - EXPECT_DOUBLE_EQ(DATA0.g, DATA1.g) << STREAM_MESSAGE; \ - EXPECT_DOUBLE_EQ(DATA0.b, DATA1.b) << STREAM_MESSAGE; \ - EXPECT_DOUBLE_EQ(DATA0.a, DATA1.a) << STREAM_MESSAGE; - -#define EXPECT_COLOR_RGBA_NEAR_STREAM(DATA0, DATA1, EPS, STREAM_MESSAGE) \ - EXPECT_NEAR(DATA0.r, DATA1.r, EPS) << STREAM_MESSAGE; \ - EXPECT_NEAR(DATA0.g, DATA1.g, EPS) << STREAM_MESSAGE; \ - EXPECT_NEAR(DATA0.b, DATA1.b, EPS) << STREAM_MESSAGE; \ - EXPECT_NEAR(DATA0.a, DATA1.a, EPS) << STREAM_MESSAGE; - #endif // TRAFFIC_SIMULATOR__TEST__EXPECT_EQ_MACROS_HPP_ diff --git a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt index 3ed41676649..3bc7c5c8d69 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt @@ -1,8 +1,2 @@ ament_add_gtest(test_traffic_light test_traffic_light.cpp) target_link_libraries(test_traffic_light traffic_simulator) - -ament_add_gtest(test_traffic_lights - test_traffic_lights_internal.cpp - test_traffic_lights.cpp -) -target_link_libraries(test_traffic_lights traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp deleted file mode 100644 index a85c91a2c75..00000000000 --- a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2024 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 TRAFFIC_SIMULATOR__TEST__TRAFFIC_LIGHTS__HELPER_HPP_ -#define TRAFFIC_SIMULATOR__TEST__TRAFFIC_LIGHTS__HELPER_HPP_ - -#include -#include - -/// Helper functions -// clang-format off -inline auto stateFromColor(const std::string & color) -> std::string { return color + " solidOn circle"; } -inline auto stateFromStatus(const std::string & status) -> std::string { return "green " + status + " circle"; } -inline auto stateFromShape(const std::string & shape) -> std::string { return "green solidOn " + shape; } -// clang-format on - -/// Returns time in nanoseconds -inline auto getTime(const builtin_interfaces::msg::Time & time) -> int -{ - static constexpr int nanosecond_multiplier = static_cast(1e+9); - return static_cast(time.sec) * nanosecond_multiplier + static_cast(time.nanosec); -} - -/// Returns time in nanoseconds -inline auto getTime(const std_msgs::msg::Header & header) -> int { return getTime(header.stamp); } - -/// Returns time in nanoseconds -inline auto getTime(const rclcpp::Time & time) -> int -{ - return static_cast(time.nanoseconds()); -} - -#endif // TRAFFIC_SIMULATOR__TEST__TRAFFIC_LIGHTS__HELPER_HPP_ diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp deleted file mode 100644 index 86aea5345b0..00000000000 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp +++ /dev/null @@ -1,151 +0,0 @@ -// Copyright 2024 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 -#include -#include -#include - -#include "../expect_eq_macros.hpp" -#include "helper.hpp" - -constexpr double timing_eps = 1e-3; -constexpr double frequency_eps = 0.5; - -class TrafficLightsTest : public testing::Test -{ -public: - const lanelet::Id id = 34836; - const lanelet::Id signal_id = 34806; - - const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsTest"); - - const std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - - const std::shared_ptr hdmap_utils_ptr = - std::make_shared( - path, geographic_msgs::build() - .latitude(35.61836750154) - .longitude(139.78066608243) - .altitude(0.0)); - - const std::string red_state = stateFromColor("red"); - const std::string yellow_state = "yellow flashing circle"; - - std::unique_ptr lights = - std::make_unique(node_ptr, hdmap_utils_ptr, "awf/universe"); -}; - -TEST_F(TrafficLightsTest, isAnyTrafficLightChanged) -{ - EXPECT_TRUE(lights->isAnyTrafficLightChanged()); -} - -TEST_F(TrafficLightsTest, getConventionalTrafficLights) -{ - { - this->lights->getConventionalTrafficLights()->setTrafficLightsState(this->id, this->red_state); - - const auto actual_state = - this->lights->getConventionalTrafficLights()->getTrafficLightsComposedState(this->id); - - EXPECT_EQ(actual_state, this->red_state); - } - { - this->lights->getConventionalTrafficLights()->setTrafficLightsState( - this->id, this->yellow_state); - - const auto actual_state = - this->lights->getConventionalTrafficLights()->getTrafficLightsComposedState(this->id); - - EXPECT_EQ(actual_state, this->yellow_state); - } -} - -TEST_F(TrafficLightsTest, getV2ITrafficLights) -{ - { - this->lights->getV2ITrafficLights()->setTrafficLightsState(this->id, this->red_state); - - const auto actual_state = - this->lights->getV2ITrafficLights()->getTrafficLightsComposedState(this->id); - - EXPECT_EQ(actual_state, this->red_state); - } - { - this->lights->getV2ITrafficLights()->setTrafficLightsState(this->id, this->yellow_state); - - const auto actual_state = - this->lights->getV2ITrafficLights()->getTrafficLightsComposedState(this->id); - - EXPECT_EQ(actual_state, this->yellow_state); - } -} - -TEST_F(TrafficLightsTest, startTrafficLightsUpdate) -{ - this->lights->getConventionalTrafficLights()->setTrafficLightsState(this->id, this->red_state); - this->lights->getV2ITrafficLights()->setTrafficLightsState(this->id, this->red_state); - - std::vector markers; - - rclcpp::Subscription::SharedPtr subscriber = - this->node_ptr->template create_subscription( - "traffic_light/marker", 10, - [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { - markers.push_back(*msg_in); - }); - - this->lights->startTrafficLightsUpdate(20.0, 10.0); - - // start time is required to be measured here and not from first message, because there are two publishers publishing to this topic at the same time - const auto start_time = node_ptr->now(); - - // spin for 1 second - const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1020); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - std::vector headers; - - // verify - for (std::size_t i = 0; i < markers.size(); ++i) { - const auto & one_marker = markers[i].markers; - EXPECT_EQ(one_marker.size(), static_cast(1)); - - if ( - one_marker.front().header.stamp.sec != 0 and one_marker.front().header.stamp.nanosec != 0u) { - headers.push_back(one_marker.front().header); - } - } - - // verify message timing - const double expected_frequency = 30.0; - const double actual_frequency = - static_cast(headers.size()) / - static_cast(getTime(headers.back()) - getTime(start_time)) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp deleted file mode 100644 index 8872be966b1..00000000000 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp +++ /dev/null @@ -1,781 +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 -#include -#include -#include - -#include "../expect_eq_macros.hpp" -#include "helper.hpp" - -constexpr double timing_eps = 1e-3; -constexpr double frequency_eps = 0.5; - -template -class TrafficLightsInternalTest : public testing::Test -{ -public: - const lanelet::Id id = 34836; - const lanelet::Id signal_id = 34806; - - const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsInternalTest"); - - const std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - - const std::shared_ptr hdmap_utils_ptr = - std::make_shared( - path, geographic_msgs::build() - .latitude(35.61836750154) - .longitude(139.78066608243) - .altitude(0.0)); - - std::unique_ptr lights; - - TrafficLightsInternalTest() - : lights([this] { - if constexpr (std::is_same_v) - return std::make_unique(node_ptr, hdmap_utils_ptr); - else if constexpr (std::is_same_v) - return std::make_unique(node_ptr, hdmap_utils_ptr, "awf/universe"); - else - throw std::runtime_error("Given TrafficLights type is not supported"); - }()) - { - } -}; - -// Alias for declaring types in typed tests -using TrafficLightsTypes = - testing::Types; - -// Test name generator -class TrafficLightsNameGenerator -{ -public: - template - static std::string GetName(int) - { - if constexpr (std::is_same_v) - return "ConventionalTrafficLights"; - if constexpr (std::is_same_v) - return "V2ITrafficLights"; - } -}; - -// Declare typed test suite -TYPED_TEST_SUITE(TrafficLightsInternalTest, TrafficLightsTypes, TrafficLightsNameGenerator); - -// Define V2I type for use in tests with V2I traffic lights only -using V2ITrafficLightsTest = TrafficLightsInternalTest; - -TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsColor) -{ - using Color = traffic_simulator::TrafficLight::Color; - - this->lights->setTrafficLightsColor(this->id, Color::green); - EXPECT_FALSE( - this->lights->getTrafficLightsComposedState(this->id).find("green") == std::string::npos); - - this->lights->setTrafficLightsColor(this->id, Color::yellow); - EXPECT_FALSE( - this->lights->getTrafficLightsComposedState(this->id).find("yellow") == std::string::npos); - - this->lights->setTrafficLightsColor(this->id, Color::red); - EXPECT_FALSE( - this->lights->getTrafficLightsComposedState(this->id).find("red") == std::string::npos); - - this->lights->setTrafficLightsColor(this->id, Color::white); - EXPECT_FALSE( - this->lights->getTrafficLightsComposedState(this->id).find("white") == std::string::npos); -} - -TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsState_color) -{ - // green - this->lights->setTrafficLightsState(this->id, stateFromColor("green")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("green")); - - this->lights->setTrafficLightsState(this->id, stateFromColor("Green")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("green")); - - // red - this->lights->setTrafficLightsState(this->id, stateFromColor("red")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("red")); - - this->lights->setTrafficLightsState(this->id, stateFromColor("Red")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("red")); - - // yellow - this->lights->setTrafficLightsState(this->id, stateFromColor("yellow")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("yellow")); - - this->lights->setTrafficLightsState(this->id, stateFromColor("Yellow")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("yellow")); - - this->lights->setTrafficLightsState(this->id, stateFromColor("amber")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("yellow")); - - // white - this->lights->setTrafficLightsState(this->id, stateFromColor("white")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromColor("white")); -} - -TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsState_status) -{ - // solid on - this->lights->setTrafficLightsState(this->id, stateFromStatus("solidOn")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOn")); - - // solid off - this->lights->setTrafficLightsState(this->id, stateFromStatus("solidOff")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); - - this->lights->setTrafficLightsState(this->id, stateFromStatus("Blank")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); - - this->lights->setTrafficLightsState(this->id, stateFromStatus("none")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("solidOff")); - - // flashing - this->lights->setTrafficLightsState(this->id, stateFromStatus("flashing")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("flashing")); - - // unknown - this->lights->setTrafficLightsState(this->id, stateFromStatus("unknown")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromStatus("unknown")); -} - -TYPED_TEST(TrafficLightsInternalTest, setTrafficLightsState_shape) -{ - this->lights->setTrafficLightsState(this->id, stateFromShape("circle")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("circle")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("cross")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("cross")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("left")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("left")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("down")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("down")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("up")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("up")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("right")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("right")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("lowerLeft")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("lowerLeft")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("upperLeft")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("upperLeft")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("lowerRight")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("lowerRight")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("upperRight")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("upperRight")); - - this->lights->setTrafficLightsState(this->id, stateFromShape("straight")); - EXPECT_EQ(this->lights->getTrafficLightsComposedState(this->id), stateFromShape("up")); -} - -TYPED_TEST(TrafficLightsInternalTest, isAnyTrafficLightChanged) -{ - EXPECT_TRUE(this->lights->isAnyTrafficLightChanged()); -} - -TYPED_TEST(TrafficLightsInternalTest, isRequiredStopTrafficLightState) -{ - this->lights->setTrafficLightsState(this->id, stateFromColor("green")); - EXPECT_FALSE(this->lights->isRequiredStopTrafficLightState(this->id)); - - this->lights->setTrafficLightsState(this->id, stateFromColor("yellow")); - EXPECT_TRUE(this->lights->isRequiredStopTrafficLightState(this->id)); - - this->lights->setTrafficLightsState(this->id, "yellow flashing circle"); - EXPECT_FALSE(this->lights->isRequiredStopTrafficLightState(this->id)); - - this->lights->setTrafficLightsState(this->id, stateFromColor("red")); - EXPECT_TRUE(this->lights->isRequiredStopTrafficLightState(this->id)); -} - -TYPED_TEST(TrafficLightsInternalTest, compareTrafficLightsState) -{ - this->lights->setTrafficLightsState(this->id, stateFromColor("green")); - EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromColor("green"))); - EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromColor("Green"))); - EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("yellow"))); - EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("red"))); - EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("amber"))); - EXPECT_FALSE(this->lights->compareTrafficLightsState(this->id, stateFromColor("white"))); - - this->lights->setTrafficLightsState(this->id, stateFromStatus("Blank")); - EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromStatus("solidOff"))); - EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromStatus("Blank"))); - EXPECT_TRUE(this->lights->compareTrafficLightsState(this->id, stateFromStatus("none"))); -} - -TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers) -{ - this->lights->setTrafficLightsState(this->id, stateFromColor("red")); - - std::vector markers; - - rclcpp::Subscription::SharedPtr subscriber = - this->node_ptr->template create_subscription( - "traffic_light/marker", 10, - [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { - markers.push_back(*msg_in); - }); - - this->lights->startUpdate(20.0); - - // spin for 1 second - const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1005); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - // verify lambdas - const auto verify_delete_marker = - [](const visualization_msgs::msg::Marker & marker, const auto & info = "") { - EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETEALL) << info; - }; - - const auto verify_add_marker = []( - const visualization_msgs::msg::Marker & marker, - const auto info = "") { - EXPECT_EQ(marker.ns, "bulb") << info; - EXPECT_EQ(marker.id, 34836) << info; - EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE) << info; - EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD) << info; - - EXPECT_POINT_NEAR_STREAM( - marker.pose.position, - geometry_msgs::build().x(3770.02).y(73738.34).z(5.80), 0.01, info); - - EXPECT_QUATERNION_EQ_STREAM(marker.pose.orientation, geometry_msgs::msg::Quaternion(), info); - - EXPECT_POINT_EQ_STREAM( - marker.scale, geometry_msgs::build().x(0.3).y(0.3).z(0.3), info); - - EXPECT_COLOR_RGBA_EQ_STREAM( - marker.color, std_msgs::build().r(1.0).g(0.0).b(0.0).a(1.0), info); - }; - - std::vector headers; - - // verify - for (std::size_t i = 0; i < markers.size(); i += 2) { - { - const auto & one_marker = markers[i].markers; - EXPECT_EQ(one_marker.size(), static_cast(1)); - verify_delete_marker(one_marker[0], "marker " + std::to_string(i)); - } - { - const auto & one_marker = markers[i + 1].markers; - EXPECT_EQ(one_marker.size(), static_cast(1)); - verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); - - headers.push_back(one_marker[0].header); - } - } - - // verify message timing - const double expected_frequency = 20.0; - const double actual_frequency = - static_cast(headers.size() - 1) / - static_cast(getTime(headers.back()) - getTime(headers.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); -} - -TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers) -{ - this->lights->setTrafficLightsState(this->id, stateFromColor("green")); - - std::vector markers, markers_reset; - - rclcpp::Subscription::SharedPtr subscriber = - this->node_ptr->template create_subscription( - "traffic_light/marker", 10, - [&markers](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { - markers.push_back(*msg_in); - }); - - this->lights->startUpdate(20.0); - - // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - subscriber = this->node_ptr->template create_subscription( - "traffic_light/marker", 10, - [&markers_reset](const visualization_msgs::msg::MarkerArray::SharedPtr msg_in) { - markers_reset.push_back(*msg_in); - }); - - this->lights->resetUpdate(10.0); - end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - // verify lambdas - const auto verify_delete_marker = - [](const visualization_msgs::msg::Marker & marker, const auto & info = "") { - EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETEALL) << info; - }; - - const auto verify_add_marker = []( - const visualization_msgs::msg::Marker & marker, - const auto info = "") { - EXPECT_EQ(marker.ns, "bulb") << info; - EXPECT_EQ(marker.id, 34836) << info; - EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE) << info; - EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD) << info; - - EXPECT_POINT_NEAR_STREAM( - marker.pose.position, - geometry_msgs::build().x(3770.02).y(73738.34).z(5.80), 0.01, info); - - EXPECT_QUATERNION_EQ_STREAM(marker.pose.orientation, geometry_msgs::msg::Quaternion(), info); - - EXPECT_POINT_EQ_STREAM( - marker.scale, geometry_msgs::build().x(0.3).y(0.3).z(0.3), info); - - EXPECT_COLOR_RGBA_NEAR_STREAM( - marker.color, std_msgs::build().r(0.0).g(0.5).b(0.0).a(1.0), 0.01, - info); - }; - - std::vector headers, headers_reset; - - // verify - for (std::size_t i = 0; i < markers.size(); i += 2) { - { - const auto & one_marker = markers[i].markers; - EXPECT_EQ(one_marker.size(), static_cast(1)); - verify_delete_marker(one_marker[0], "marker " + std::to_string(i)); - } - { - const auto & one_marker = markers[i + 1].markers; - EXPECT_EQ(one_marker.size(), static_cast(1)); - verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); - - headers.push_back(one_marker[0].header); - } - } - for (std::size_t i = 0; i < markers_reset.size(); i += 2) { - { - const auto & one_marker = markers_reset[i].markers; - EXPECT_EQ(one_marker.size(), static_cast(1)); - verify_delete_marker(one_marker[0], "marker " + std::to_string(i)); - } - { - const auto & one_marker = markers_reset[i + 1].markers; - EXPECT_EQ(one_marker.size(), static_cast(1)); - verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1)); - - headers_reset.push_back(one_marker[0].header); - } - } - - // verify message timing - { - const double expected_frequency = 20.0; - const double actual_frequency = - static_cast(headers.size() - 1) / - static_cast(getTime(headers.back()) - getTime(headers.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); - } - { - const double expected_frequency = 10.0; - const double actual_frequency = - static_cast(headers_reset.size() - 1) / - static_cast(getTime(headers_reset.back()) - getTime(headers_reset.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); - } -} - -TYPED_TEST(TrafficLightsInternalTest, generateAutowarePerceptionMsg) -{ - this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights->setTrafficLightsConfidence(this->id, 0.7); - - const auto msg = this->lights->generateAutowarePerceptionMsg(); - - const double expected_time = - static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; - const double actual_time = static_cast(getTime(msg.stamp)) * 1e-9; - EXPECT_NEAR(actual_time, expected_time, timing_eps); - - EXPECT_EQ(msg.signals.size(), static_cast(1)); - EXPECT_EQ(msg.signals.front().elements.size(), static_cast(2)); - - EXPECT_EQ(msg.signals[0].traffic_signal_id, this->signal_id); - - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - // signals are parsed in reverse order - EXPECT_EQ(msg.signals[0].elements[0].color, TrafficSignalElement::AMBER); - EXPECT_EQ(msg.signals[0].elements[0].status, TrafficSignalElement::FLASHING); - EXPECT_EQ(msg.signals[0].elements[0].shape, TrafficSignalElement::CIRCLE); - EXPECT_NEAR(msg.signals[0].elements[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(msg.signals[0].elements[1].color, TrafficSignalElement::RED); - EXPECT_EQ(msg.signals[0].elements[1].status, TrafficSignalElement::SOLID_ON); - EXPECT_EQ(msg.signals[0].elements[1].shape, TrafficSignalElement::CIRCLE); - EXPECT_NEAR(msg.signals[0].elements[1].confidence, 0.7, 1e-6); -} - -TYPED_TEST(TrafficLightsInternalTest, generateAutowareAutoPerceptionMsg) -{ - this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights->setTrafficLightsConfidence(this->id, 0.7); - - const auto msg = this->lights->generateAutowareAutoPerceptionMsg(); - - const double expected_time = - static_cast(getTime(this->node_ptr->get_clock()->now())) * 1e-9; - const double actual_time = static_cast(getTime(msg.header)) * 1e-9; - EXPECT_NEAR(actual_time, expected_time, timing_eps); - - EXPECT_EQ(msg.signals.size(), static_cast(1)); - EXPECT_EQ(msg.signals.front().lights.size(), static_cast(2)); - - EXPECT_EQ(msg.header.frame_id, "camera_link"); - EXPECT_EQ(msg.signals[0].map_primitive_id, this->id); - - using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; - // signals are parsed in reverse order - EXPECT_EQ(msg.signals[0].lights[0].color, TrafficLight::AMBER); - EXPECT_EQ(msg.signals[0].lights[0].status, TrafficLight::FLASHING); - EXPECT_EQ(msg.signals[0].lights[0].shape, TrafficLight::CIRCLE); - EXPECT_NEAR(msg.signals[0].lights[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(msg.signals[0].lights[1].color, TrafficLight::RED); - EXPECT_EQ(msg.signals[0].lights[1].status, TrafficLight::SOLID_ON); - EXPECT_EQ(msg.signals[0].lights[1].shape, TrafficLight::CIRCLE); - EXPECT_NEAR(msg.signals[0].lights[1].confidence, 0.7, 1e-6); -} - -TEST_F(V2ITrafficLightsTest, startUpdate_publishSignals) -{ - using namespace autoware_perception_msgs::msg; - - this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights->setTrafficLightsConfidence(this->id, 0.7); - - std::vector signals; - - rclcpp::Subscription::SharedPtr subscriber = - this->node_ptr->create_subscription( - "/perception/traffic_light_recognition/external/traffic_signals", 10, - [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); - - this->lights->startUpdate(20.0); - - // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1005); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - std::vector stamps; - - for (std::size_t i = 0; i < signals.size(); ++i) { - stamps.push_back(signals[i].stamp); - - const auto & one_message = signals[i].signals; - std::string info = "signals message number " + std::to_string(i); - - EXPECT_EQ(one_message.size(), static_cast(1)) << info; - EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; - - EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; - - EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; - EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; - EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; - EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; - EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); - } - - // verify message timing - const double expected_frequency = 20.0; - const double actual_frequency = - static_cast(stamps.size() - 1) / - static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; - - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); -} - -TEST_F(V2ITrafficLightsTest, startUpdate_publishSignalsLegacy) -{ - using namespace autoware_perception_msgs::msg; - - this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights->setTrafficLightsConfidence(this->id, 0.7); - - std::vector signals; - - rclcpp::Subscription::SharedPtr subscriber = - this->node_ptr->create_subscription( - "/v2x/traffic_signals", 10, - [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); - - this->lights->startUpdate(20.0); - - // spin for 1 second - const auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(1005); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - std::vector stamps; - - for (std::size_t i = 0; i < signals.size(); ++i) { - stamps.push_back(signals[i].stamp); - - const auto & one_message = signals[i].signals; - std::string info = "signals message number " + std::to_string(i); - - EXPECT_EQ(one_message.size(), static_cast(1)) << info; - EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; - - EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; - - EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; - EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; - EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; - EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; - EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); - } - - // verify message timing - const double expected_frequency = 20.0; - const double actual_frequency = - static_cast(stamps.size() - 1) / - static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); -} - -TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignals) -{ - using namespace autoware_perception_msgs::msg; - - this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights->setTrafficLightsConfidence(this->id, 0.7); - - std::vector signals, signals_reset; - - rclcpp::Subscription::SharedPtr subscriber = - this->node_ptr->create_subscription( - "/perception/traffic_light_recognition/external/traffic_signals", 10, - [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); - - this->lights->startUpdate(20.0); - - // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - subscriber = this->node_ptr->create_subscription( - "/perception/traffic_light_recognition/external/traffic_signals", 10, - [&signals_reset](const TrafficSignalArray::SharedPtr msg_in) { - signals_reset.push_back(*msg_in); - }); - - this->lights->resetUpdate(10.0); - end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - std::vector stamps, stamps_reset; - - for (std::size_t i = 0; i < signals.size(); ++i) { - stamps.push_back(signals[i].stamp); - - const auto & one_message = signals[i].signals; - std::string info = "signals message number " + std::to_string(i); - - EXPECT_EQ(one_message.size(), static_cast(1)) << info; - EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; - - EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; - - EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; - EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; - EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; - EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; - EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); - } - - for (std::size_t i = 0; i < signals_reset.size(); ++i) { - stamps_reset.push_back(signals_reset[i].stamp); - - const auto & one_message = signals_reset[i].signals; - std::string info = "signals message number " + std::to_string(i); - - EXPECT_EQ(one_message.size(), static_cast(1)) << info; - EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; - - EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; - - EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; - EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; - EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; - EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; - EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); - } - - // verify message timing - { - const double expected_frequency = 20.0; - const double actual_frequency = - static_cast(stamps.size() - 1) / - static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); - } - { - const double expected_frequency = 10.0; - const double actual_frequency = - static_cast(stamps_reset.size() - 1) / - static_cast(getTime(stamps_reset.back()) - getTime(stamps_reset.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); - } -} - -TEST_F(V2ITrafficLightsTest, resetUpdate_publishSignalsLegacy) -{ - using namespace autoware_perception_msgs::msg; - - this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle"); - this->lights->setTrafficLightsConfidence(this->id, 0.7); - - std::vector signals, signals_reset; - - rclcpp::Subscription::SharedPtr subscriber = - this->node_ptr->create_subscription( - "/v2x/traffic_signals", 10, - [&signals](const TrafficSignalArray::SharedPtr msg_in) { signals.push_back(*msg_in); }); - - this->lights->startUpdate(20.0); - - // spin for 1 second - auto end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - subscriber = this->node_ptr->create_subscription( - "/v2x/traffic_signals", 10, [&signals_reset](const TrafficSignalArray::SharedPtr msg_in) { - signals_reset.push_back(*msg_in); - }); - - this->lights->resetUpdate(10.0); - end = std::chrono::system_clock::now() + std::chrono::milliseconds(505); - while (std::chrono::system_clock::now() < end) { - rclcpp::spin_some(this->node_ptr); - } - - std::vector stamps, stamps_reset; - - for (std::size_t i = 0; i < signals.size(); ++i) { - stamps.push_back(signals[i].stamp); - - const auto & one_message = signals[i].signals; - std::string info = "signals message number " + std::to_string(i); - - EXPECT_EQ(one_message.size(), static_cast(1)) << info; - EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; - - EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; - - EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; - EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; - EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; - EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; - EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); - } - - for (std::size_t i = 0; i < signals_reset.size(); ++i) { - stamps_reset.push_back(signals_reset[i].stamp); - - const auto & one_message = signals_reset[i].signals; - std::string info = "signals message number " + std::to_string(i); - - EXPECT_EQ(one_message.size(), static_cast(1)) << info; - EXPECT_EQ(one_message[0].traffic_signal_id, signal_id) << info; - - EXPECT_EQ(one_message[0].elements.size(), static_cast(2)) << info; - - EXPECT_EQ(one_message[0].elements[0].color, TrafficSignalElement::AMBER) << info; - EXPECT_EQ(one_message[0].elements[0].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[0].status, TrafficSignalElement::FLASHING) << info; - EXPECT_NEAR(one_message[0].elements[0].confidence, 0.7, 1e-6); - - EXPECT_EQ(one_message[0].elements[1].color, TrafficSignalElement::RED) << info; - EXPECT_EQ(one_message[0].elements[1].shape, TrafficSignalElement::CIRCLE) << info; - EXPECT_EQ(one_message[0].elements[1].status, TrafficSignalElement::SOLID_ON) << info; - EXPECT_NEAR(one_message[0].elements[1].confidence, 0.7, 1e-6); - } - - // verify message timing - { - const double expected_frequency = 20.0; - const double actual_frequency = - static_cast(stamps.size() - 1) / - static_cast(getTime(stamps.back()) - getTime(stamps.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); - } - { - const double expected_frequency = 10.0; - const double actual_frequency = - static_cast(stamps_reset.size() - 1) / - static_cast(getTime(stamps_reset.back()) - getTime(stamps_reset.front())) * 1e+9; - EXPECT_NEAR(actual_frequency, expected_frequency, frequency_eps); - } -} From b39433a978850cc3a78ddfe7ee2485ff45ea2e28 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Mon, 29 Jul 2024 11:32:05 +0200 Subject: [PATCH 30/45] Revert "ref(traffic_lights): rename rest of TrafficLight**->TrafficLights**" This reverts commit 87bcb80e204684c476e64c9db6fa5a2bb78e6313. Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/CMakeLists.txt | 14 +++++++------- .../traffic_simulator/entity/entity_manager.hpp | 4 ++-- ...sher.hpp => traffic_light_marker_publisher.hpp} | 10 +++++----- ...s_publisher.hpp => traffic_light_publisher.hpp} | 6 +++--- .../traffic_lights/traffic_lights.hpp | 2 +- .../traffic_lights/traffic_lights_base.hpp | 6 +++--- ...sher.cpp => traffic_light_marker_publisher.cpp} | 6 +++--- ...s_publisher.cpp => traffic_light_publisher.cpp} | 2 +- 8 files changed, 25 insertions(+), 25 deletions(-) rename simulation/traffic_simulator/include/traffic_simulator/traffic_lights/{traffic_lights_marker_publisher.hpp => traffic_light_marker_publisher.hpp} (81%) rename simulation/traffic_simulator/include/traffic_simulator/traffic_lights/{traffic_lights_publisher.hpp => traffic_light_publisher.hpp} (89%) rename simulation/traffic_simulator/src/traffic_lights/{traffic_lights_marker_publisher.cpp => traffic_light_marker_publisher.cpp} (87%) rename simulation/traffic_simulator/src/traffic_lights/{traffic_lights_publisher.cpp => traffic_light_publisher.cpp} (95%) diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 6b9e1b9b441..638ddd73fcf 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -45,18 +45,18 @@ ament_auto_add_library(traffic_simulator SHARED src/entity/vehicle_entity.cpp src/hdmap_utils/hdmap_utils.cpp src/helper/helper.cpp - src/job/job.cpp src/job/job_list.cpp + src/job/job.cpp src/simulation_clock/simulation_clock.cpp - src/traffic/traffic_controller.cpp - src/traffic/traffic_sink.cpp - src/traffic/traffic_source.cpp src/traffic_lights/configurable_rate_updater.cpp + src/traffic_lights/traffic_light_marker_publisher.cpp + src/traffic_lights/traffic_light_publisher.cpp src/traffic_lights/traffic_light.cpp - src/traffic_lights/traffic_lights.cpp src/traffic_lights/traffic_lights_base.cpp - src/traffic_lights/traffic_lights_marker_publisher.cpp - src/traffic_lights/traffic_lights_publisher.cpp + src/traffic_lights/traffic_lights.cpp + src/traffic/traffic_controller.cpp + src/traffic/traffic_sink.cpp + src/traffic/traffic_source.cpp src/utils/distance.cpp src/utils/pose.cpp ) 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 72c45da167b..3f24d946a43 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -40,9 +40,9 @@ #include #include #include +#include +#include #include -#include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_marker_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp similarity index 81% rename from simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_marker_publisher.hpp rename to simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp index 6ae4708cf3f..b19d3fd6333 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_marker_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_MARKER_PUBLISHER_HPP -#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_MARKER_PUBLISHER_HPP +#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP #include #include namespace traffic_simulator { -class TrafficLightsMarkerPublisher +class TrafficLightMarkerPublisher { public: template - explicit TrafficLightsMarkerPublisher( + explicit TrafficLightMarkerPublisher( const NodeTypePointer & node_ptr, const std::string & frame = "map") : frame_(frame), clock_ptr_(node_ptr->get_clock()), @@ -44,4 +44,4 @@ class TrafficLightsMarkerPublisher const rclcpp::Publisher::SharedPtr publisher_; }; } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_MARKER_PUBLISHER_HPP +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp similarity index 89% rename from simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp rename to simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp index 18761700a08..e51b1450222 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ -#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ +#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ #include #include @@ -57,4 +57,4 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; }; } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ 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 478e8e21574..be05d6cb716 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,8 +17,8 @@ #include #include +#include #include -#include namespace traffic_simulator { 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 f2f82c9d124..840fc767527 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 @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ class TrafficLightsBase const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) : hdmap_utils_(hdmap_utils), clock_ptr_(node_ptr->get_clock()), - marker_publisher_ptr_(std::make_unique(node_ptr)), + marker_publisher_ptr_(std::make_unique(node_ptr)), rate_updater_(node_ptr, [this]() { update(); }) { } @@ -95,7 +95,7 @@ class TrafficLightsBase const rclcpp::Clock::SharedPtr clock_ptr_; std::unordered_map traffic_lights_map_; - const std::unique_ptr marker_publisher_ptr_; + const std::unique_ptr marker_publisher_ptr_; ConfigurableRateUpdater rate_updater_; }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_marker_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp similarity index 87% rename from simulation/traffic_simulator/src/traffic_lights/traffic_lights_marker_publisher.cpp rename to simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp index 41e5d10a44b..225008624f7 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_marker_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include namespace traffic_simulator { -auto TrafficLightsMarkerPublisher::deleteMarkers() const -> void +auto TrafficLightMarkerPublisher::deleteMarkers() const -> void { visualization_msgs::msg::MarkerArray marker_array; visualization_msgs::msg::Marker marker; @@ -26,7 +26,7 @@ auto TrafficLightsMarkerPublisher::deleteMarkers() const -> void publisher_->publish(marker_array); } -auto TrafficLightsMarkerPublisher::drawMarkers( +auto TrafficLightMarkerPublisher::drawMarkers( const std::unordered_map & traffic_lights_map) const -> void { visualization_msgs::msg::MarkerArray marker_array; diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp similarity index 95% rename from simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp rename to simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index c49713dd080..caa770fd1d9 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include namespace traffic_simulator { From a06f4b4cae4643f7fa4ac8e7a328139ef4411b7b Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Mon, 29 Jul 2024 12:39:34 +0200 Subject: [PATCH 31/45] Rename traffic light classes to match filenames Signed-off-by: Mateusz Palczuk --- .../traffic_lights/traffic_light_publisher.hpp | 12 ++++++------ .../traffic_lights/traffic_lights.hpp | 12 ++++++------ .../src/traffic_lights/traffic_light_publisher.cpp | 6 +++--- 3 files changed, 15 insertions(+), 15 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 e51b1450222..b3a15f25b0a 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 @@ -24,22 +24,22 @@ namespace traffic_simulator { -class TrafficLightsPublisherBase +class TrafficLightPublisherBase { public: virtual auto publish(const TrafficLightsBase & traffic_lights) const -> void = 0; - virtual ~TrafficLightsPublisherBase() = default; + virtual ~TrafficLightPublisherBase() = default; }; template -class TrafficLightsPublisher : public TrafficLightsPublisherBase +class TrafficLightPublisher : public TrafficLightPublisherBase { public: template - explicit TrafficLightsPublisher( + explicit TrafficLightPublisher( const NodeTypePointer & node_ptr, const std::string & topic_name, const std::string & frame = "camera_link") - : TrafficLightsPublisherBase(), + : TrafficLightPublisherBase(), frame_(frame), clock_ptr_(node_ptr->get_clock()), traffic_light_state_array_publisher_(rclcpp::create_publisher( @@ -47,7 +47,7 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase { } - ~TrafficLightsPublisher() override = default; + ~TrafficLightPublisher() override = default; auto publish(const TrafficLightsBase & traffic_lights) const -> void override; 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 be05d6cb716..3299e8df1bc 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 @@ -30,7 +30,7 @@ class ConventionalTrafficLights : public TrafficLightsBase const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) : TrafficLightsBase(node_ptr, hdmap_utils), backward_compatible_publisher_ptr_( - std::make_unique>( + std::make_unique>( node_ptr, "/simulation/traffic_lights")) { } @@ -47,7 +47,7 @@ class ConventionalTrafficLights : public TrafficLightsBase marker_publisher_ptr_->drawMarkers(traffic_lights_map_); } - const std::unique_ptr backward_compatible_publisher_ptr_; + const std::unique_ptr backward_compatible_publisher_ptr_; }; class V2ITrafficLights : public TrafficLightsBase @@ -81,7 +81,7 @@ class V2ITrafficLights : public TrafficLightsBase template auto makePublisher( const NodeTypePointer & node_ptr, const std::string & architecture_type, - const std::string & topic_name) -> std::unique_ptr + const std::string & topic_name) -> std::unique_ptr { /* Here autoware_perception_msgs is used for all awf/universe/.... @@ -93,7 +93,7 @@ class V2ITrafficLights : public TrafficLightsBase */ if (architecture_type.find("awf/universe") != std::string::npos) { return std::make_unique< - TrafficLightsPublisher>( + TrafficLightPublisher>( node_ptr, topic_name); } else { throw common::SemanticError( @@ -102,8 +102,8 @@ class V2ITrafficLights : public TrafficLightsBase } } - const std::unique_ptr publisher_ptr_; - const std::unique_ptr legacy_topic_publisher_ptr_; + const std::unique_ptr publisher_ptr_; + const std::unique_ptr legacy_topic_publisher_ptr_; }; class TrafficLights 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 caa770fd1d9..dd4b795af05 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -19,21 +19,21 @@ namespace traffic_simulator { template <> -auto TrafficLightsPublisher::publish( +auto TrafficLightPublisher::publish( const TrafficLightsBase & traffic_lights) const -> void { traffic_light_state_array_publisher_->publish(traffic_lights.generateAutowareAutoPerceptionMsg()); } template <> -auto TrafficLightsPublisher::publish( +auto TrafficLightPublisher::publish( const TrafficLightsBase & traffic_lights) const -> void { traffic_light_state_array_publisher_->publish(traffic_lights.generateAutowarePerceptionMsg()); } template <> -auto TrafficLightsPublisher::publish( +auto TrafficLightPublisher::publish( const TrafficLightsBase & traffic_lights) const -> void { traffic_light_state_array_publisher_->publish(traffic_lights.generateTrafficSimulatorV1Msg()); From 0656f86c5101e620c038c90853779680c5cc9ba4 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 2 Oct 2024 12:17:28 +0200 Subject: [PATCH 32/45] Fix spelling Signed-off-by: Mateusz Palczuk --- .../traffic_lights/traffic_lights_publisher.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index f79a38e4c7b..ff95f6fea7c 100644 --- 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 @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TTRAFFIC_LIGHT_PUBLISHER_HPP_ -#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TTRAFFIC_LIGHT_PUBLISHER_HPP_ +#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 @@ -53,4 +53,4 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase }; } // namespace traffic_lights } // namespace simple_sensor_simulator -#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TTRAFFIC_LIGHT_PUBLISHER_HPP_ +#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ From d8c1fcf47089bbec51428d318f34b83626e3d58d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 9 Oct 2024 18:14:07 +0200 Subject: [PATCH 33/45] Add traffic light status unknown & Take advantage of compile time enum check Use switch without 'default' so that compiler warns when any case is not covered Signed-off-by: Mateusz Palczuk --- .../traffic_lights/traffic_light.hpp | 142 ++++++++++++------ 1 file changed, 92 insertions(+), 50 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index bdec753c2d6..27d416999fb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -256,64 +256,84 @@ struct TrafficLight template < typename MessageT, typename std::enable_if_t< - std::is_same_v || + std::is_same_v or std::is_same_v, int> = 0> explicit operator MessageT() const { const auto color = [this] { + auto color = MessageT::UNKNOWN; switch (std::get(value).value) { case Color::green: - return MessageT::GREEN; + color = MessageT::GREEN; + break; case Color::yellow: - return MessageT::AMBER; + color = MessageT::AMBER; + break; case Color::red: - return MessageT::RED; + color = MessageT::RED; + break; case Color::white: - return MessageT::WHITE; - default: - return MessageT::UNKNOWN; + color = MessageT::WHITE; + break; } + return color; }; const auto status = [this] { + auto status = MessageT::UNKNOWN; switch (std::get(value).value) { case Status::solid_on: - return MessageT::SOLID_ON; + status = MessageT::SOLID_ON; + break; case Status::solid_off: - return MessageT::SOLID_OFF; + status = MessageT::SOLID_OFF; + break; case Status::flashing: - return MessageT::FLASHING; - default: - return MessageT::UNKNOWN; + status = MessageT::FLASHING; + break; + case Status::unknown: + status = MessageT::UNKNOWN; + break; } + return status; }; const auto shape = [this] { + auto shape = MessageT::UNKNOWN; switch (std::get(value).value) { case Shape::circle: - return MessageT::CIRCLE; + shape = MessageT::CIRCLE; + break; case Shape::cross: - return MessageT::CROSS; + shape = MessageT::CROSS; + break; case Shape::left: - return MessageT::LEFT_ARROW; + shape = MessageT::LEFT_ARROW; + break; case Shape::down: - return MessageT::DOWN_ARROW; + shape = MessageT::DOWN_ARROW; + break; case Shape::up: - return MessageT::UP_ARROW; + shape = MessageT::UP_ARROW; + break; case Shape::right: - return MessageT::RIGHT_ARROW; + shape = MessageT::RIGHT_ARROW; + break; case Shape::lower_left: - return MessageT::DOWN_LEFT_ARROW; + shape = MessageT::DOWN_LEFT_ARROW; + break; case Shape::lower_right: - return MessageT::DOWN_RIGHT_ARROW; + shape = MessageT::DOWN_RIGHT_ARROW; + break; case Shape::upper_left: - return MessageT::UP_LEFT_ARROW; + shape = MessageT::UP_LEFT_ARROW; + break; case Shape::upper_right: - return MessageT::UP_RIGHT_ARROW; - default: - return MessageT::UNKNOWN; - }; + shape = MessageT::UP_RIGHT_ARROW; + break; + } + return shape; }; MessageT msg; @@ -329,62 +349,84 @@ struct TrafficLight // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported explicit operator autoware_perception_msgs::msg::TrafficSignalElement() const { + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + auto color = [this]() { + auto color = TrafficSignalElement::UNKNOWN; switch (std::get(value).value) { case Color::green: - return autoware_perception_msgs::msg::TrafficSignalElement::GREEN; + color = TrafficSignalElement::GREEN; + break; case Color::yellow: - return autoware_perception_msgs::msg::TrafficSignalElement::AMBER; + color = TrafficSignalElement::AMBER; + break; case Color::red: - return autoware_perception_msgs::msg::TrafficSignalElement::RED; + color = TrafficSignalElement::RED; + break; case Color::white: - return autoware_perception_msgs::msg::TrafficSignalElement::WHITE; - default: - throw common::SyntaxError(std::get(value), " is not supported color."); + color = TrafficSignalElement::WHITE; + break; } + return color; }; auto status = [this]() { + auto status = TrafficSignalElement::UNKNOWN; switch (std::get(value).value) { case Status::solid_on: - return autoware_perception_msgs::msg::TrafficSignalElement::SOLID_ON; + status = TrafficSignalElement::SOLID_ON; + break; case Status::solid_off: - return autoware_perception_msgs::msg::TrafficSignalElement::SOLID_OFF; + status = TrafficSignalElement::SOLID_OFF; + break; case Status::flashing: - return autoware_perception_msgs::msg::TrafficSignalElement::FLASHING; - default: - throw common::SyntaxError(std::get(value), " is not supported color."); + status = TrafficSignalElement::FLASHING; + break; + case Status::unknown: + status = TrafficSignalElement::UNKNOWN; + break; } + return status; }; auto shape = [this]() { + auto shape = TrafficSignalElement::UNKNOWN; switch (std::get(value).value) { case Shape::circle: - return autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE; + shape = TrafficSignalElement::CIRCLE; + break; case Shape::cross: - return autoware_perception_msgs::msg::TrafficSignalElement::CROSS; + shape = TrafficSignalElement::CROSS; + break; case Shape::left: - return autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW; + shape = TrafficSignalElement::LEFT_ARROW; + break; case Shape::down: - return autoware_perception_msgs::msg::TrafficSignalElement::DOWN_ARROW; + shape = TrafficSignalElement::DOWN_ARROW; + break; case Shape::up: - return autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW; + shape = TrafficSignalElement::UP_ARROW; + break; case Shape::right: - return autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW; + shape = TrafficSignalElement::RIGHT_ARROW; + break; case Shape::lower_left: - return autoware_perception_msgs::msg::TrafficSignalElement::DOWN_LEFT_ARROW; + shape = TrafficSignalElement::DOWN_LEFT_ARROW; + break; case Shape::lower_right: - return autoware_perception_msgs::msg::TrafficSignalElement::DOWN_RIGHT_ARROW; + shape = TrafficSignalElement::DOWN_RIGHT_ARROW; + break; case Shape::upper_left: - return autoware_perception_msgs::msg::TrafficSignalElement::UP_LEFT_ARROW; + shape = TrafficSignalElement::UP_LEFT_ARROW; + break; case Shape::upper_right: - return autoware_perception_msgs::msg::TrafficSignalElement::UP_RIGHT_ARROW; - default: - throw common::SyntaxError(std::get(value), " is not supported color."); - }; + shape = TrafficSignalElement::UP_RIGHT_ARROW; + break; + } + return shape; }; - autoware_perception_msgs::msg::TrafficSignalElement msg; + TrafficSignalElement msg; msg.color = color(); msg.status = status(); msg.shape = shape(); From 1e7a1b14e286b013aa5f53cfa248a6bb0716a5ec Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 10 Oct 2024 16:27:26 +0200 Subject: [PATCH 34/45] Fix comment Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/entity/entity_manager.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 3f24d946a43..47c98cd586c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -103,9 +103,9 @@ class EntityManager public: /** - This function is necessary because the TrafficLights object is created after the EntityManager, - so it can be assigned during the call of the EntityManager constructor. - TrafficLights cannot be created before the EntityManager due to the dependency on HdMapUtils. + * This function is necessary because the TrafficLights object is created after the EntityManager, + * so it cannot be assigned during the call of the EntityManager constructor. + * TrafficLights cannot be created before the EntityManager due to the dependency on HdMapUtils. */ auto setTrafficLights( const std::shared_ptr & traffic_lights_ptr) -> void From a956863034e05aec234412325370c64cc3704b10 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 10 Oct 2024 17:31:13 +0200 Subject: [PATCH 35/45] Fix comment style Signed-off-by: Mateusz Palczuk --- .../traffic_lights/traffic_lights_base.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 840fc767527..d2f7a6f96a3 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 @@ -31,9 +31,9 @@ namespace traffic_simulator { /* - TrafficLightsBase class is designed in such a way that while trying to perform an operation - on a TrafficLight (add, set, etc.) that is not added to traffic_light_map_, - it adds it first and then performs the operation, so that the methods + TrafficLightsBase class is designed in such a way that while trying to perform an operation + on a TrafficLight (add, set, etc.) that is not added to traffic_light_map_, + it adds the traffic light first and then performs the operation, so that the methods here cannot be tagged with the "const" specifier */ class TrafficLightsBase From e33cd93388d99fbac9b3540e5bc8ddab07174d30 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 14 Oct 2024 13:59:29 +0200 Subject: [PATCH 36/45] fix(traffic_lights): improve the meaning of comments --- .../traffic_lights/traffic_lights_detector.hpp | 8 ++++++-- .../traffic_lights/traffic_lights.hpp | 12 ++++++------ 2 files changed, 12 insertions(+), 8 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 7b76b22aa14..fd64f5f32d2 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 @@ -51,8 +51,12 @@ class TrafficLightsDetector -> std::unique_ptr { /* - V2ITrafficLights in TrafficSimulator publishes using topics "/v2x/traffic_signals" and - "/perception/traffic_light_recognition/external/traffic_signals" for all "awf/universe/..." + 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; 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 3299e8df1bc..aa2cd52226f 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 @@ -84,12 +84,12 @@ class V2ITrafficLights : public TrafficLightsBase const std::string & topic_name) -> std::unique_ptr { /* - Here autoware_perception_msgs is used for all awf/universe/.... - Perhaps autoware_auto_perception_msgs should be used for >= "awf/universe/20230906"? - - PseudoTrafficLightsDetector in SimpleSensorSimulator publishes using topic - "/perception/traffic_light_recognition/traffic_signals" for >= "awf/universe/20230906" - otherwise uses "/perception/traffic_light_recognition/internal/traffic_signals". + V2ITrafficLights in TrafficSimulator publishes publishes using architecture-independent topics ("awf/universe..."): + "/v2x/traffic_signals" and "/perception/traffic_light_recognition/external/traffic_signals" + + 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" */ if (architecture_type.find("awf/universe") != std::string::npos) { return std::make_unique< From c5a2aa6a4fa4db066ff9290b7962783da2bb69bc Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 16 Oct 2024 11:02:52 +0200 Subject: [PATCH 37/45] Fix copyright notice date Signed-off-by: Mateusz Palczuk --- .../sensor_simulation/imu/imu_sensor.hpp | 2 +- .../traffic_lights/traffic_lights_detector.hpp | 2 +- .../traffic_lights/traffic_lights_publisher.hpp | 2 +- .../src/sensor_simulation/imu/imu_sensor.cpp | 2 +- .../traffic_lights/traffic_lights_publisher.cpp | 2 +- .../traffic_lights/configurable_rate_updater.hpp | 2 +- .../traffic_lights/traffic_light_marker_publisher.hpp | 2 +- .../traffic_lights/traffic_light_publisher.hpp | 2 +- .../include/traffic_simulator/traffic_lights/traffic_lights.hpp | 2 +- .../traffic_simulator/traffic_lights/traffic_lights_base.hpp | 2 +- .../include/traffic_simulator/utils/distance.hpp | 2 +- .../include/traffic_simulator/utils/node_parameters.hpp | 2 +- .../traffic_simulator/include/traffic_simulator/utils/pose.hpp | 2 +- .../src/traffic_lights/configurable_rate_updater.cpp | 2 +- .../src/traffic_lights/traffic_light_marker_publisher.cpp | 2 +- .../src/traffic_lights/traffic_light_publisher.cpp | 2 +- .../traffic_simulator/src/traffic_lights/traffic_lights.cpp | 2 +- .../src/traffic_lights/traffic_lights_base.cpp | 2 +- simulation/traffic_simulator/src/utils/distance.cpp | 2 +- simulation/traffic_simulator/src/utils/pose.cpp | 2 +- 20 files changed, 20 insertions(+), 20 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp index 03a1c7313a7..2672a1e2987 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp +++ b/simulation/simple_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/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 fd64f5f32d2..a93fc1e9dd4 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 @@ -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/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 index ff95f6fea7c..87ce6f2f61a 100644 --- 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 @@ -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/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp index 9a86e1a3731..6fbb73119fb 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp @@ -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/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 index 5cab1aa4c0f..36b680138c2 100644 --- 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 @@ -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/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp index b6604f45e63..8833fd138b3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.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/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp index b19d3fd6333..7cdcedb61b8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.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/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 b3a15f25b0a..618dda3a59a 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 @@ -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/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp index aa2cd52226f..2689e6b9ad3 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 @@ -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/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 d2f7a6f96a3..37c03c0af14 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 @@ -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/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index f269ecdc0f9..80b1a287e88 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.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/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp index 5114d9997af..c24f848e462 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.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/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 5a0f6cae523..37428420fde 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.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/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp index b638b08f5a6..6929aac1b14 100644 --- a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp @@ -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/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp index 225008624f7..e447f35d385 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp @@ -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/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index dd4b795af05..91035d0ce38 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -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/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp index b233cce9158..5f19b6a31f0 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp @@ -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/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp index 9e3294dba63..e750a534e73 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -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/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index af12722401e..3d5bba9926a 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -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/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 5603d3909ab..5cc0c5ffcfa 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -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. From 3aca48f81e27783e80a4cff7e973edba509348ac Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 17 Oct 2024 08:36:53 +0200 Subject: [PATCH 38/45] Use explicit in traffic lights constructors Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/traffic_lights/traffic_lights.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 2689e6b9ad3..e2ae7cbd4b7 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 @@ -26,7 +26,7 @@ class ConventionalTrafficLights : public TrafficLightsBase { public: template - ConventionalTrafficLights( + explicit ConventionalTrafficLights( const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) : TrafficLightsBase(node_ptr, hdmap_utils), backward_compatible_publisher_ptr_( @@ -54,7 +54,7 @@ class V2ITrafficLights : public TrafficLightsBase { public: template - V2ITrafficLights( + explicit V2ITrafficLights( const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils, const std::string & architecture_type) : TrafficLightsBase(node_ptr, hdmap_utils), From 62e6fd610717930cd38ba1c821ff2c982715fdea Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 17 Oct 2024 08:37:53 +0200 Subject: [PATCH 39/45] Change traffic_lights_msg to traffic_lights_message Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/api/api.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 5c7bc4c73fb..72cf8744d1a 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -291,9 +291,9 @@ bool API::updateTrafficLightsInSim() { if (traffic_lights_ptr_->isAnyTrafficLightChanged()) { simulation_api_schema::UpdateTrafficLightsRequest request; - const auto traffic_lights_msg = + const auto traffic_lights_message = traffic_lights_ptr_->getConventionalTrafficLights()->generateAutowareAutoPerceptionMsg(); - simulation_interface::toProto(traffic_lights_msg, request); + simulation_interface::toProto(traffic_lights_message, request); // here is a lack of information in autoware_auto_perception_msgs::msg // to complete the relation_ids, so complete it manually here for (auto & traffic_signal : *request.mutable_states()) { From 9ba77014af1d226e5fc3b46d16f387715f7d5c65 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 17 Oct 2024 08:38:21 +0200 Subject: [PATCH 40/45] Use alternative operators Signed-off-by: Mateusz Palczuk --- .../src/traffic_lights/configurable_rate_updater.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp index 6929aac1b14..7858114ccb0 100644 --- a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp @@ -29,7 +29,7 @@ auto ConfigurableRateUpdater::startTimer(const double update_rate) -> void auto ConfigurableRateUpdater::resetTimer(const double update_rate) -> void { - if (timer_ && !timer_->is_canceled()) { + if (timer_ and not timer_->is_canceled()) { timer_->cancel(); timer_.reset(); } From c31313d9bfe407f6f138374b85a6b794ea71e695 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 17 Oct 2024 09:13:58 +0200 Subject: [PATCH 41/45] Change variable names to be different from lambda names - color => color_message - status => status_message - shape => shape_message Signed-off-by: Mateusz Palczuk --- .../traffic_lights/traffic_light.hpp | 96 +++++++++---------- 1 file changed, 48 insertions(+), 48 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index 27d416999fb..db4c8972071 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -262,78 +262,78 @@ struct TrafficLight explicit operator MessageT() const { const auto color = [this] { - auto color = MessageT::UNKNOWN; + auto color_message = MessageT::UNKNOWN; switch (std::get(value).value) { case Color::green: - color = MessageT::GREEN; + color_message = MessageT::GREEN; break; case Color::yellow: - color = MessageT::AMBER; + color_message = MessageT::AMBER; break; case Color::red: - color = MessageT::RED; + color_message = MessageT::RED; break; case Color::white: - color = MessageT::WHITE; + color_message = MessageT::WHITE; break; } - return color; + return color_message; }; const auto status = [this] { - auto status = MessageT::UNKNOWN; + auto status_message = MessageT::UNKNOWN; switch (std::get(value).value) { case Status::solid_on: - status = MessageT::SOLID_ON; + status_message = MessageT::SOLID_ON; break; case Status::solid_off: - status = MessageT::SOLID_OFF; + status_message = MessageT::SOLID_OFF; break; case Status::flashing: - status = MessageT::FLASHING; + status_message = MessageT::FLASHING; break; case Status::unknown: - status = MessageT::UNKNOWN; + status_message = MessageT::UNKNOWN; break; } - return status; + return status_message; }; const auto shape = [this] { - auto shape = MessageT::UNKNOWN; + auto shape_message = MessageT::UNKNOWN; switch (std::get(value).value) { case Shape::circle: - shape = MessageT::CIRCLE; + shape_message = MessageT::CIRCLE; break; case Shape::cross: - shape = MessageT::CROSS; + shape_message = MessageT::CROSS; break; case Shape::left: - shape = MessageT::LEFT_ARROW; + shape_message = MessageT::LEFT_ARROW; break; case Shape::down: - shape = MessageT::DOWN_ARROW; + shape_message = MessageT::DOWN_ARROW; break; case Shape::up: - shape = MessageT::UP_ARROW; + shape_message = MessageT::UP_ARROW; break; case Shape::right: - shape = MessageT::RIGHT_ARROW; + shape_message = MessageT::RIGHT_ARROW; break; case Shape::lower_left: - shape = MessageT::DOWN_LEFT_ARROW; + shape_message = MessageT::DOWN_LEFT_ARROW; break; case Shape::lower_right: - shape = MessageT::DOWN_RIGHT_ARROW; + shape_message = MessageT::DOWN_RIGHT_ARROW; break; case Shape::upper_left: - shape = MessageT::UP_LEFT_ARROW; + shape_message = MessageT::UP_LEFT_ARROW; break; case Shape::upper_right: - shape = MessageT::UP_RIGHT_ARROW; + shape_message = MessageT::UP_RIGHT_ARROW; break; } - return shape; + return shape_message; }; MessageT msg; @@ -352,78 +352,78 @@ struct TrafficLight using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; auto color = [this]() { - auto color = TrafficSignalElement::UNKNOWN; + auto color_message = TrafficSignalElement::UNKNOWN; switch (std::get(value).value) { case Color::green: - color = TrafficSignalElement::GREEN; + color_message = TrafficSignalElement::GREEN; break; case Color::yellow: - color = TrafficSignalElement::AMBER; + color_message = TrafficSignalElement::AMBER; break; case Color::red: - color = TrafficSignalElement::RED; + color_message = TrafficSignalElement::RED; break; case Color::white: - color = TrafficSignalElement::WHITE; + color_message = TrafficSignalElement::WHITE; break; } - return color; + return color_message; }; auto status = [this]() { - auto status = TrafficSignalElement::UNKNOWN; + auto status_message = TrafficSignalElement::UNKNOWN; switch (std::get(value).value) { case Status::solid_on: - status = TrafficSignalElement::SOLID_ON; + status_message = TrafficSignalElement::SOLID_ON; break; case Status::solid_off: - status = TrafficSignalElement::SOLID_OFF; + status_message = TrafficSignalElement::SOLID_OFF; break; case Status::flashing: - status = TrafficSignalElement::FLASHING; + status_message = TrafficSignalElement::FLASHING; break; case Status::unknown: - status = TrafficSignalElement::UNKNOWN; + status_message = TrafficSignalElement::UNKNOWN; break; } - return status; + return status_message; }; auto shape = [this]() { - auto shape = TrafficSignalElement::UNKNOWN; + auto shape_message = TrafficSignalElement::UNKNOWN; switch (std::get(value).value) { case Shape::circle: - shape = TrafficSignalElement::CIRCLE; + shape_message = TrafficSignalElement::CIRCLE; break; case Shape::cross: - shape = TrafficSignalElement::CROSS; + shape_message = TrafficSignalElement::CROSS; break; case Shape::left: - shape = TrafficSignalElement::LEFT_ARROW; + shape_message = TrafficSignalElement::LEFT_ARROW; break; case Shape::down: - shape = TrafficSignalElement::DOWN_ARROW; + shape_message = TrafficSignalElement::DOWN_ARROW; break; case Shape::up: - shape = TrafficSignalElement::UP_ARROW; + shape_message = TrafficSignalElement::UP_ARROW; break; case Shape::right: - shape = TrafficSignalElement::RIGHT_ARROW; + shape_message = TrafficSignalElement::RIGHT_ARROW; break; case Shape::lower_left: - shape = TrafficSignalElement::DOWN_LEFT_ARROW; + shape_message = TrafficSignalElement::DOWN_LEFT_ARROW; break; case Shape::lower_right: - shape = TrafficSignalElement::DOWN_RIGHT_ARROW; + shape_message = TrafficSignalElement::DOWN_RIGHT_ARROW; break; case Shape::upper_left: - shape = TrafficSignalElement::UP_LEFT_ARROW; + shape_message = TrafficSignalElement::UP_LEFT_ARROW; break; case Shape::upper_right: - shape = TrafficSignalElement::UP_RIGHT_ARROW; + shape_message = TrafficSignalElement::UP_RIGHT_ARROW; break; } - return shape; + return shape_message; }; TrafficSignalElement msg; From de0e44a52c0c2cd7a09c0330f1d7bef77023008b Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 30 Oct 2024 14:57:25 +0100 Subject: [PATCH 42/45] ref(traffic_simulator): remove autoware::msgs dependency, go back to a proto-centric conversion architecture --- .../simulation_interface/conversions.hpp | 15 -- .../simulation_interface/src/conversions.cpp | 90 -------- .../traffic_lights/traffic_light.hpp | 202 +++++++----------- .../traffic_lights/traffic_lights_base.hpp | 8 +- simulation/traffic_simulator/src/api/api.cpp | 16 +- .../traffic_light_publisher.cpp | 43 +++- .../traffic_lights/traffic_lights_base.cpp | 42 +--- 7 files changed, 135 insertions(+), 281 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 15f95868e72..4ec8320616b 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -23,9 +23,6 @@ #include #include -#include -#include -#include #include #include #include @@ -188,18 +185,6 @@ auto toProto( autoware_auto_vehicle_msgs::msg::GearCommand> &, traffic_simulator_msgs::VehicleCommand &) -> void; -auto toProto( - const autoware_auto_perception_msgs::msg::TrafficLight & message, - simulation_api_schema::TrafficLight & proto) -> void; - -auto toProto( - const autoware_auto_perception_msgs::msg::TrafficSignal & message, - simulation_api_schema::TrafficSignal & proto) -> void; - -auto toProto( - const autoware_auto_perception_msgs::msg::TrafficSignalArray & message, - simulation_api_schema::UpdateTrafficLightsRequest & proto) -> void; - template auto toMsg( const simulation_api_schema::TrafficLight & proto, diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index 7b40b973ff6..1804eb11c1a 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -603,96 +603,6 @@ auto toProto( toProto(std::get<1>(message), *proto.mutable_gear_command()); } -auto toProto( - const autoware_auto_perception_msgs::msg::TrafficLight & message, - simulation_api_schema::TrafficLight & proto) -> void -{ - auto color = [&]() { - switch (message.color) { - case autoware_auto_perception_msgs::msg::TrafficLight::GREEN: - return simulation_api_schema::TrafficLight_Color_GREEN; - case autoware_auto_perception_msgs::msg::TrafficLight::AMBER: - return simulation_api_schema::TrafficLight_Color_AMBER; - case autoware_auto_perception_msgs::msg::TrafficLight::RED: - return simulation_api_schema::TrafficLight_Color_RED; - case autoware_auto_perception_msgs::msg::TrafficLight::WHITE: - return simulation_api_schema::TrafficLight_Color_WHITE; - default: - return simulation_api_schema::TrafficLight_Color_UNKNOWN_COLOR; - } - }; - - auto status = [&]() { - switch (message.status) { - case autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON: - return simulation_api_schema::TrafficLight_Status_SOLID_ON; - case autoware_auto_perception_msgs::msg::TrafficLight::SOLID_OFF: - return simulation_api_schema::TrafficLight_Status_SOLID_OFF; - case autoware_auto_perception_msgs::msg::TrafficLight::FLASHING: - return simulation_api_schema::TrafficLight_Status_FLASHING; - default: - return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; - } - }; - - auto shape = [&]() { - switch (message.shape) { - case autoware_auto_perception_msgs::msg::TrafficLight::CIRCLE: - return simulation_api_schema::TrafficLight_Shape_CIRCLE; - case autoware_auto_perception_msgs::msg::TrafficLight::CROSS: - return simulation_api_schema::TrafficLight_Shape_CROSS; - case autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW: - return simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; - case autoware_auto_perception_msgs::msg::TrafficLight::DOWN_ARROW: - return simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; - case autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW: - return simulation_api_schema::TrafficLight_Shape_UP_ARROW; - case autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW: - return simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; - case autoware_auto_perception_msgs::msg::TrafficLight::DOWN_LEFT_ARROW: - return simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; - case autoware_auto_perception_msgs::msg::TrafficLight::DOWN_RIGHT_ARROW: - return simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; - case autoware_auto_perception_msgs::msg::TrafficLight::UP_LEFT_ARROW: - return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; - case autoware_auto_perception_msgs::msg::TrafficLight::UP_RIGHT_ARROW: - return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; - default: - return simulation_api_schema::TrafficLight_Shape_UNKNOWN_SHAPE; - } - }; - - proto.set_status(status()); - proto.set_shape(shape()); - proto.set_color(color()); - proto.set_confidence(message.confidence); -} - -auto toProto( - const autoware_auto_perception_msgs::msg::TrafficSignal & message, - simulation_api_schema::TrafficSignal & proto) -> void -{ - // here is a lack of information in autoware_auto_perception_msgs::msg - // to complete relation_ids in simulation_api_schema::TrafficSignal - proto.set_id(message.map_primitive_id); - for (const auto & traffic_light : message.lights) { - simulation_api_schema::TrafficLight traffic_light_proto; - toProto(traffic_light, traffic_light_proto); - *proto.add_traffic_light_status() = traffic_light_proto; - } -} - -auto toProto( - const autoware_auto_perception_msgs::msg::TrafficSignalArray & message, - simulation_api_schema::UpdateTrafficLightsRequest & proto) -> void -{ - for (const auto & traffic_signal : message.signals) { - simulation_api_schema::TrafficSignal traffic_signal_proto; - toProto(traffic_signal, traffic_signal_proto); - *proto.add_states() = traffic_signal_proto; - } -} - auto toProtobufMessage(const traffic_simulator_msgs::msg::Vertex & message) -> traffic_simulator_msgs::Vertex { diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index db4c8972071..dd4238087ed 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -15,9 +15,8 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_ -#include -#include -#include +#include + #include #include #include @@ -253,185 +252,160 @@ struct TrafficLight friend auto operator<<(std::ostream & os, const Bulb & bulb) -> std::ostream &; - template < - typename MessageT, - typename std::enable_if_t< - std::is_same_v or - std::is_same_v, - int> = 0> - explicit operator MessageT() const + explicit operator simulation_api_schema::TrafficLight() const { - const auto color = [this] { - auto color_message = MessageT::UNKNOWN; + auto color = [this]() { switch (std::get(value).value) { case Color::green: - color_message = MessageT::GREEN; - break; + return simulation_api_schema::TrafficLight_Color_GREEN; case Color::yellow: - color_message = MessageT::AMBER; - break; + return simulation_api_schema::TrafficLight_Color_AMBER; case Color::red: - color_message = MessageT::RED; - break; + return simulation_api_schema::TrafficLight_Color_RED; case Color::white: - color_message = MessageT::WHITE; - break; + return simulation_api_schema::TrafficLight_Color_WHITE; + default: + throw common::SyntaxError(std::get(value), " is not supported color."); } - return color_message; }; - const auto status = [this] { - auto status_message = MessageT::UNKNOWN; + auto status = [this]() { switch (std::get(value).value) { case Status::solid_on: - status_message = MessageT::SOLID_ON; - break; + return simulation_api_schema::TrafficLight_Status_SOLID_ON; case Status::solid_off: - status_message = MessageT::SOLID_OFF; - break; + return simulation_api_schema::TrafficLight_Status_SOLID_OFF; case Status::flashing: - status_message = MessageT::FLASHING; - break; + return simulation_api_schema::TrafficLight_Status_FLASHING; case Status::unknown: - status_message = MessageT::UNKNOWN; - break; + return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; + default: + throw common::SyntaxError(std::get(value), " is not supported as a status."); } - return status_message; }; - const auto shape = [this] { - auto shape_message = MessageT::UNKNOWN; + auto shape = [this]() { switch (std::get(value).value) { case Shape::circle: - shape_message = MessageT::CIRCLE; - break; + return simulation_api_schema::TrafficLight_Shape_CIRCLE; case Shape::cross: - shape_message = MessageT::CROSS; - break; + return simulation_api_schema::TrafficLight_Shape_CROSS; case Shape::left: - shape_message = MessageT::LEFT_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; case Shape::down: - shape_message = MessageT::DOWN_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; case Shape::up: - shape_message = MessageT::UP_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_UP_ARROW; case Shape::right: - shape_message = MessageT::RIGHT_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; case Shape::lower_left: - shape_message = MessageT::DOWN_LEFT_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; case Shape::lower_right: - shape_message = MessageT::DOWN_RIGHT_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; case Shape::upper_left: - shape_message = MessageT::UP_LEFT_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; case Shape::upper_right: - shape_message = MessageT::UP_RIGHT_ARROW; - break; + return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; + default: + throw common::SyntaxError(std::get(value), " is not supported as a shape."); } - return shape_message; }; - MessageT msg; - msg.color = color(); - msg.status = status(); - msg.shape = shape(); - // NOTE: confidence will be overwritten - msg.confidence = 1.0; - // NOTE: unused data member 'enum_revision' for traffic_simulator_msgs::msg::TrafficLightBulbV1 - return msg; + simulation_api_schema::TrafficLight traffic_light_bulb_proto; + traffic_light_bulb_proto.set_status(status()); + traffic_light_bulb_proto.set_shape(shape()); + traffic_light_bulb_proto.set_color(color()); + // NOTE: confidence will be overwritten in TrafficLight::operator simulation_api_schema::TrafficSignal() + traffic_light_bulb_proto.set_confidence(1.0); + + return traffic_light_bulb_proto; } - // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported - explicit operator autoware_perception_msgs::msg::TrafficSignalElement() const + explicit operator traffic_simulator_msgs::msg::TrafficLightBulbV1() const { - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - - auto color = [this]() { - auto color_message = TrafficSignalElement::UNKNOWN; + const auto color = [this] { + auto color = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; switch (std::get(value).value) { case Color::green: - color_message = TrafficSignalElement::GREEN; + color = traffic_simulator_msgs::msg::TrafficLightBulbV1::GREEN; break; case Color::yellow: - color_message = TrafficSignalElement::AMBER; + color = traffic_simulator_msgs::msg::TrafficLightBulbV1::AMBER; break; case Color::red: - color_message = TrafficSignalElement::RED; + color = traffic_simulator_msgs::msg::TrafficLightBulbV1::RED; break; case Color::white: - color_message = TrafficSignalElement::WHITE; + color = traffic_simulator_msgs::msg::TrafficLightBulbV1::WHITE; break; } - return color_message; + return color; }; - auto status = [this]() { - auto status_message = TrafficSignalElement::UNKNOWN; + const auto status = [this] { + auto status = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; switch (std::get(value).value) { case Status::solid_on: - status_message = TrafficSignalElement::SOLID_ON; + status = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_ON; break; case Status::solid_off: - status_message = TrafficSignalElement::SOLID_OFF; + status = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_OFF; break; case Status::flashing: - status_message = TrafficSignalElement::FLASHING; + status = traffic_simulator_msgs::msg::TrafficLightBulbV1::FLASHING; break; case Status::unknown: - status_message = TrafficSignalElement::UNKNOWN; + status = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; break; } - return status_message; + return status; }; - auto shape = [this]() { - auto shape_message = TrafficSignalElement::UNKNOWN; + const auto shape = [this] { + auto shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; switch (std::get(value).value) { case Shape::circle: - shape_message = TrafficSignalElement::CIRCLE; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::CIRCLE; break; case Shape::cross: - shape_message = TrafficSignalElement::CROSS; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::CROSS; break; case Shape::left: - shape_message = TrafficSignalElement::LEFT_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::LEFT_ARROW; break; case Shape::down: - shape_message = TrafficSignalElement::DOWN_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_ARROW; break; case Shape::up: - shape_message = TrafficSignalElement::UP_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_ARROW; break; case Shape::right: - shape_message = TrafficSignalElement::RIGHT_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::RIGHT_ARROW; break; case Shape::lower_left: - shape_message = TrafficSignalElement::DOWN_LEFT_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_LEFT_ARROW; break; case Shape::lower_right: - shape_message = TrafficSignalElement::DOWN_RIGHT_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_RIGHT_ARROW; break; case Shape::upper_left: - shape_message = TrafficSignalElement::UP_LEFT_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_LEFT_ARROW; break; case Shape::upper_right: - shape_message = TrafficSignalElement::UP_RIGHT_ARROW; + shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_RIGHT_ARROW; break; } - return shape_message; + return shape; }; - TrafficSignalElement msg; + traffic_simulator_msgs::msg::TrafficLightBulbV1 msg; msg.color = color(); msg.status = status(); msg.shape = shape(); // NOTE: confidence will be overwritten msg.confidence = 1.0; + // NOTE: unused data member 'enum_revision' for + // traffic_simulator_msgs::msg::TrafficLightBulbV1 return msg; } }; @@ -503,16 +477,17 @@ struct TrafficLight friend auto operator<<(std::ostream & os, const TrafficLight & traffic_light) -> std::ostream &; - explicit operator autoware_auto_perception_msgs::msg::TrafficSignal() const + explicit operator simulation_api_schema::TrafficSignal() const { - autoware_auto_perception_msgs::msg::TrafficSignal traffic_signal; - traffic_signal.map_primitive_id = way_id; + simulation_api_schema::TrafficSignal traffic_signal_proto; + + traffic_signal_proto.set_id(way_id); for (const auto & bulb : bulbs) { - auto traffic_light_bulb = static_cast(bulb); - traffic_light_bulb.confidence = confidence; - traffic_signal.lights.push_back(traffic_light_bulb); + auto traffic_light_bulb_proto = static_cast(bulb); + traffic_light_bulb_proto.set_confidence(confidence); + *traffic_signal_proto.add_traffic_light_status() = traffic_light_bulb_proto; } - return traffic_signal; + return traffic_signal_proto; } explicit operator traffic_simulator_msgs::msg::TrafficLightV1() const @@ -526,29 +501,6 @@ struct TrafficLight } return traffic_signal; } - - // it will be removed when autoware_perception_msgs::msg::TrafficSignal is no longer supported - explicit operator std::vector() const - { - // skip if the traffic light has no bulbs - if (bulbs.empty()) { - return {}; - } else { - std::vector traffic_signals; - for (const auto & regulatory_element : regulatory_elements_ids) { - autoware_perception_msgs::msg::TrafficSignal traffic_signal; - traffic_signal.traffic_signal_id = regulatory_element; - for (const auto & bulb : bulbs) { - auto traffic_light_bulb = - static_cast(bulb); - traffic_light_bulb.confidence = confidence; - traffic_signal.elements.push_back(traffic_light_bulb); - } - traffic_signals.push_back(traffic_signal); - } - return traffic_signals; - } - } }; } // namespace traffic_simulator 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 37c03c0af14..7dd959fa168 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 @@ -15,6 +15,8 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ +#include + #include #include #include @@ -72,10 +74,8 @@ class TrafficLightsBase auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string; - auto generateAutowarePerceptionMsg() const -> autoware_perception_msgs::msg::TrafficSignalArray; - - auto generateAutowareAutoPerceptionMsg() const - -> autoware_auto_perception_msgs::msg::TrafficSignalArray; + auto generateUpdateTrafficLightsRequest() const + -> simulation_api_schema::UpdateTrafficLightsRequest; auto generateTrafficSimulatorV1Msg() const -> traffic_simulator_msgs::msg::TrafficLightArrayV1; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 72cf8744d1a..1f24e201b3c 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -290,20 +290,8 @@ bool API::updateTimeInSim() bool API::updateTrafficLightsInSim() { if (traffic_lights_ptr_->isAnyTrafficLightChanged()) { - simulation_api_schema::UpdateTrafficLightsRequest request; - const auto traffic_lights_message = - traffic_lights_ptr_->getConventionalTrafficLights()->generateAutowareAutoPerceptionMsg(); - simulation_interface::toProto(traffic_lights_message, request); - // here is a lack of information in autoware_auto_perception_msgs::msg - // to complete the relation_ids, so complete it manually here - for (auto & traffic_signal : *request.mutable_states()) { - const auto relation_ids = - entity_manager_ptr_->getHdmapUtils()->getTrafficLightRegulatoryElementIDsFromTrafficLight( - traffic_signal.id()); - for (const auto & relation_id : relation_ids) { - traffic_signal.add_relation_ids(relation_id); - } - } + auto request = + traffic_lights_ptr_->getConventionalTrafficLights()->generateUpdateTrafficLightsRequest(); return zeromq_client_.call(request).result().success(); } /// @todo handle response 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 91035d0ce38..d957cad3430 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,7 @@ #include #include +#include #include namespace traffic_simulator @@ -22,14 +23,52 @@ template <> auto TrafficLightPublisher::publish( const TrafficLightsBase & traffic_lights) const -> void { - traffic_light_state_array_publisher_->publish(traffic_lights.generateAutowareAutoPerceptionMsg()); + const auto request = traffic_lights.generateUpdateTrafficLightsRequest(); + autoware_auto_perception_msgs::msg::TrafficSignalArray message; + using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; + message.header.frame_id = "camera_link"; // DIRTY HACK!!! + message.header.stamp = clock_ptr_->now(); + 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()) { + using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; + TrafficLightBulbType light_bulb_message; + simulation_interface::toMsg(bulb_status, light_bulb_message); + traffic_light_message.lights.push_back(light_bulb_message); + } + message.signals.push_back(traffic_light_message); + } + traffic_light_state_array_publisher_->publish(message); } template <> auto TrafficLightPublisher::publish( const TrafficLightsBase & traffic_lights) const -> void { - traffic_light_state_array_publisher_->publish(traffic_lights.generateAutowarePerceptionMsg()); + const auto request = traffic_lights.generateUpdateTrafficLightsRequest(); + autoware_perception_msgs::msg::TrafficSignalArray message; + message.stamp = clock_ptr_->now(); + 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()) { + using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; + TrafficLightType traffic_light_message; + traffic_light_message.traffic_signal_id = relation_id; + + for (const auto & bulb_status : traffic_light.traffic_light_status()) { + using TrafficLightBulbType = + autoware_perception_msgs::msg::TrafficSignal::_elements_type::value_type; + TrafficLightBulbType light_bulb_message; + simulation_interface::toMsg(bulb_status, light_bulb_message); + traffic_light_message.elements.push_back(light_bulb_message); + } + message.signals.push_back(traffic_light_message); + } + } + } + traffic_light_state_array_publisher_->publish(message); } template <> 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 e750a534e73..620a666d68c 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -93,38 +93,18 @@ auto TrafficLightsBase::getTrafficLightsComposedState(const lanelet::Id lanelet_ return ss.str(); } -auto TrafficLightsBase::generateAutowarePerceptionMsg() const - -> autoware_perception_msgs::msg::TrafficSignalArray -{ - autoware_perception_msgs::msg::TrafficSignalArray traffic_lights_message; - traffic_lights_message.stamp = clock_ptr_->now(); - - size_t total_number_of_signals{0}; - for (const auto & [id, traffic_light] : traffic_lights_map_) { - total_number_of_signals += traffic_light.regulatory_elements_ids.size(); - } - traffic_lights_message.signals.reserve(total_number_of_signals); - - for (const auto & [id, traffic_light] : traffic_lights_map_) { - const auto traffic_signals = - static_cast>(traffic_light); - traffic_lights_message.signals.insert( - traffic_lights_message.signals.end(), traffic_signals.begin(), traffic_signals.end()); - } - return traffic_lights_message; -} - -auto TrafficLightsBase::generateAutowareAutoPerceptionMsg() const - -> autoware_auto_perception_msgs::msg::TrafficSignalArray -{ - autoware_auto_perception_msgs::msg::TrafficSignalArray traffic_lights_message; - traffic_lights_message.header.frame_id = "camera_link"; /// DIRTY HACK! - traffic_lights_message.header.stamp = clock_ptr_->now(); - for (const auto & [id, traffic_light] : traffic_lights_map_) { - traffic_lights_message.signals.push_back( - static_cast(traffic_light)); +auto TrafficLightsBase::generateUpdateTrafficLightsRequest() const + -> simulation_api_schema::UpdateTrafficLightsRequest +{ + simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; + for (auto && [lanelet_id, traffic_light] : traffic_lights_map_) { + auto traffic_signal = static_cast(traffic_light); + for (const auto & relation_id : traffic_light.regulatory_elements_ids) { + traffic_signal.add_relation_ids(relation_id); + } + *update_traffic_lights_request.add_states() = traffic_signal; } - return traffic_lights_message; + return update_traffic_lights_request; } auto TrafficLightsBase::generateTrafficSimulatorV1Msg() const From 44a600db9d10d39310acbb27eb712276733037fd Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 6 Nov 2024 22:47:39 +0100 Subject: [PATCH 43/45] ref(traffic_simulator): simple traffic_light_publisher refactor --- .../traffic_light_publisher.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) 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 d957cad3430..dfaee1e9098 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -23,16 +23,18 @@ template <> auto TrafficLightPublisher::publish( const TrafficLightsBase & traffic_lights) const -> void { - const auto request = traffic_lights.generateUpdateTrafficLightsRequest(); + const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest(); autoware_auto_perception_msgs::msg::TrafficSignalArray message; - using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; + message.header.frame_id = "camera_link"; // DIRTY HACK!!! message.header.stamp = clock_ptr_->now(); - for (const auto & traffic_light : request.states()) { + + 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()) { TrafficLightType traffic_light_message; traffic_light_message.map_primitive_id = traffic_light.id(); for (const auto & bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; TrafficLightBulbType light_bulb_message; simulation_interface::toMsg(bulb_status, light_bulb_message); traffic_light_message.lights.push_back(light_bulb_message); @@ -46,20 +48,21 @@ template <> auto TrafficLightPublisher::publish( const TrafficLightsBase & traffic_lights) const -> void { - const auto request = traffic_lights.generateUpdateTrafficLightsRequest(); + const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest(); autoware_perception_msgs::msg::TrafficSignalArray message; + message.stamp = clock_ptr_->now(); - for (const auto & traffic_light : request.states()) { + + 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 & relation_id : traffic_light.relation_ids()) { // skip if the traffic light has no bulbs if (not traffic_light.traffic_light_status().empty()) { - using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; TrafficLightType traffic_light_message; traffic_light_message.traffic_signal_id = relation_id; - for (const auto & bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = - autoware_perception_msgs::msg::TrafficSignal::_elements_type::value_type; TrafficLightBulbType light_bulb_message; simulation_interface::toMsg(bulb_status, light_bulb_message); traffic_light_message.elements.push_back(light_bulb_message); From b9bdd82db09144f566e348f133f93ba54cafe2d9 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 7 Nov 2024 13:28:54 +0100 Subject: [PATCH 44/45] Remove unused headers Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/traffic_lights/traffic_lights_base.hpp | 2 -- 1 file changed, 2 deletions(-) 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 7dd959fa168..8220cf8d6a5 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 @@ -17,8 +17,6 @@ #include -#include -#include #include #include #include From 91c6b12e13008d056e95069ae6b0b886da5e37b0 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 7 Nov 2024 13:32:09 +0100 Subject: [PATCH 45/45] Utilize compile time check for TrafficLight Do not use 'default' in switch over enum to get compilation error when not all cases are considered Signed-off-by: Mateusz Palczuk --- .../traffic_lights/traffic_light.hpp | 120 ++++++++++-------- 1 file changed, 69 insertions(+), 51 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index dd4238087ed..e6a92f122a0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -254,61 +254,79 @@ struct TrafficLight explicit operator simulation_api_schema::TrafficLight() const { - auto color = [this]() { + const auto color = [this]() { + auto color_message = simulation_api_schema::TrafficLight_Color_UNKNOWN_COLOR; switch (std::get(value).value) { case Color::green: - return simulation_api_schema::TrafficLight_Color_GREEN; + color_message = simulation_api_schema::TrafficLight_Color_GREEN; + break; case Color::yellow: - return simulation_api_schema::TrafficLight_Color_AMBER; + color_message = simulation_api_schema::TrafficLight_Color_AMBER; + break; case Color::red: - return simulation_api_schema::TrafficLight_Color_RED; + color_message = simulation_api_schema::TrafficLight_Color_RED; + break; case Color::white: - return simulation_api_schema::TrafficLight_Color_WHITE; - default: - throw common::SyntaxError(std::get(value), " is not supported color."); + color_message = simulation_api_schema::TrafficLight_Color_WHITE; + break; } + return color_message; }; - auto status = [this]() { + const auto status = [this]() { + auto status_message = simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; switch (std::get(value).value) { case Status::solid_on: - return simulation_api_schema::TrafficLight_Status_SOLID_ON; + status_message = simulation_api_schema::TrafficLight_Status_SOLID_ON; + break; case Status::solid_off: - return simulation_api_schema::TrafficLight_Status_SOLID_OFF; + status_message = simulation_api_schema::TrafficLight_Status_SOLID_OFF; + break; case Status::flashing: - return simulation_api_schema::TrafficLight_Status_FLASHING; + status_message = simulation_api_schema::TrafficLight_Status_FLASHING; + break; case Status::unknown: - return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; - default: - throw common::SyntaxError(std::get(value), " is not supported as a status."); + status_message = simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; + break; } + return status_message; }; - auto shape = [this]() { + const auto shape = [this]() { + auto shape_message = simulation_api_schema::TrafficLight_Shape_UNKNOWN_SHAPE; switch (std::get(value).value) { case Shape::circle: - return simulation_api_schema::TrafficLight_Shape_CIRCLE; + shape_message = simulation_api_schema::TrafficLight_Shape_CIRCLE; + break; case Shape::cross: - return simulation_api_schema::TrafficLight_Shape_CROSS; + shape_message = simulation_api_schema::TrafficLight_Shape_CROSS; + break; case Shape::left: - return simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; + break; case Shape::down: - return simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; + break; case Shape::up: - return simulation_api_schema::TrafficLight_Shape_UP_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_UP_ARROW; + break; case Shape::right: - return simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; + break; case Shape::lower_left: - return simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; + break; case Shape::lower_right: - return simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; + break; case Shape::upper_left: - return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; + break; case Shape::upper_right: - return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; - default: - throw common::SyntaxError(std::get(value), " is not supported as a shape."); + shape_message = simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; + break; } + return shape_message; }; simulation_api_schema::TrafficLight traffic_light_bulb_proto; @@ -324,78 +342,78 @@ struct TrafficLight explicit operator traffic_simulator_msgs::msg::TrafficLightBulbV1() const { const auto color = [this] { - auto color = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + auto color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; switch (std::get(value).value) { case Color::green: - color = traffic_simulator_msgs::msg::TrafficLightBulbV1::GREEN; + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::GREEN; break; case Color::yellow: - color = traffic_simulator_msgs::msg::TrafficLightBulbV1::AMBER; + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::AMBER; break; case Color::red: - color = traffic_simulator_msgs::msg::TrafficLightBulbV1::RED; + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::RED; break; case Color::white: - color = traffic_simulator_msgs::msg::TrafficLightBulbV1::WHITE; + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::WHITE; break; } - return color; + return color_message; }; const auto status = [this] { - auto status = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + auto status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; switch (std::get(value).value) { case Status::solid_on: - status = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_ON; + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_ON; break; case Status::solid_off: - status = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_OFF; + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_OFF; break; case Status::flashing: - status = traffic_simulator_msgs::msg::TrafficLightBulbV1::FLASHING; + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::FLASHING; break; case Status::unknown: - status = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; break; } - return status; + return status_message; }; const auto shape = [this] { - auto shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + auto shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; switch (std::get(value).value) { case Shape::circle: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::CIRCLE; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::CIRCLE; break; case Shape::cross: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::CROSS; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::CROSS; break; case Shape::left: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::LEFT_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::LEFT_ARROW; break; case Shape::down: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_ARROW; break; case Shape::up: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_ARROW; break; case Shape::right: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::RIGHT_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::RIGHT_ARROW; break; case Shape::lower_left: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_LEFT_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_LEFT_ARROW; break; case Shape::lower_right: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_RIGHT_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_RIGHT_ARROW; break; case Shape::upper_left: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_LEFT_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_LEFT_ARROW; break; case Shape::upper_right: - shape = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_RIGHT_ARROW; + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_RIGHT_ARROW; break; } - return shape; + return shape_message; }; traffic_simulator_msgs::msg::TrafficLightBulbV1 msg;