feat(autoware_test_utils): add traffic light msgs parser (#9177)

Signed-off-by: Mamoru Sobue <[email protected]>

common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp

@@ -16,9 +16,11 @@
 #define AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_
 
 #include <builtin_interfaces/msg/duration.hpp>
+#include <builtin_interfaces/msg/time.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 #include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/msg/lanelet_segment.hpp>
@@ -36,16 +38,22 @@
 
 namespace autoware::test_utils
 {
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_perception_msgs::msg::ObjectClassification;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjectKinematics;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_perception_msgs::msg::PredictedPath;
 using autoware_perception_msgs::msg::Shape;
+using autoware_perception_msgs::msg::TrafficLightElement;
+using autoware_perception_msgs::msg::TrafficLightGroup;
+using autoware_perception_msgs::msg::TrafficLightGroupArray;
+
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::LaneletRoute;
 using autoware_planning_msgs::msg::LaneletSegment;
 using builtin_interfaces::msg::Duration;
+using builtin_interfaces::msg::Time;
 using geometry_msgs::msg::Accel;
 using geometry_msgs::msg::AccelWithCovariance;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
@@ -79,6 +87,9 @@ Header parse(const YAML::Node & node);
 template <>
 Duration parse(const YAML::Node & node);
 
+template <>
+Time parse(const YAML::Node & node);
+
 template <>
 std::vector<Point> parse(const YAML::Node & node);
 
@@ -142,6 +153,18 @@ PredictedObject parse(const YAML::Node & node);
 template <>
 PredictedObjects parse(const YAML::Node & node);
 
+template <>
+TrafficLightGroupArray parse(const YAML::Node & node);
+
+template <>
+TrafficLightGroup parse(const YAML::Node & node);
+
+template <>
+TrafficLightElement parse(const YAML::Node & node);
+
+template <>
+OperationModeState parse(const YAML::Node & node);
+
 /**
  * @brief Parses a YAML file and converts it into an object of type T.
  *

common/autoware_test_utils/src/mock_data_parser.cpp

@@ -44,6 +44,15 @@ Duration parse(const YAML::Node & node)
   return msg;
 }
 
+template <>
+Time parse(const YAML::Node & node)
+{
+  Time msg;
+  msg.sec = node["sec"].as<int>();
+  msg.nanosec = node["nanosec"].as<int>();
+  return msg;
+}
+
 template <>
 std::array<double, 36> parse(const YAML::Node & node)
 {
@@ -321,6 +330,54 @@ PredictedObjects parse(const YAML::Node & node)
   return msg;
 }
 
+template <>
+TrafficLightElement parse(const YAML::Node & node)
+{
+  TrafficLightElement msg;
+  msg.color = node["color"].as<uint8_t>();
+  msg.shape = node["shape"].as<uint8_t>();
+  msg.status = node["status"].as<uint8_t>();
+  msg.confidence = node["confidence"].as<float>();
+  return msg;
+}
+
+template <>
+TrafficLightGroup parse(const YAML::Node & node)
+{
+  TrafficLightGroup msg;
+  msg.traffic_light_group_id = node["traffic_light_group_id"].as<int>();
+  for (const auto & element_node : node["elements"]) {
+    msg.elements.push_back(parse<TrafficLightElement>(element_node));
+  }
+  return msg;
+}
+
+template <>
+TrafficLightGroupArray parse(const YAML::Node & node)
+{
+  TrafficLightGroupArray msg;
+  msg.stamp = parse<Time>(node["stamp"]);
+  for (const auto & traffic_light_group_node : node["traffic_light_groups"]) {
+    msg.traffic_light_groups.push_back(parse<TrafficLightGroup>(traffic_light_group_node));
+  }
+  return msg;
+}
+
+template <>
+OperationModeState parse(const YAML::Node & node)
+{
+  OperationModeState msg;
+  msg.stamp = parse<Time>(node["stamp"]);
+  msg.mode = node["mode"].as<uint8_t>();
+  msg.is_autoware_control_enabled = node["is_autoware_control_enabled"].as<bool>();
+  msg.is_in_transition = node["is_in_transition"].as<bool>();
+  msg.is_stop_mode_available = node["is_stop_mode_available"].as<bool>();
+  msg.is_autonomous_mode_available = node["is_autonomous_mode_available"].as<bool>();
+  msg.is_local_mode_available = node["is_local_mode_available"].as<bool>();
+  msg.is_remote_mode_available = node["is_remote_mode_available"].as<bool>();
+  return msg;
+}
+
 template <>
 LaneletRoute parse(const std::string & filename)
 {

common/autoware_test_utils/test/test_mock_data_parser.cpp

@@ -448,6 +448,17 @@ const char g_complete_yaml[] = R"(
           shape: 1
           status: 2
           confidence: 1.00000
+operation_mode:
+  stamp:
+    sec: 0
+    nanosec: 204702031
+  mode: 1
+  is_autoware_control_enabled: true
+  is_in_transition: false
+  is_stop_mode_available: true
+  is_autonomous_mode_available: true
+  is_local_mode_available: true
+  is_remote_mode_available: true
 )";
 
 TEST(ParseFunctions, CompleteYAMLTest)
@@ -541,6 +552,26 @@ TEST(ParseFunctions, CompleteYAMLTest)
   EXPECT_EQ(dynamic_object.objects.at(0).shape.type, 0);
   EXPECT_DOUBLE_EQ(dynamic_object.objects.at(0).shape.dimensions.x, 4.40000);
   EXPECT_DOUBLE_EQ(dynamic_object.objects.at(0).shape.dimensions.y, 1.85564);
+
+  const auto traffic_signal = parse<TrafficLightGroupArray>(config["traffic_signal"]);
+  EXPECT_EQ(traffic_signal.stamp.sec, 1730184609);
+  EXPECT_EQ(traffic_signal.stamp.nanosec, 816275300);
+  EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).traffic_light_group_id, 10352);
+  EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).color, 3);
+  EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).shape, 1);
+  EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).status, 2);
+  EXPECT_FLOAT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).confidence, 1.0);
+
+  const auto operation_mode = parse<OperationModeState>(config["operation_mode"]);
+  EXPECT_EQ(operation_mode.stamp.sec, 0);
+  EXPECT_EQ(operation_mode.stamp.nanosec, 204702031);
+  EXPECT_EQ(operation_mode.mode, 1);
+  EXPECT_EQ(operation_mode.is_autoware_control_enabled, true);
+  EXPECT_EQ(operation_mode.is_in_transition, false);
+  EXPECT_EQ(operation_mode.is_stop_mode_available, true);
+  EXPECT_EQ(operation_mode.is_autonomous_mode_available, true);
+  EXPECT_EQ(operation_mode.is_local_mode_available, true);
+  EXPECT_EQ(operation_mode.is_remote_mode_available, true);
 }
 
 TEST(ParseFunction, CompleteFromFilename)