2 #include <lanelet2_core/primitives/Lanelet.h>
5 #include "gtest/gtest.h"
55 lanelet::Point3d p1{1, 0, 0},
p2{2, 2, 0},
p3{3, 0, 2},
p4{4, 2, 2},
p5{5, 0, 4},
p6{6, 2, 4},
p7{7, 0, 6},
56 p8{8, 2, 6},
p9{9, 4, 2},
p10{10, 4, 4},
p11{11, 4, 0},
p12{12, 5, 1};
57 lanelet::LineString3d ls1{1, {
p1,
p2}},
ls2{2, {
p3,
p4}},
ls3{3, {
p5,
p6}},
ls4{4, {
p7,
p8}},
ls5{5, {
p4,
p9}},
58 ls6{6, {
p6,
p10}},
ls7{7, {
p2,
p4}},
ls8{8, {
p9,
p11,
p2}},
ls9{9, {
p9,
p12,
p11}};
61 lanelet::Lanelet lanelet{1,
ls3,
ls2,
vehicleAttr},
left{2,
ls4,
ls3,
vehicleAttr},
right{3,
ls2,
ls1,
vehicleAttr},
89 using namespace std::string_literals;
91 EXPECT_TRUE(germanVehicle->canPass(
lanelet));
92 EXPECT_TRUE(germanVehicle->isOneWay(
lanelet));
97 EXPECT_FALSE(germanVehicle->canPass(
lanelet));
101 using namespace std::string_literals;
103 lanelet.setAttribute(Participants::tag(Participants::Vehicle),
true);
104 EXPECT_TRUE(germanVehicle->canPass(
lanelet));
105 EXPECT_TRUE(germanVehicle->isOneWay(
lanelet));
110 lanelet.setAttribute(Participants::tag(Participants::Vehicle),
false);
111 EXPECT_FALSE(germanVehicle->canPass(
lanelet));
116 EXPECT_FALSE(germanVehicle->isOneWay(
lanelet));
120 EXPECT_TRUE(germanVehicle->canPass(
lanelet, next));
124 EXPECT_FALSE(germanVehicle->canPass(next,
lanelet));
128 EXPECT_FALSE(germanVehicle->canPass(next.invert(),
lanelet.invert()));
133 lanelet.setAttribute(Participants::tag(Participants::VehicleCar),
true);
134 EXPECT_FALSE(germanVehicle->canPass(
lanelet));
140 lanelet.setAttribute(Participants::tag(Participants::VehicleCar),
true);
141 auto germanCar = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::VehicleCar);
142 EXPECT_TRUE(germanCar->canPass(
lanelet));
148 EXPECT_FALSE(germanVehicle->canPass(next,
lanelet));
149 EXPECT_TRUE(germanVehicle->canPass(next.invert(),
lanelet.invert()));
153 using namespace std::string_literals;
155 next.setAttribute(
AttrStr::OneWay +
":"s + Participants::Bicycle,
false);
156 EXPECT_FALSE(germanVehicle->canPass(next,
lanelet));
157 EXPECT_FALSE(germanVehicle->canPass(next.invert(),
lanelet.invert()));
161 EXPECT_FALSE(germanVehicle->canPass(left, right));
166 lanelet.setAttribute(Participants::tag(Participants::Vehicle),
true);
167 EXPECT_FALSE(germanBike->canPass(
lanelet));
168 EXPECT_TRUE(germanBike->isOneWay(
lanelet));
173 EXPECT_TRUE(germanBike->canPass(
lanelet));
174 EXPECT_TRUE(germanBike->isOneWay(
lanelet));
179 lanelet.setAttribute(Participants::tag(Participants::Vehicle),
false);
180 EXPECT_FALSE(germanBike->canPass(
lanelet));
185 EXPECT_FALSE(germanBike->isOneWay(
lanelet));
191 EXPECT_TRUE(germanBike->canPass(
lanelet, next));
197 EXPECT_FALSE(germanBike->canPass(next,
lanelet));
203 EXPECT_FALSE(germanBike->canPass(next.invert(),
lanelet.invert()));
211 EXPECT_FALSE(germanBike->canPass(next,
lanelet));
212 EXPECT_TRUE(germanBike->canPass(next.invert(),
lanelet.invert()));
216 using namespace std::string_literals;
222 next.setAttribute(
AttrStr::OneWay +
":"s + Participants::Bicycle,
false);
223 EXPECT_FALSE(germanBike->canPass(next,
lanelet));
224 EXPECT_TRUE(germanBike->canPass(next.invert(),
lanelet.invert()));
228 EXPECT_FALSE(germanBike->canPass(left, right));
233 lanelet.setAttribute(Participants::tag(Participants::Bicycle),
false);
234 EXPECT_FALSE(germanBike->canPass(
lanelet));
239 lanelet.setAttribute(Participants::tag(Participants::Bicycle),
false);
240 lanelet.setAttribute(Participants::tag(Participants::Vehicle),
true);
241 EXPECT_FALSE(germanBike->canPass(
lanelet));
245 lanelet.setAttribute(Participants::tag(Participants::Bicycle),
true);
246 next.setAttribute(Participants::tag(Participants::Bicycle),
false);
247 next.setAttribute(Participants::tag(Participants::Vehicle),
true);
248 EXPECT_FALSE(germanBike->canPass(
lanelet, next));
252 EXPECT_TRUE(germanPedestrian->canPass(
lanelet, next));
256 EXPECT_FALSE(germanPedestrian->canPass(left));
260 next.setAttribute(
AttrStr::OneWay + std::string(
":") + Participants::Pedestrian,
true);
261 EXPECT_FALSE(germanPedestrian->canPass(next.invert(),
lanelet.invert()));
267 EXPECT_TRUE(germanPedestrian->canPass(next.invert(),
lanelet.invert()));
271 area.attributes() = pedestrianAttr;
272 EXPECT_TRUE(germanPedestrian->canPass(area));
276 area.attributes() = pedestrianAttr;
277 right.attributes() = pedestrianAttr;
278 EXPECT_FALSE(germanPedestrian->canPass(area, right));
282 area.attributes() = pedestrianAttr;
283 right.attributes() = pedestrianAttr;
287 EXPECT_TRUE(germanPedestrian->canPass(area, right.invert()));
288 EXPECT_FALSE(germanPedestrian->canPass(area, right));
289 EXPECT_TRUE(germanPedestrian->canPass(right, area));
293 area.attributes() = pedestrianAttr;
294 nextArea.attributes() = pedestrianAttr;
297 EXPECT_TRUE(germanPedestrian->canPass(area, nextArea));
298 EXPECT_TRUE(germanPedestrian->canPass(nextArea, area));
302 area.attributes() = pedestrianAttr;
303 next.attributes() = pedestrianAttr;
306 EXPECT_TRUE(germanPedestrian->canPass(area, next));
307 EXPECT_TRUE(germanPedestrian->canPass(next, area));
311 area.attributes() = pedestrianAttr;
312 left.attributes() = pedestrianAttr;
315 EXPECT_FALSE(germanPedestrian->canPass(area, left));
316 EXPECT_FALSE(germanPedestrian->canPass(left, area));
324 auto limit = germanVehicle->speedLimit(
lanelet);
325 EXPECT_EQ(limit.speedLimit, 50_kmh);
326 EXPECT_TRUE(limit.isMandatory);
331 auto limit = germanVehicle->speedLimit(
lanelet);
332 EXPECT_EQ(limit.speedLimit, 100_kmh);
333 EXPECT_TRUE(limit.isMandatory);
339 auto limit = germanVehicle->speedLimit(
lanelet);
340 EXPECT_EQ(limit.speedLimit, 130_kmh);
341 EXPECT_FALSE(limit.isMandatory);
348 auto limit = germanVehicle->speedLimit(
lanelet);
349 EXPECT_EQ(limit.speedLimit, 0_kmh);
350 EXPECT_TRUE(limit.isMandatory);
358 auto limit = germanVehicle->speedLimit(
lanelet);
359 EXPECT_EQ(limit.speedLimit, 50_kmh);
360 EXPECT_TRUE(limit.isMandatory);
364 using namespace std::string_literals;
370 auto limit = germanVehicle->speedLimit(
lanelet);
371 EXPECT_EQ(limit.speedLimit, 50_kmh);
372 EXPECT_TRUE(limit.isMandatory);
376 using namespace std::string_literals;
382 auto limit = germanVehicle->speedLimit(
lanelet);
383 EXPECT_EQ(limit.speedLimit, 100_kmh);
384 EXPECT_TRUE(limit.isMandatory);
389 auto limit = germanVehicle->speedLimit(
lanelet);
390 EXPECT_EQ(limit.speedLimit, 30_kmh);
391 EXPECT_TRUE(limit.isMandatory);
396 auto limit = germanVehicle->speedLimit(
lanelet);
397 EXPECT_EQ(limit.speedLimit, 55_kmh);
398 EXPECT_TRUE(limit.isMandatory);
404 auto limit = germanVehicle->speedLimit(
lanelet);
405 EXPECT_EQ(limit.speedLimit, 50_kmh);
406 EXPECT_TRUE(limit.isMandatory);
413 auto limit = germanVehicle->speedLimit(
lanelet);
414 EXPECT_EQ(limit.speedLimit, 120_kmh);
415 EXPECT_TRUE(limit.isMandatory);
422 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
426 EXPECT_FALSE(germanVehicle->canChangeLane(right, left));
432 EXPECT_TRUE(germanVehicle->canChangeLane(
lanelet, left));
438 EXPECT_TRUE(germanVehicle->canChangeLane(left,
lanelet));
444 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left.invert()));
449 left.setAttribute(Participants::tag(Participants::Vehicle),
false);
450 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
455 EXPECT_TRUE(germanVehicle->canChangeLane(left,
lanelet));
456 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
461 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
466 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
473 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
481 EXPECT_TRUE(germanVehicle->canChangeLane(
lanelet.invert(), left.invert()));
482 EXPECT_FALSE(germanVehicle->canChangeLane(left.invert(),
lanelet.invert()));
487 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
488 EXPECT_FALSE(germanVehicle->canChangeLane(left,
lanelet));
495 EXPECT_TRUE(germanVehicle->canChangeLane(
lanelet, left));
496 EXPECT_TRUE(germanVehicle->canChangeLane(left,
lanelet));
503 EXPECT_TRUE(germanVehicle->canChangeLane(
lanelet, left));
504 EXPECT_TRUE(germanVehicle->canChangeLane(left,
lanelet));
510 EXPECT_TRUE(germanVehicle->canChangeLane(
lanelet, left));
511 EXPECT_FALSE(germanVehicle->canChangeLane(left,
lanelet));
517 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
518 EXPECT_TRUE(germanVehicle->canChangeLane(left,
lanelet));
524 EXPECT_TRUE(germanVehicle->canChangeLane(
lanelet, left));
525 EXPECT_FALSE(germanVehicle->canChangeLane(left,
lanelet));
530 EXPECT_FALSE(germanVehicle->canChangeLane(
lanelet, left));
531 EXPECT_TRUE(germanVehicle->canChangeLane(left,
lanelet));
539 EXPECT_FALSE(germanVehicle->hasDynamicRules(
lanelet));
544 lanelet.addRegulatoryElement(regelem);
545 EXPECT_TRUE(germanVehicle->hasDynamicRules(
lanelet));
550 EXPECT_EQ(germanVehicle->location(), Locations::Germany);
555 EXPECT_EQ(germanPedestrian->location(), Locations::Germany);
560 auto trafficRules = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::VehicleCar, {});
561 EXPECT_EQ(trafficRules->participant(), Participants::VehicleCar);