Skip to content

Commit 458f3d9

Browse files
soblinsoblin
authored
feat(autoware_test_utils): add traffic light msgs parser (#9177)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 6a06241 commit 458f3d9

File tree

3 files changed

+111
-0
lines changed

3 files changed

+111
-0
lines changed

‎common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp

+23
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,11 @@
1616
#define AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_
1717

1818
#include <builtin_interfaces/msg/duration.hpp>
19+
#include <builtin_interfaces/msg/time.hpp>
1920

2021
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
2122
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
23+
#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
2224
#include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
2325
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
2426
#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
@@ -36,16 +38,22 @@
3638

3739
namespace autoware::test_utils
3840
{
41+
using autoware_adapi_v1_msgs::msg::OperationModeState;
3942
using autoware_perception_msgs::msg::ObjectClassification;
4043
using autoware_perception_msgs::msg::PredictedObject;
4144
using autoware_perception_msgs::msg::PredictedObjectKinematics;
4245
using autoware_perception_msgs::msg::PredictedObjects;
4346
using autoware_perception_msgs::msg::PredictedPath;
4447
using autoware_perception_msgs::msg::Shape;
48+
using autoware_perception_msgs::msg::TrafficLightElement;
49+
using autoware_perception_msgs::msg::TrafficLightGroup;
50+
using autoware_perception_msgs::msg::TrafficLightGroupArray;
51+
4552
using autoware_planning_msgs::msg::LaneletPrimitive;
4653
using autoware_planning_msgs::msg::LaneletRoute;
4754
using autoware_planning_msgs::msg::LaneletSegment;
4855
using builtin_interfaces::msg::Duration;
56+
using builtin_interfaces::msg::Time;
4957
using geometry_msgs::msg::Accel;
5058
using geometry_msgs::msg::AccelWithCovariance;
5159
using geometry_msgs::msg::AccelWithCovarianceStamped;
@@ -79,6 +87,9 @@ Header parse(const YAML::Node & node);
7987
template <>
8088
Duration parse(const YAML::Node & node);
8189

90+
template <>
91+
Time parse(const YAML::Node & node);
92+
8293
template <>
8394
std::vector<Point> parse(const YAML::Node & node);
8495

@@ -142,6 +153,18 @@ PredictedObject parse(const YAML::Node & node);
142153
template <>
143154
PredictedObjects parse(const YAML::Node & node);
144155

156+
template <>
157+
TrafficLightGroupArray parse(const YAML::Node & node);
158+
159+
template <>
160+
TrafficLightGroup parse(const YAML::Node & node);
161+
162+
template <>
163+
TrafficLightElement parse(const YAML::Node & node);
164+
165+
template <>
166+
OperationModeState parse(const YAML::Node & node);
167+
145168
/**
146169
* @brief Parses a YAML file and converts it into an object of type T.
147170
*

‎common/autoware_test_utils/src/mock_data_parser.cpp

+57
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,15 @@ Duration parse(const YAML::Node & node)
4444
return msg;
4545
}
4646

47+
template <>
48+
Time parse(const YAML::Node & node)
49+
{
50+
Time msg;
51+
msg.sec = node["sec"].as<int>();
52+
msg.nanosec = node["nanosec"].as<int>();
53+
return msg;
54+
}
55+
4756
template <>
4857
std::array<double, 36> parse(const YAML::Node & node)
4958
{
@@ -321,6 +330,54 @@ PredictedObjects parse(const YAML::Node & node)
321330
return msg;
322331
}
323332

333+
template <>
334+
TrafficLightElement parse(const YAML::Node & node)
335+
{
336+
TrafficLightElement msg;
337+
msg.color = node["color"].as<uint8_t>();
338+
msg.shape = node["shape"].as<uint8_t>();
339+
msg.status = node["status"].as<uint8_t>();
340+
msg.confidence = node["confidence"].as<float>();
341+
return msg;
342+
}
343+
344+
template <>
345+
TrafficLightGroup parse(const YAML::Node & node)
346+
{
347+
TrafficLightGroup msg;
348+
msg.traffic_light_group_id = node["traffic_light_group_id"].as<int>();
349+
for (const auto & element_node : node["elements"]) {
350+
msg.elements.push_back(parse<TrafficLightElement>(element_node));
351+
}
352+
return msg;
353+
}
354+
355+
template <>
356+
TrafficLightGroupArray parse(const YAML::Node & node)
357+
{
358+
TrafficLightGroupArray msg;
359+
msg.stamp = parse<Time>(node["stamp"]);
360+
for (const auto & traffic_light_group_node : node["traffic_light_groups"]) {
361+
msg.traffic_light_groups.push_back(parse<TrafficLightGroup>(traffic_light_group_node));
362+
}
363+
return msg;
364+
}
365+
366+
template <>
367+
OperationModeState parse(const YAML::Node & node)
368+
{
369+
OperationModeState msg;
370+
msg.stamp = parse<Time>(node["stamp"]);
371+
msg.mode = node["mode"].as<uint8_t>();
372+
msg.is_autoware_control_enabled = node["is_autoware_control_enabled"].as<bool>();
373+
msg.is_in_transition = node["is_in_transition"].as<bool>();
374+
msg.is_stop_mode_available = node["is_stop_mode_available"].as<bool>();
375+
msg.is_autonomous_mode_available = node["is_autonomous_mode_available"].as<bool>();
376+
msg.is_local_mode_available = node["is_local_mode_available"].as<bool>();
377+
msg.is_remote_mode_available = node["is_remote_mode_available"].as<bool>();
378+
return msg;
379+
}
380+
324381
template <>
325382
LaneletRoute parse(const std::string & filename)
326383
{

‎common/autoware_test_utils/test/test_mock_data_parser.cpp

+31
Original file line numberDiff line numberDiff line change
@@ -448,6 +448,17 @@ const char g_complete_yaml[] = R"(
448448
shape: 1
449449
status: 2
450450
confidence: 1.00000
451+
operation_mode:
452+
stamp:
453+
sec: 0
454+
nanosec: 204702031
455+
mode: 1
456+
is_autoware_control_enabled: true
457+
is_in_transition: false
458+
is_stop_mode_available: true
459+
is_autonomous_mode_available: true
460+
is_local_mode_available: true
461+
is_remote_mode_available: true
451462
)";
452463

453464
TEST(ParseFunctions, CompleteYAMLTest)
@@ -541,6 +552,26 @@ TEST(ParseFunctions, CompleteYAMLTest)
541552
EXPECT_EQ(dynamic_object.objects.at(0).shape.type, 0);
542553
EXPECT_DOUBLE_EQ(dynamic_object.objects.at(0).shape.dimensions.x, 4.40000);
543554
EXPECT_DOUBLE_EQ(dynamic_object.objects.at(0).shape.dimensions.y, 1.85564);
555+
556+
const auto traffic_signal = parse<TrafficLightGroupArray>(config["traffic_signal"]);
557+
EXPECT_EQ(traffic_signal.stamp.sec, 1730184609);
558+
EXPECT_EQ(traffic_signal.stamp.nanosec, 816275300);
559+
EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).traffic_light_group_id, 10352);
560+
EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).color, 3);
561+
EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).shape, 1);
562+
EXPECT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).status, 2);
563+
EXPECT_FLOAT_EQ(traffic_signal.traffic_light_groups.at(0).elements.at(0).confidence, 1.0);
564+
565+
const auto operation_mode = parse<OperationModeState>(config["operation_mode"]);
566+
EXPECT_EQ(operation_mode.stamp.sec, 0);
567+
EXPECT_EQ(operation_mode.stamp.nanosec, 204702031);
568+
EXPECT_EQ(operation_mode.mode, 1);
569+
EXPECT_EQ(operation_mode.is_autoware_control_enabled, true);
570+
EXPECT_EQ(operation_mode.is_in_transition, false);
571+
EXPECT_EQ(operation_mode.is_stop_mode_available, true);
572+
EXPECT_EQ(operation_mode.is_autonomous_mode_available, true);
573+
EXPECT_EQ(operation_mode.is_local_mode_available, true);
574+
EXPECT_EQ(operation_mode.is_remote_mode_available, true);
544575
}
545576

546577
TEST(ParseFunction, CompleteFromFilename)

0 commit comments

Comments
 (0)