diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 708861c3307..6b6a4a5b5c4 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -56,8 +56,8 @@ ament_auto_add_library(traffic_simulator SHARED 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_light_marker_publisher.cpp + src/traffic_lights/traffic_light_publisher.cpp src/utils/distance.cpp src/utils/node_parameters.cpp src/utils/pose.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index d90d92a0ab2..802e4d51459 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -165,5 +165,4 @@ class EgoEntity : public VehicleEntity }; } // namespace entity } // namespace traffic_simulator - #endif // TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_ 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 77% 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..b3a15f25b0a 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 @@ -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; @@ -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..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 @@ -17,8 +17,8 @@ #include #include +#include #include -#include namespace traffic_simulator { @@ -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/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 79% 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..dd4b795af05 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -14,26 +14,26 @@ #include #include -#include +#include 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()); 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); - } -}