Merge remote-tracking branch 'tier4/RJD-1057-traffic-lights-tests' in…

…to RJD-1057-remove-functions-forwarded-to-entity-base-middle

Signed-off-by: Mateusz Palczuk <[email protected]>

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_

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)

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 <rclcpp/time.hpp>
+#include <std_msgs/msg/header.hpp>
+
+/// 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<int>(1e+9);
+  return static_cast<int>(time.sec) * nanosecond_multiplier + static_cast<int>(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<int>(time.nanoseconds());
+}
+
+#endif  // TRAFFIC_SIMULATOR__TEST__TRAFFIC_LIGHTS__HELPER_HPP_

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 <gtest/gtest.h>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <traffic_simulator/traffic_lights/traffic_lights.hpp>
+#include <traffic_simulator/traffic_lights/traffic_lights_base.hpp>
+
+#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::HdMapUtils> hdmap_utils_ptr =
+    std::make_shared<hdmap_utils::HdMapUtils>(
+      path, geographic_msgs::build<geographic_msgs::msg::GeoPoint>()
+              .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<traffic_simulator::TrafficLights> lights =
+    std::make_unique<traffic_simulator::TrafficLights>(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<visualization_msgs::msg::MarkerArray> markers;
+
+  rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr subscriber =
+    this->node_ptr->template create_subscription<visualization_msgs::msg::MarkerArray>(
+      "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<std_msgs::msg::Header> 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<std::size_t>(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<double>(headers.size()) /
+    static_cast<double>(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();
+}