Skip to content

Commit f9ca79b

Browse files
f0reachARRf0reachARR
committed
update tests for multiple traffic light
1 parent 40eb8b2 commit f9ca79b

File tree

2 files changed

+91
-25
lines changed

2 files changed

+91
-25
lines changed

‎simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -127,7 +127,15 @@ TEST_F(TrafficLightsTest, startTrafficLightsUpdate)
127127
std::vector<std_msgs::msg::Header> headers;
128128
for (std::size_t i = 0; i < markers.size(); ++i) {
129129
const auto & one_marker = markers[i].markers;
130-
EXPECT_EQ(one_marker.size(), static_cast<std::size_t>(1));
130+
if (one_marker.size() == 1) {
131+
// DELETEALL marker
132+
EXPECT_EQ(one_marker.front().action, visualization_msgs::msg::Marker::DELETEALL);
133+
} else {
134+
// ADD marker
135+
EXPECT_EQ(one_marker.size(), static_cast<std::size_t>(3));
136+
EXPECT_EQ(one_marker.front().action, visualization_msgs::msg::Marker::ADD);
137+
EXPECT_EQ(one_marker.front().ns, "bulb");
138+
}
131139
if (
132140
one_marker.front().header.stamp.sec != 0 and one_marker.front().header.stamp.nanosec != 0u) {
133141
headers.push_back(one_marker.front().header);

‎simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal_common.cpp‎

Lines changed: 82 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -181,7 +181,7 @@ TYPED_TEST(TrafficLightsInternalTest, compareTrafficLightsState)
181181

182182
TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers)
183183
{
184-
const auto marker_id = this->id;
184+
const auto base_marker_id = this->id;
185185
constexpr const char * color_name = "red";
186186
this->lights->setTrafficLightsState(this->id, stateFromColor(color_name));
187187

@@ -205,25 +205,56 @@ TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers)
205205
EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETEALL) << info;
206206
};
207207

208-
const auto verify_add_marker = [&color_name, &marker_id](
209-
const visualization_msgs::msg::Marker & marker,
208+
const auto verify_add_marker = [&base_marker_id](
209+
const visualization_msgs::msg::Marker marker,
210+
const geometry_msgs::msg::Point position,
211+
const std::string color_name, const bool is_on,
210212
const auto info = "") {
211213
EXPECT_EQ(marker.ns, "bulb") << info;
212-
EXPECT_EQ(marker.id, marker_id) << info;
214+
EXPECT_EQ(marker.id >> 2, base_marker_id) << info;
213215
EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE) << info;
214216
EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD) << info;
215217

216-
EXPECT_POINT_NEAR_STREAM(
217-
marker.pose.position,
218-
geometry_msgs::build<geometry_msgs::msg::Point>().x(3770.02).y(73738.34).z(5.80),
219-
position_eps, info);
218+
EXPECT_POINT_NEAR_STREAM(marker.pose.position, position, position_eps, info);
220219

221220
EXPECT_QUATERNION_EQ_STREAM(marker.pose.orientation, geometry_msgs::msg::Quaternion(), info);
222221

223222
EXPECT_POINT_EQ_STREAM(
224223
marker.scale, geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.3).y(0.3).z(0.3), info);
225224

226-
EXPECT_COLOR_RGBA_NEAR_STREAM(marker.color, color_names::makeColorMsg(color_name), eps, info);
225+
auto expected_color = color_names::makeColorMsg(color_name);
226+
expected_color.a = is_on ? 1.0 : 0.3;
227+
EXPECT_COLOR_RGBA_NEAR_STREAM(marker.color, expected_color, eps, info);
228+
};
229+
230+
const auto verify_add_markers = [&base_marker_id, &color_name, &verify_add_marker](
231+
const std::vector<visualization_msgs::msg::Marker> & markers) {
232+
EXPECT_EQ(markers.size(), static_cast<std::size_t>(3));
233+
for (auto & marker : markers) {
234+
using Color = traffic_simulator::TrafficLight::Color;
235+
236+
const auto color_value = static_cast<Color::Value>(marker.id & 0b11);
237+
const auto color = static_cast<Color>(color_value);
238+
239+
if (color.is(Color::red)) {
240+
const auto red_position =
241+
geometry_msgs::build<geometry_msgs::msg::Point>().x(3769.15).y(73737.92).z(5.81);
242+
verify_add_marker(
243+
marker, red_position, "red", true, "red marker " + std::to_string(marker.id));
244+
} else if (color.is(Color::yellow)) {
245+
const auto yellow_position =
246+
geometry_msgs::build<geometry_msgs::msg::Point>().x(3769.60).y(73738.12).z(5.81);
247+
verify_add_marker(
248+
marker, yellow_position, "yellow", false, "yellow marker " + std::to_string(marker.id));
249+
} else if (color.is(Color::green)) {
250+
const auto green_position =
251+
geometry_msgs::build<geometry_msgs::msg::Point>().x(3770.02).y(73738.34).z(5.80);
252+
verify_add_marker(
253+
marker, green_position, "green", false, "green marker " + std::to_string(marker.id));
254+
} else {
255+
FAIL() << "Unknown color for marker id: " << marker.id;
256+
}
257+
}
227258
};
228259

229260
// verify contents of messages
@@ -237,8 +268,7 @@ TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers)
237268
}
238269
{
239270
const auto & one_marker = markers[i + 1].markers;
240-
EXPECT_EQ(one_marker.size(), static_cast<std::size_t>(1));
241-
verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1));
271+
verify_add_markers(one_marker);
242272

243273
headers.push_back(one_marker[0].header);
244274
}
@@ -254,7 +284,7 @@ TYPED_TEST(TrafficLightsInternalTest, startUpdate_publishMarkers)
254284

255285
TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers)
256286
{
257-
const auto marker_id = this->id;
287+
const auto base_marker_id = this->id;
258288
constexpr const char * color_name = "green";
259289
this->lights->setTrafficLightsState(this->id, stateFromColor(color_name));
260290

@@ -297,27 +327,57 @@ TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers)
297327
EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETEALL) << info;
298328
};
299329

300-
const auto verify_add_marker = [&color_name, &marker_id](
301-
const visualization_msgs::msg::Marker & marker,
330+
const auto verify_add_marker = [&base_marker_id](
331+
const visualization_msgs::msg::Marker marker,
332+
const geometry_msgs::msg::Point position,
333+
const std::string color_name, const bool is_on,
302334
const auto info = "") {
303335
EXPECT_EQ(marker.ns, "bulb") << info;
304-
EXPECT_EQ(marker.id, marker_id) << info;
336+
EXPECT_EQ(marker.id >> 2, base_marker_id) << info;
305337
EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE) << info;
306338
EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD) << info;
307339

308-
EXPECT_POINT_NEAR_STREAM(
309-
marker.pose.position,
310-
geometry_msgs::build<geometry_msgs::msg::Point>().x(3770.02).y(73738.34).z(5.80),
311-
position_eps, info);
340+
EXPECT_POINT_NEAR_STREAM(marker.pose.position, position, position_eps, info);
312341

313342
EXPECT_QUATERNION_EQ_STREAM(marker.pose.orientation, geometry_msgs::msg::Quaternion(), info);
314343

315344
EXPECT_POINT_EQ_STREAM(
316345
marker.scale, geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.3).y(0.3).z(0.3), info);
317346

318-
EXPECT_COLOR_RGBA_NEAR_STREAM(marker.color, color_names::makeColorMsg(color_name), eps, info);
347+
auto expected_color = color_names::makeColorMsg(color_name);
348+
expected_color.a = is_on ? 1.0 : 0.3;
349+
EXPECT_COLOR_RGBA_NEAR_STREAM(marker.color, expected_color, eps, info);
319350
};
320351

352+
const auto verify_add_markers = [&base_marker_id, &color_name, &verify_add_marker](
353+
const std::vector<visualization_msgs::msg::Marker> & markers) {
354+
EXPECT_EQ(markers.size(), static_cast<std::size_t>(3));
355+
for (auto & marker : markers) {
356+
using Color = traffic_simulator::TrafficLight::Color;
357+
358+
const auto color_value = static_cast<Color::Value>(marker.id & 0b11);
359+
const auto color = static_cast<Color>(color_value);
360+
361+
if (color.is(Color::red)) {
362+
const auto red_position =
363+
geometry_msgs::build<geometry_msgs::msg::Point>().x(3769.15).y(73737.92).z(5.81);
364+
verify_add_marker(
365+
marker, red_position, "red", false, "red marker " + std::to_string(marker.id));
366+
} else if (color.is(Color::yellow)) {
367+
const auto yellow_position =
368+
geometry_msgs::build<geometry_msgs::msg::Point>().x(3769.60).y(73738.12).z(5.81);
369+
verify_add_marker(
370+
marker, yellow_position, "yellow", false, "yellow marker " + std::to_string(marker.id));
371+
} else if (color.is(Color::green)) {
372+
const auto green_position =
373+
geometry_msgs::build<geometry_msgs::msg::Point>().x(3770.02).y(73738.34).z(5.80);
374+
verify_add_marker(
375+
marker, green_position, "green", true, "green marker " + std::to_string(marker.id));
376+
} else {
377+
FAIL() << "Unknown color for marker id: " << marker.id;
378+
}
379+
}
380+
};
321381
// verify contents of messages - before reset
322382
std::vector<std_msgs::msg::Header> headers;
323383
for (std::size_t i = 0; i < markers.size(); i += 2) {
@@ -329,8 +389,7 @@ TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers)
329389
}
330390
{
331391
const auto & one_marker = markers[i + 1].markers;
332-
EXPECT_EQ(one_marker.size(), static_cast<std::size_t>(1));
333-
verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1));
392+
verify_add_markers(one_marker);
334393

335394
headers.push_back(one_marker[0].header);
336395
}
@@ -355,8 +414,7 @@ TYPED_TEST(TrafficLightsInternalTest, resetUpdate_publishMarkers)
355414
}
356415
{
357416
const auto & one_marker = markers_reset[i + 1].markers;
358-
EXPECT_EQ(one_marker.size(), static_cast<std::size_t>(1));
359-
verify_add_marker(one_marker[0], "marker " + std::to_string(i + 1));
417+
verify_add_markers(one_marker);
360418

361419
headers_reset.push_back(one_marker[0].header);
362420
}

0 commit comments

Comments
 (0)