diff --git a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp index 2fb23334578..b9f794effb3 100644 --- a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp @@ -57,6 +57,8 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode api_.resetBehaviorPlugin( "pedestrian_spawn_with_behavior_tree", traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing()); + + /// @note After the reset, the Entity objects are invalidated, so we need to obtain new ones. if ( api_.getEntity("vehicle_spawn_with_behavior_tree")->getCurrentAction() != "do_nothing" || api_.getEntity("pedestrian_spawn_with_behavior_tree")->getCurrentAction() != "do_nothing") { diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index b28e33e5de8..0c4d56e5410 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -63,7 +63,7 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(15); + ego_entity->setLinearVelocity(15); auto npc_entity = api_.spawn( "npc", diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp index a9c3a99e6c6..3e20a4db95d 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -44,14 +44,14 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { - auto npc = api_.getEntity("npc"); - auto ego = api_.getEntity("ego"); + auto ego_entity = api_.getEntity("ego"); + auto npc_entity = api_.getEntity("npc"); // SUCCESS if ( - npc->requestSynchronize("ego", ego_target, npc_target, 0, 0.5) && - ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) && - npc->getCurrentTwist().linear.x < 0.5) { + npc_entity->requestSynchronize("ego", ego_target, npc_target, 0, 0.5) && + ego_entity->reachPosition(ego_target, 1.0) && npc_entity->reachPosition(npc_target, 1.0) && + npc_entity->getCurrentTwist().linear.x < 0.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } @@ -65,34 +65,31 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode } void onInitialize() override { - api_.spawn( + auto ego_entity = api_.spawn( "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34976, 20, 0, api_.getHdmapUtils()), getVehicleParameters()); - auto ego = api_.getEntity("ego"); - ego->setLinearVelocity(3); - ego->requestSpeedChange(3, true); + ego_entity->setLinearVelocity(3); + ego_entity->requestSpeedChange(3, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 20, 0, api_.getHdmapUtils())); - ego->requestAssignRoute(goal_poses); + ego_entity->requestAssignRoute(goal_poses); - api_.spawn( + auto npc_entity = api_.spawn( "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 0, 0, api_.getHdmapUtils()), getVehicleParameters()); - auto npc = api_.getEntity("npc"); - std::vector npc_goal_poses; npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34564, 20, 0, api_.getHdmapUtils())); - npc->requestAssignRoute(npc_goal_poses); - npc->setLinearVelocity(6); + npc_entity->requestAssignRoute(npc_goal_poses); + npc_entity->setLinearVelocity(6); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp index 5395b90f857..f7c4a3fabd5 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp @@ -44,14 +44,14 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { - auto npc = api_.getEntity("npc"); - auto ego = api_.getEntity("ego"); + auto npc_entity = api_.getEntity("npc"); + auto ego_entity = api_.getEntity("ego"); // SUCCESS if ( - npc->requestSynchronize("ego", ego_target, npc_target, 2, 0.5) && - ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) && - npc->getCurrentTwist().linear.x < 2.5) { + npc_entity->requestSynchronize("ego", ego_target, npc_target, 2, 0.5) && + ego_entity->reachPosition(ego_target, 1.0) && npc_entity->reachPosition(npc_target, 1.0) && + npc_entity->getCurrentTwist().linear.x < 2.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } @@ -63,36 +63,34 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); } } + void onInitialize() override { - api_.spawn( + auto ego_entity = api_.spawn( "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34976, 20, 0, api_.getHdmapUtils()), getVehicleParameters()); - auto ego = api_.getEntity("ego"); - ego->setLinearVelocity(3); - ego->requestSpeedChange(3, true); + ego_entity->setLinearVelocity(3); + ego_entity->requestSpeedChange(3, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 20, 0, api_.getHdmapUtils())); - ego->requestAssignRoute(goal_poses); + ego_entity->requestAssignRoute(goal_poses); - api_.spawn( + auto npc_entity = api_.spawn( "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 0, 0, api_.getHdmapUtils()), getVehicleParameters()); - auto npc = api_.getEntity("npc"); - std::vector npc_goal_poses; npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34564, 20, 0, api_.getHdmapUtils())); - npc->requestAssignRoute(npc_goal_poses); - npc->setLinearVelocity(6); + npc_entity->requestAssignRoute(npc_goal_poses); + npc_entity->setLinearVelocity(6); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 68656f7bae9..723d6d08be3 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -156,6 +156,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode auto obstacle_entity = api_.spawn( "obstacle", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), getMiscObjectParameters()); + obstacle_entity->setStatus( ego_entity->getMapPose(), traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), traffic_simulator::helper::constructActionStatus()); 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 4d6c15e291a..6f247f480bc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -73,7 +73,7 @@ class EntityBase : public std::enable_shared_from_this } } - template + template /* */ auto as() const -> std::shared_ptr { if (auto derived = std::dynamic_pointer_cast(shared_from_this()); !derived) { 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/CMakeLists.txt b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt index 3bc7c5c8d69..3ed41676649 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt @@ -1,2 +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 + 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 new file mode 100644 index 00000000000..8872be966b1 --- /dev/null +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp @@ -0,0 +1,781 @@ +// 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); + } +}