test: enhance OpenDriveMap tests with routing checks and improve readability

Author: pageldev

tests/test.cpp

@@ -1,18 +1,14 @@
 #include "OpenDriveMap.h"
 #include <catch2/catch_test_macros.hpp>
+#include <catch2/matchers/catch_matchers_vector.hpp>
 #include <iostream>
 
-TEST_CASE("Basic OpenDriveMap check", "[xodr]")
+TEST_CASE("basic map check", "[xodr]")
 {
     odr::OpenDriveMap odr_map("test.xodr");
     REQUIRE(odr_map.xml_parse_result);
     REQUIRE(!odr_map.get_roads().empty());
 
-    // test routing
-    auto graph = odr_map.get_routing_graph();
-    auto path = graph.shortest_path(odr::LaneKey("43", 0.0, 1), odr::LaneKey("41", 0.0, 1));
-    REQUIRE(path.size() == 15);
-
     for (const odr::Road& road : odr_map.get_roads())
     {
         INFO("road: " << road.id << ", length: " << road.length);
@@ -30,4 +26,38 @@ TEST_CASE("Basic OpenDriveMap check", "[xodr]")
             }
         }
     }
+}
+
+TEST_CASE("routing check", "[routing]")
+{
+    odr::OpenDriveMap odr_map("test.xodr");
+
+    const odr::RoutingGraph graph = odr_map.get_routing_graph();
+
+    std::vector<odr::LaneKey> path, expected_path;
+    const odr::LaneKey        posA("43", 0.0, 1);
+    const odr::LaneKey        posB("41", 0.0, 1);
+    const odr::LaneKey        posC("13", 0.0, -1);
+    const odr::LaneKey        posNaN("no-exists", 0.0, 1);
+
+    path = graph.shortest_path(posA, posB);
+    REQUIRE(path.size() == 15);
+    REQUIRE(path.front() == posA);
+    REQUIRE(path.back() == posB);
+
+    path = graph.shortest_path(posA, posA);
+    REQUIRE(path.size() == 1);
+    REQUIRE(path.front() == posA);
+
+    path = graph.shortest_path(posC, posB);
+    expected_path = {posC,
+                     odr::LaneKey("405", 0.0, -1),
+                     odr::LaneKey("405", 14.0521019230889283591068306122906506061553955078125, -1),
+                     odr::LaneKey("16", 0.0, -1),
+                     odr::LaneKey("17", 0.0, -1),
+                     odr::LaneKey("18", 0.0, -1),
+                     odr::LaneKey("747", 0.0, -1),
+                     odr::LaneKey("747", 1.76237248299736393164494074881076812744140625, -1),
+                     posB};
+    REQUIRE_THAT(path, Catch::Matchers::Equals(expected_path));
 }