diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7ca54ff..c3167ad 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -39,6 +39,7 @@ file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/data/DefaultObjContent.obj DESTINATION ${T file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/data/HighlightedSegments.obj DESTINATION ${TEST_PATH}) file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/data/NoBranchPointsObjContent.obj DESTINATION ${TEST_PATH}) file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/data/StackedBranchPointsObjContent.obj DESTINATION ${TEST_PATH}) +file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resources/odr/MultipleStraightSegments.xodr DESTINATION ${TEST_PATH}) ament_add_gtest(derive_lane_s_routes_test derive_lane_s_routes_test.cc) ament_add_gtest(distance_router_test distance_router_test.cc) @@ -51,6 +52,7 @@ ament_add_gtest(intersection_book_loader_test intersection_book_loader_test.cc) ament_add_gmock(object_query_test object_query_test.cc) ament_add_gtest(phase_ring_book_loader_test phase_ring_book_loader_test.cc) ament_add_gtest(road_rulebook_loader_test road_rulebook_loader_test.cc) +ament_add_gtest(route_end_to_end_connectivity_test route_end_to_end_connectivity_test.cc WORKING_DIRECTORY ${TEST_PATH}) ament_add_gtest(segment_analysis_test segment_analysis_test.cc) ament_add_gtest(traffic_light_book_loader_test traffic_light_book_loader_test.cc) ament_add_gtest(waypoints_test waypoints_test.cc) @@ -66,6 +68,7 @@ add_dependencies_to_test(intersection_book_loader_test) add_dependencies_to_test(object_query_test) add_dependencies_to_test(phase_ring_book_loader_test) add_dependencies_to_test(road_rulebook_loader_test) +add_dependencies_to_test(route_end_to_end_connectivity_test) add_dependencies_to_test(segment_analysis_test) add_dependencies_to_test(traffic_light_book_loader_test) add_dependencies_to_test(waypoints_test) diff --git a/test/distance_router_test.cc b/test/distance_router_test.cc index 2d3d5e8..2e7d270 100644 --- a/test/distance_router_test.cc +++ b/test/distance_router_test.cc @@ -63,23 +63,33 @@ static constexpr char kMalidriveResourcesPath[] = DEF_MALIDRIVE_RESOURCES; void CheckRoutingPhase(const routing::Phase& phase, int index, double tolerance, const std::vector& start_pos, const std::vector& end_pos, const std::vector& lane_s_ranges) { - ASSERT_EQ(index, phase.index()); - ASSERT_EQ(tolerance, phase.lane_s_range_tolerance()); - ASSERT_EQ(start_pos.size(), phase.start_positions().size()); + ASSERT_EQ(index, phase.index()) << "phase " << index << " with indices are not equal."; + ASSERT_EQ(tolerance, phase.lane_s_range_tolerance()) << "phase " << index << " with tolerances are not equal."; + ASSERT_EQ(start_pos.size(), phase.start_positions().size()) + << "phase " << index << " with start position sizes are not equal."; for (size_t i = 0; i < start_pos.size(); ++i) { - ASSERT_EQ(start_pos[i].lane, phase.start_positions()[i].lane); - ASSERT_TRUE(AssertCompare(IsLanePositionClose(start_pos[i].pos, phase.start_positions()[i].pos, tolerance))); + ASSERT_EQ(start_pos[i].lane, phase.start_positions()[i].lane) + << "phase " << index << " with index " << i << " with mismatched RoadPositions."; + ASSERT_TRUE(AssertCompare(IsLanePositionClose(start_pos[i].pos, phase.start_positions()[i].pos, tolerance))) + << "phase " << index << " with index " << i << " with mismatched RoadPositions."; } - ASSERT_EQ(end_pos.size(), phase.end_positions().size()); + ASSERT_EQ(end_pos.size(), phase.end_positions().size()) + << "phase " << index << " with end position sizes are not equal."; for (size_t i = 0; i < start_pos.size(); ++i) { - ASSERT_EQ(end_pos[i].lane, phase.end_positions()[i].lane); - ASSERT_TRUE(AssertCompare(IsLanePositionClose(end_pos[i].pos, phase.end_positions()[i].pos, tolerance))); + ASSERT_EQ(end_pos[i].lane, phase.end_positions()[i].lane) + << "phase " << index << " with index " << i << " with mismatched RoadPositions."; + ASSERT_TRUE(AssertCompare(IsLanePositionClose(end_pos[i].pos, phase.end_positions()[i].pos, tolerance))) + << "phase " << index << " with index " << i << " with mismatched RoadPositions."; } - ASSERT_EQ(lane_s_ranges.size(), phase.lane_s_ranges().size()); + ASSERT_EQ(lane_s_ranges.size(), phase.lane_s_ranges().size()) + << "phase " << index << " with LaneSRanges sizes are not equal."; for (size_t i = 0; i < lane_s_ranges.size(); ++i) { - ASSERT_EQ(lane_s_ranges[i].lane_id().string(), phase.lane_s_ranges()[i].lane_id().string()); - ASSERT_NEAR(lane_s_ranges[i].s_range().s0(), phase.lane_s_ranges()[i].s_range().s0(), tolerance); - ASSERT_NEAR(lane_s_ranges[i].s_range().s1(), phase.lane_s_ranges()[i].s_range().s1(), tolerance); + ASSERT_EQ(lane_s_ranges[i].lane_id().string(), phase.lane_s_ranges()[i].lane_id().string()) + << "phase " << index << " with index " << i << " with mismatched LaneIds."; + ASSERT_NEAR(lane_s_ranges[i].s_range().s0(), phase.lane_s_ranges()[i].s_range().s0(), tolerance) + << "phase " << index << " with index " << i << " with mismatched SRange::s0."; + ASSERT_NEAR(lane_s_ranges[i].s_range().s1(), phase.lane_s_ranges()[i].s_range().s1(), tolerance) + << "phase " << index << " with index " << i << " with mismatched SRange::s1."; } } @@ -99,9 +109,12 @@ class TShapeRoadRoutingTest : public ::testing::Test { }; const std::string kTShapeRoadFilePath{std::string(kMalidriveResourcesPath) + std::string("/resources/odr/TShapeRoad.xodr")}; - - std::unique_ptr road_network_{}; - std::unique_ptr dut_{}; + const api::LaneId kLaneId_0_0_1{"0_0_1"}; + const api::LaneId kLaneId_0_0_m1{"0_0_-1"}; + const api::LaneId kLaneId_1_0_1{"1_0_1"}; + const api::LaneId kLaneId_1_0_m1{"1_0_-1"}; + const api::LaneId kLaneId_4_0_1{"4_0_1"}; + const api::LaneId kLaneId_5_0_m1{"5_0_-1"}; void SetUp() override { std::map road_network_configuration; @@ -115,23 +128,39 @@ class TShapeRoadRoutingTest : public ::testing::Test { road_network_ = malidrive::loader::Load(road_network_configuration); dut_ = std::make_unique(*road_network_, kLinearTolerance); + + lane_0_0_1_ = road_network_->road_geometry()->ById().GetLane(kLaneId_0_0_1); + lane_0_0_m1_ = road_network_->road_geometry()->ById().GetLane(kLaneId_0_0_m1); + lane_1_0_1_ = road_network_->road_geometry()->ById().GetLane(kLaneId_1_0_1); + lane_1_0_m1_ = road_network_->road_geometry()->ById().GetLane(kLaneId_1_0_m1); + lane_4_0_1_ = road_network_->road_geometry()->ById().GetLane(kLaneId_4_0_1); + lane_5_0_m1_ = road_network_->road_geometry()->ById().GetLane(kLaneId_5_0_m1); + + ASSERT_NE(lane_0_0_1_, nullptr); + ASSERT_NE(lane_0_0_m1_, nullptr); + ASSERT_NE(lane_1_0_1_, nullptr); + ASSERT_NE(lane_1_0_m1_, nullptr); + ASSERT_NE(lane_4_0_1_, nullptr); + ASSERT_NE(lane_5_0_m1_, nullptr); } + + std::unique_ptr road_network_{}; + std::unique_ptr dut_{}; + const api::Lane* lane_0_0_1_{}; + const api::Lane* lane_0_0_m1_{}; + const api::Lane* lane_1_0_1_{}; + const api::Lane* lane_1_0_m1_{}; + const api::Lane* lane_4_0_1_{}; + const api::Lane* lane_5_0_m1_{}; }; // Defines the test cases for the TShapeRoad where the start and end positions are on the very same Lane. class RoutingInTheSameLaneTest : public TShapeRoadRoutingTest { public: - const api::LaneId kStartLaneId{"0_0_1"}; - const api::LaneId kEndLaneId{"0_0_1"}; - const api::Lane* start_lane_; - const api::Lane* end_lane_; - void SetUp() override { TShapeRoadRoutingTest::SetUp(); - start_lane_ = road_network_->road_geometry()->ById().GetLane(kStartLaneId); - end_lane_ = road_network_->road_geometry()->ById().GetLane(kStartLaneId); - start_ = api::RoadPosition(start_lane_, api::LanePosition(1., 0., 0.)); - end_ = api::RoadPosition(end_lane_, api::LanePosition(10., 0., 0.)); + start_ = api::RoadPosition(lane_0_0_1_, api::LanePosition(1., 0., 0.)); + end_ = api::RoadPosition(lane_0_0_1_, api::LanePosition(10., 0., 0.)); } api::RoadPosition start_; @@ -140,7 +169,8 @@ class RoutingInTheSameLaneTest : public TShapeRoadRoutingTest { // No constraints are provided and the only possible route is returned. TEST_F(RoutingInTheSameLaneTest, WithDefaultConstraintsReturnsTheLane) { - const std::vector kPhaseLaneSRanges{api::LaneSRange(kStartLaneId, api::SRange(1., 10.))}; + const std::vector kPhaseLaneSRanges{api::LaneSRange(kLaneId_0_0_m1, api::SRange(1., 10.)), + api::LaneSRange(kLaneId_0_0_1, api::SRange(1., 10.))}; const std::vector routes = dut_->ComputeRoutes(start_, end_, kDefaultRoutingConstraints); @@ -170,68 +200,96 @@ TEST_F(RoutingInTheSameLaneTest, WithConstrainedRouteCostReturnsEmpty) { // three roads aligned in a straight line. class DriveBackwardStraightOverMultipleLanesTest : public TShapeRoadRoutingTest { public: - const api::LaneId kStartLaneId{"1_0_1"}; - const api::LaneId kIntermediateLaneId{"4_0_1"}; - const api::LaneId kEndLaneId{"0_0_1"}; - const std::vector kPhase0LaneSRanges{api::LaneSRange{kStartLaneId, api::SRange{1., 0.}}}; - const std::vector kPhase1LaneSRanges{api::LaneSRange{kIntermediateLaneId, api::SRange{8., 0.}}}; - const std::vector kPhase2LaneSRanges{api::LaneSRange{kEndLaneId, api::SRange{46., 10.}}}; - const api::Lane* start_lane_; - const api::Lane* intermediate_lane_; - const api::Lane* end_lane_; + const std::vector kRoute0Phase0LaneSRanges{api::LaneSRange{kLaneId_1_0_m1, api::SRange{1., 0.}}, + api::LaneSRange{kLaneId_1_0_1, api::SRange{1., 0.}}}; + const std::vector kRoute0Phase1LaneSRanges{api::LaneSRange{kLaneId_4_0_1, api::SRange{8., 0.}}}; + const std::vector kRoute0Phase2LaneSRanges{api::LaneSRange{kLaneId_0_0_m1, api::SRange{46., 10.}}, + api::LaneSRange{kLaneId_0_0_1, api::SRange{46., 10.}}}; + const std::vector kRoute1Phase0LaneSRanges{api::LaneSRange{kLaneId_1_0_m1, api::SRange{1., 0.}}, + api::LaneSRange{kLaneId_1_0_1, api::SRange{1., 0.}}}; + const std::vector kRoute1Phase1LaneSRanges{api::LaneSRange{kLaneId_5_0_m1, api::SRange{8., 0.}}}; + const std::vector kRoute1Phase2LaneSRanges{api::LaneSRange{kLaneId_0_0_m1, api::SRange{46., 10.}}, + api::LaneSRange{kLaneId_0_0_1, api::SRange{46., 10.}}}; void SetUp() override { TShapeRoadRoutingTest::SetUp(); - start_lane_ = road_network_->road_geometry()->ById().GetLane(kStartLaneId); - intermediate_lane_ = road_network_->road_geometry()->ById().GetLane(kIntermediateLaneId); - end_lane_ = road_network_->road_geometry()->ById().GetLane(kEndLaneId); - start_phase_0_ = api::RoadPosition(start_lane_, api::LanePosition(1., 0., 0.)); - end_phase_0_ = api::RoadPosition(start_lane_, api::LanePosition(0., 0., 0.)); - start_phase_1_ = api::RoadPosition(intermediate_lane_, api::LanePosition(8., 0., 0.)); - end_phase_1_ = api::RoadPosition(intermediate_lane_, api::LanePosition(0., 0., 0.)); - start_phase_2_ = api::RoadPosition(end_lane_, api::LanePosition(46., 0., 0.)); - end_phase_2_ = api::RoadPosition(end_lane_, api::LanePosition(10., 0., 0.)); + start_ = api::RoadPosition(lane_1_0_1_, api::LanePosition(1., 0., 0.)); + end_ = api::RoadPosition(lane_0_0_1_, api::LanePosition(10., 0., 0.)); + // Route 0 + route_0_phase_0_start_ = start_; + route_0_phase_0_end_ = api::RoadPosition(lane_1_0_1_, api::LanePosition(0., 0., 0.)); + route_0_phase_1_start_ = api::RoadPosition(lane_4_0_1_, api::LanePosition(8., 0., 0.)); + route_0_phase_1_end_ = api::RoadPosition(lane_4_0_1_, api::LanePosition(0., 0., 0.)); + route_0_phase_2_start_ = api::RoadPosition(lane_0_0_1_, api::LanePosition(46., 0., 0.)); + route_0_phase_2_end_ = end_; + // Route 1 + route_1_phase_0_start_ = start_; + route_1_phase_0_end_ = api::RoadPosition(lane_1_0_m1_, api::LanePosition(0., 0., 0.)); + route_1_phase_1_start_ = api::RoadPosition(lane_5_0_m1_, api::LanePosition(8., 0., 0.)); + route_1_phase_1_end_ = api::RoadPosition(lane_5_0_m1_, api::LanePosition(0., 0., 0.)); + route_1_phase_2_start_ = api::RoadPosition(lane_0_0_m1_, api::LanePosition(46., 0., 0.)); + route_1_phase_2_end_ = end_; } - api::RoadPosition start_phase_0_; - api::RoadPosition end_phase_0_; - api::RoadPosition start_phase_1_; - api::RoadPosition end_phase_1_; - api::RoadPosition start_phase_2_; - api::RoadPosition end_phase_2_; + api::RoadPosition start_; + api::RoadPosition end_; + api::RoadPosition route_0_phase_0_start_; + api::RoadPosition route_0_phase_0_end_; + api::RoadPosition route_0_phase_1_start_; + api::RoadPosition route_0_phase_1_end_; + api::RoadPosition route_0_phase_2_start_; + api::RoadPosition route_0_phase_2_end_; + api::RoadPosition route_1_phase_0_start_; + api::RoadPosition route_1_phase_0_end_; + api::RoadPosition route_1_phase_1_start_; + api::RoadPosition route_1_phase_1_end_; + api::RoadPosition route_1_phase_2_start_; + api::RoadPosition route_1_phase_2_end_; }; // No constraints are provided and the only possible route is returned. TEST_F(DriveBackwardStraightOverMultipleLanesTest, WithDefaultConstraintsReturnsRouteWithThreePhases) { - const std::vector routes = - dut_->ComputeRoutes(start_phase_0_, end_phase_2_, kDefaultRoutingConstraints); + const std::vector routes = dut_->ComputeRoutes(start_, end_, kDefaultRoutingConstraints); - ASSERT_EQ(1u, routes.size()); - const routing::Route& route = routes[0]; - ASSERT_EQ(3, route.size()); - const routing::Phase& phase_0 = route.Get(0); - const routing::Phase& phase_1 = route.Get(1); - const routing::Phase& phase_2 = route.Get(2); - CheckRoutingPhase(phase_0, 0, kLinearTolerance, std::vector{start_phase_0_}, - std::vector{end_phase_0_}, kPhase0LaneSRanges); - CheckRoutingPhase(phase_1, 1, kLinearTolerance, std::vector{start_phase_1_}, - std::vector{end_phase_1_}, kPhase1LaneSRanges); - CheckRoutingPhase(phase_2, 2, kLinearTolerance, std::vector{start_phase_2_}, - std::vector{end_phase_2_}, kPhase2LaneSRanges); + ASSERT_EQ(2u, routes.size()); + { + const routing::Route& route = routes[0]; + ASSERT_EQ(3, route.size()); + const routing::Phase& phase_0 = route.Get(0); + const routing::Phase& phase_1 = route.Get(1); + const routing::Phase& phase_2 = route.Get(2); + CheckRoutingPhase(phase_0, 0, kLinearTolerance, std::vector{route_0_phase_0_start_}, + std::vector{route_0_phase_0_end_}, kRoute0Phase0LaneSRanges); + CheckRoutingPhase(phase_1, 1, kLinearTolerance, std::vector{route_0_phase_1_start_}, + std::vector{route_0_phase_1_end_}, kRoute0Phase1LaneSRanges); + CheckRoutingPhase(phase_2, 2, kLinearTolerance, std::vector{route_0_phase_2_start_}, + std::vector{route_0_phase_2_end_}, kRoute0Phase2LaneSRanges); + } + { + const routing::Route& route = routes[1]; + ASSERT_EQ(3, route.size()); + const routing::Phase& phase_0 = route.Get(0); + const routing::Phase& phase_1 = route.Get(1); + const routing::Phase& phase_2 = route.Get(2); + CheckRoutingPhase(phase_0, 0, kLinearTolerance, std::vector{route_1_phase_0_start_}, + std::vector{route_1_phase_0_end_}, kRoute1Phase0LaneSRanges); + CheckRoutingPhase(phase_1, 1, kLinearTolerance, std::vector{route_1_phase_1_start_}, + std::vector{route_1_phase_1_end_}, kRoute1Phase1LaneSRanges); + CheckRoutingPhase(phase_2, 2, kLinearTolerance, std::vector{route_1_phase_2_start_}, + std::vector{route_1_phase_2_end_}, kRoute1Phase2LaneSRanges); + } } // The maximum cost of the phase is smaller than the solution's phase cost, so no routes can be found. TEST_F(DriveBackwardStraightOverMultipleLanesTest, WithConstrainedPhaseCostReturnsEmpty) { - const std::vector routes = - dut_->ComputeRoutes(start_phase_0_, end_phase_2_, kSmallPhaseCostConstraint); + const std::vector routes = dut_->ComputeRoutes(start_, end_, kSmallPhaseCostConstraint); ASSERT_TRUE(routes.empty()); } // The maximum cost of the route is smaller than the solution's phase cost, so no routes can be found. TEST_F(DriveBackwardStraightOverMultipleLanesTest, WithConstrainedRouteCostReturnsEmpty) { - const std::vector routes = - dut_->ComputeRoutes(start_phase_0_, end_phase_2_, kSmallRouteCostConstraint); + const std::vector routes = dut_->ComputeRoutes(start_, end_, kSmallRouteCostConstraint); ASSERT_TRUE(routes.empty()); } @@ -240,68 +298,96 @@ TEST_F(DriveBackwardStraightOverMultipleLanesTest, WithConstrainedRouteCostRetur // three roads aligned in a straight line. class DriveForwardStraightOverMultipleLanesTest : public TShapeRoadRoutingTest { public: - const api::LaneId kStartLaneId{"0_0_-1"}; - const api::LaneId kIntermediateLaneId{"5_0_-1"}; - const api::LaneId kEndLaneId{"1_0_-1"}; - const std::vector kPhase0LaneSRanges{api::LaneSRange(kStartLaneId, api::SRange(1., 46.))}; - const std::vector kPhase1LaneSRanges{api::LaneSRange(kIntermediateLaneId, api::SRange(0., 8.))}; - const std::vector kPhase2LaneSRanges{api::LaneSRange(kEndLaneId, api::SRange(0., 10.))}; - const api::Lane* start_lane_; - const api::Lane* intermediate_lane_; - const api::Lane* end_lane_; + const std::vector kRoute0Phase0LaneSRanges{api::LaneSRange{kLaneId_0_0_m1, api::SRange{1., 46.}}, + api::LaneSRange{kLaneId_0_0_1, api::SRange{1., 46.}}}; + const std::vector kRoute0Phase1LaneSRanges{api::LaneSRange{kLaneId_4_0_1, api::SRange{0., 8.}}}; + const std::vector kRoute0Phase2LaneSRanges{api::LaneSRange{kLaneId_1_0_m1, api::SRange{0., 10.}}, + api::LaneSRange{kLaneId_1_0_1, api::SRange{0., 10.}}}; + const std::vector kRoute1Phase0LaneSRanges{api::LaneSRange{kLaneId_0_0_m1, api::SRange{1., 46.}}, + api::LaneSRange{kLaneId_0_0_1, api::SRange{1., 46.}}}; + const std::vector kRoute1Phase1LaneSRanges{api::LaneSRange{kLaneId_5_0_m1, api::SRange{0., 8.}}}; + const std::vector kRoute1Phase2LaneSRanges{api::LaneSRange{kLaneId_1_0_m1, api::SRange{0., 10.}}, + api::LaneSRange{kLaneId_1_0_1, api::SRange{0., 10.}}}; void SetUp() override { TShapeRoadRoutingTest::SetUp(); - start_lane_ = road_network_->road_geometry()->ById().GetLane(kStartLaneId); - intermediate_lane_ = road_network_->road_geometry()->ById().GetLane(kIntermediateLaneId); - end_lane_ = road_network_->road_geometry()->ById().GetLane(kEndLaneId); - start_phase_0_ = api::RoadPosition(start_lane_, api::LanePosition(1., 0., 0.)); - end_phase_0_ = api::RoadPosition(start_lane_, api::LanePosition(46., 0., 0.)); - start_phase_1_ = api::RoadPosition(intermediate_lane_, api::LanePosition(0., 0., 0.)); - end_phase_1_ = api::RoadPosition(intermediate_lane_, api::LanePosition(8., 0., 0.)); - start_phase_2_ = api::RoadPosition(end_lane_, api::LanePosition(0., 0., 0.)); - end_phase_2_ = api::RoadPosition(end_lane_, api::LanePosition(10., 0., 0.)); + start_ = api::RoadPosition(lane_0_0_m1_, api::LanePosition(1., 0., 0.)); + end_ = api::RoadPosition(lane_1_0_m1_, api::LanePosition(10., 0., 0.)); + // Route 0 + route_0_phase_0_start_ = start_; + route_0_phase_0_end_ = api::RoadPosition(lane_0_0_1_, api::LanePosition(46., 0., 0.)); + route_0_phase_1_start_ = api::RoadPosition(lane_4_0_1_, api::LanePosition(0., 0., 0.)); + route_0_phase_1_end_ = api::RoadPosition(lane_4_0_1_, api::LanePosition(8., 0., 0.)); + route_0_phase_2_start_ = api::RoadPosition(lane_1_0_1_, api::LanePosition(0., 0., 0.)); + route_0_phase_2_end_ = end_; + // Route 1 + route_1_phase_0_start_ = start_; + route_1_phase_0_end_ = api::RoadPosition(lane_0_0_m1_, api::LanePosition(46., 0., 0.)); + route_1_phase_1_start_ = api::RoadPosition(lane_5_0_m1_, api::LanePosition(0., 0., 0.)); + route_1_phase_1_end_ = api::RoadPosition(lane_5_0_m1_, api::LanePosition(8., 0., 0.)); + route_1_phase_2_start_ = api::RoadPosition(lane_1_0_m1_, api::LanePosition(0., 0., 0.)); + route_1_phase_2_end_ = end_; } - api::RoadPosition start_phase_0_; - api::RoadPosition end_phase_0_; - api::RoadPosition start_phase_1_; - api::RoadPosition end_phase_1_; - api::RoadPosition start_phase_2_; - api::RoadPosition end_phase_2_; + api::RoadPosition start_; + api::RoadPosition end_; + api::RoadPosition route_0_phase_0_start_; + api::RoadPosition route_0_phase_0_end_; + api::RoadPosition route_0_phase_1_start_; + api::RoadPosition route_0_phase_1_end_; + api::RoadPosition route_0_phase_2_start_; + api::RoadPosition route_0_phase_2_end_; + api::RoadPosition route_1_phase_0_start_; + api::RoadPosition route_1_phase_0_end_; + api::RoadPosition route_1_phase_1_start_; + api::RoadPosition route_1_phase_1_end_; + api::RoadPosition route_1_phase_2_start_; + api::RoadPosition route_1_phase_2_end_; }; // No constraints are provided and the only possible route is returned. TEST_F(DriveForwardStraightOverMultipleLanesTest, WithDefaultConstraintsReturnsRouteWithThreePhases) { - const std::vector routes = - dut_->ComputeRoutes(start_phase_0_, end_phase_2_, kDefaultRoutingConstraints); + const std::vector routes = dut_->ComputeRoutes(start_, end_, kDefaultRoutingConstraints); - ASSERT_EQ(1u, routes.size()); - const routing::Route& route = routes[0]; - ASSERT_EQ(3, route.size()); - const routing::Phase& phase_0 = route.Get(0); - const routing::Phase& phase_1 = route.Get(1); - const routing::Phase& phase_2 = route.Get(2); - CheckRoutingPhase(phase_0, 0, kLinearTolerance, std::vector{start_phase_0_}, - std::vector{end_phase_0_}, kPhase0LaneSRanges); - CheckRoutingPhase(phase_1, 1, kLinearTolerance, std::vector{start_phase_1_}, - std::vector{end_phase_1_}, kPhase1LaneSRanges); - CheckRoutingPhase(phase_2, 2, kLinearTolerance, std::vector{start_phase_2_}, - std::vector{end_phase_2_}, kPhase2LaneSRanges); + ASSERT_EQ(2u, routes.size()); + { + const routing::Route& route = routes[0]; + ASSERT_EQ(3, route.size()); + const routing::Phase& phase_0 = route.Get(0); + const routing::Phase& phase_1 = route.Get(1); + const routing::Phase& phase_2 = route.Get(2); + CheckRoutingPhase(phase_0, 0, kLinearTolerance, std::vector{route_0_phase_0_start_}, + std::vector{route_0_phase_0_end_}, kRoute0Phase0LaneSRanges); + CheckRoutingPhase(phase_1, 1, kLinearTolerance, std::vector{route_0_phase_1_start_}, + std::vector{route_0_phase_1_end_}, kRoute0Phase1LaneSRanges); + CheckRoutingPhase(phase_2, 2, kLinearTolerance, std::vector{route_0_phase_2_start_}, + std::vector{route_0_phase_2_end_}, kRoute0Phase2LaneSRanges); + } + { + const routing::Route& route = routes[1]; + ASSERT_EQ(3, route.size()); + const routing::Phase& phase_0 = route.Get(0); + const routing::Phase& phase_1 = route.Get(1); + const routing::Phase& phase_2 = route.Get(2); + CheckRoutingPhase(phase_0, 0, kLinearTolerance, std::vector{route_1_phase_0_start_}, + std::vector{route_1_phase_0_end_}, kRoute1Phase0LaneSRanges); + CheckRoutingPhase(phase_1, 1, kLinearTolerance, std::vector{route_1_phase_1_start_}, + std::vector{route_1_phase_1_end_}, kRoute1Phase1LaneSRanges); + CheckRoutingPhase(phase_2, 2, kLinearTolerance, std::vector{route_1_phase_2_start_}, + std::vector{route_1_phase_2_end_}, kRoute1Phase2LaneSRanges); + } } // The maximum cost of the phase is smaller than the solution's phase cost, so no routes can be found. TEST_F(DriveForwardStraightOverMultipleLanesTest, WithConstrainedPhaseCostReturnsEmpty) { - const std::vector routes = - dut_->ComputeRoutes(start_phase_0_, end_phase_2_, kSmallPhaseCostConstraint); + const std::vector routes = dut_->ComputeRoutes(start_, end_, kSmallPhaseCostConstraint); ASSERT_TRUE(routes.empty()); } // The maximum cost of the route is smaller than the solution's phase cost, so no routes can be found. TEST_F(DriveForwardStraightOverMultipleLanesTest, WithConstrainedRouteCostReturnsEmpty) { - const std::vector routes = - dut_->ComputeRoutes(start_phase_0_, end_phase_2_, kSmallRouteCostConstraint); + const std::vector routes = dut_->ComputeRoutes(start_, end_, kSmallRouteCostConstraint); ASSERT_TRUE(routes.empty()); } diff --git a/test/resources/odr/MultipleStraightSegments.xodr b/test/resources/odr/MultipleStraightSegments.xodr new file mode 100644 index 0000000..bd17ff6 --- /dev/null +++ b/test/resources/odr/MultipleStraightSegments.xodr @@ -0,0 +1,146 @@ + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + +
+
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + +
+
+ + +
+
diff --git a/test/route_end_to_end_connectivity_test.cc b/test/route_end_to_end_connectivity_test.cc new file mode 100644 index 0000000..287a6fb --- /dev/null +++ b/test/route_end_to_end_connectivity_test.cc @@ -0,0 +1,299 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Woven by Toyota. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "assert_compare.h" + +// @file This file evaluates the end to end connectivity constraints of a maliput::routing::Route. +// @details This is not done in maliput because of the need of a concrete maliput::api::RoadGeometry +// to evaluate all the topological and geometrical constraints. Mocking that for the purposes of +// this test would require implementing a concrete backend. In this test, an ad-hoc OpenDRIVE map is used. +// We don't need any fancy geometry or network topology, just two roads with two lanes each are enough +// to validate the end to end connectivity is correct. + +namespace maliput { +namespace test { +namespace { + +// Converts @p errors into a single string by concatenating each item with a " | " as separation. +// @param errors A vector of strings. +// @return A string with all items in @p error concatenated with " | " as separation. +std::string SerializeErrors(const std::vector& errors) { + std::string result; + for (const std::string& error : errors) { + result += error + " | "; + } + return result; +} + +class MultipleStraightSegmentsTest : public ::testing::Test { + protected: + //@{ Tolerances set to match the involved geometries and the parser resolution. + static constexpr double kLinearTolerance{1e-6}; + static constexpr double kAngularTolerance{1e-6}; + static constexpr double kScaleLength{1.0}; + const std::string kXodrFileName{"MultipleStraightSegments.xodr"}; + //@} + + static constexpr double kLaneLength{100.}; + const api::LaneId kLane_1_0_1{"1_0_1"}; + const api::LaneId kLane_1_0_m1{"1_0_-1"}; + const api::LaneId kLane_2_0_1{"2_0_1"}; + const api::LaneId kLane_2_0_m1{"2_0_-1"}; + + void SetUp() override { + std::map road_network_configuration; + road_network_configuration.emplace(malidrive::builder::params::kRoadGeometryId, "malidrive_rg"); + road_network_configuration.emplace(malidrive::builder::params::kOpendriveFile, kXodrFileName); + road_network_configuration.emplace(malidrive::builder::params::kLinearTolerance, std::to_string(kLinearTolerance)); + road_network_configuration.emplace(malidrive::builder::params::kAngularTolerance, + std::to_string(kAngularTolerance)); + road_network_configuration.emplace(malidrive::builder::params::kScaleLength, std::to_string(kScaleLength)); + road_network_configuration.emplace(malidrive::builder::params::kInertialToBackendFrameTranslation, "{0., 0., 0.}"); + road_network_ = malidrive::loader::Load(road_network_configuration); + lane_1_0_1_ = road_network_->road_geometry()->ById().GetLane(kLane_1_0_1); + lane_1_0_m1_ = road_network_->road_geometry()->ById().GetLane(kLane_1_0_m1); + lane_2_0_1_ = road_network_->road_geometry()->ById().GetLane(kLane_2_0_1); + lane_2_0_m1_ = road_network_->road_geometry()->ById().GetLane(kLane_2_0_m1); + + ASSERT_NE(nullptr, lane_1_0_1_); + ASSERT_NE(nullptr, lane_1_0_m1_); + ASSERT_NE(nullptr, lane_2_0_1_); + ASSERT_NE(nullptr, lane_2_0_m1_); + } + + const api::Lane* lane_1_0_1_{}; + const api::Lane* lane_1_0_m1_{}; + const api::Lane* lane_2_0_1_{}; + const api::Lane* lane_2_0_m1_{}; + std::unique_ptr road_network_{}; +}; + +class SinglePhaseTest : public MultipleStraightSegmentsTest {}; + +TEST_F(SinglePhaseTest, SinglePhaseWithOneLaneIsConnectedEndToEnd) { + const api::RoadPosition start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange lane_s_range(kLane_1_0_1, api::SRange(0, kLaneLength)); + const routing::Phase phase(0 /*index*/, kLinearTolerance, {start_position}, {end_position}, {lane_s_range}, + road_network_.get()); + const routing::Route dut({phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_TRUE(errors.empty()) << SerializeErrors(errors); +} + +TEST_F(SinglePhaseTest, SinglePhaseWithTwoLanesIsConnectedEndToEnd) { + const api::RoadPosition start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange lane_s_range_1(kLane_1_0_1, api::SRange(0, kLaneLength)); + const api::LaneSRange lane_s_range_m1(kLane_1_0_m1, api::SRange(0, kLaneLength)); + const routing::Phase phase(0 /*index*/, kLinearTolerance, {start_position}, {end_position}, + {lane_s_range_m1, lane_s_range_1}, road_network_.get()); + const routing::Route dut({phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_TRUE(errors.empty()) << SerializeErrors(errors); +} + +// Asserts that the first routing::Phase contains only one start position constraint is checked. +TEST_F(SinglePhaseTest, SinglePhaseWithTwoLanesAndStartPositionsIsNotConnectedEndToEnd) { + const api::RoadPosition start_position_1{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition start_position_m1{lane_1_0_m1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange lane_s_range_1(kLane_1_0_1, api::SRange(0, kLaneLength)); + const api::LaneSRange lane_s_range_m1(kLane_1_0_m1, api::SRange(0, kLaneLength)); + const routing::Phase phase(0 /*index*/, kLinearTolerance, {start_position_1, start_position_m1}, {end_position}, + {lane_s_range_m1, lane_s_range_1}, road_network_.get()); + const routing::Route dut({phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_FALSE(errors.empty()) << SerializeErrors(errors); +} + +// Asserts that the last routing::Phase contains only one end position constraint is checked. +TEST_F(SinglePhaseTest, SinglePhaseWithTwoLanesAndEndPositionsIsNotConnectedEndToEnd) { + const api::RoadPosition start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition end_position_1{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::RoadPosition end_position_m1{lane_1_0_m1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange lane_s_range_1(kLane_1_0_1, api::SRange(0, kLaneLength)); + const api::LaneSRange lane_s_range_m1(kLane_1_0_m1, api::SRange(0, kLaneLength)); + const routing::Phase phase(0 /*index*/, kLinearTolerance, {start_position}, {end_position_1, end_position_m1}, + {lane_s_range_m1, lane_s_range_1}, road_network_.get()); + const routing::Route dut({phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_FALSE(errors.empty()) << SerializeErrors(errors); +} + +class MultiPhaseTest : public MultipleStraightSegmentsTest {}; + +TEST_F(MultiPhaseTest, MultiPhaseWithOneLaneEachIsConnectedEndToEnd) { + const api::RoadPosition first_start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition first_end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange first_lane_s_range(kLane_1_0_1, api::SRange(0, kLaneLength)); + const routing::Phase first_phase(0 /*index*/, kLinearTolerance, {first_start_position}, {first_end_position}, + {first_lane_s_range}, road_network_.get()); + const api::RoadPosition last_start_position{lane_2_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition last_end_position{lane_2_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange last_lane_s_range(kLane_2_0_1, api::SRange(0, kLaneLength)); + const routing::Phase last_phase(1 /*index*/, kLinearTolerance, {last_start_position}, {last_end_position}, + {last_lane_s_range}, road_network_.get()); + const routing::Route dut({first_phase, last_phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_TRUE(errors.empty()) << SerializeErrors(errors); +} + +// Asserts that adjacent routing::Phases have geometrically matching end and start positions constraint is checked. +TEST_F(MultiPhaseTest, MultiPhaseWithOneLaneEachIsNotGeometricallyConnectedEndToEnd) { + const api::RoadPosition first_start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition first_end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange first_lane_s_range(kLane_1_0_1, api::SRange(0, kLaneLength)); + const routing::Phase first_phase(0 /*index*/, kLinearTolerance, {first_start_position}, {first_end_position}, + {first_lane_s_range}, road_network_.get()); + const api::RoadPosition last_start_position{lane_2_0_1_, api::LanePosition{10., 0., 0.}}; + const api::RoadPosition last_end_position{lane_2_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange last_lane_s_range(kLane_2_0_1, api::SRange(10., kLaneLength)); + const routing::Phase last_phase(1 /*index*/, kLinearTolerance, {last_start_position}, {last_end_position}, + {last_lane_s_range}, road_network_.get()); + const routing::Route dut({first_phase, last_phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_FALSE(errors.empty()) << SerializeErrors(errors); +} + +// Asserts that adjacent routing::Phases have topologically connected end and start positions by inspecting +// the api::BranchPoint associations constraint is checked. +TEST_F(MultiPhaseTest, MultiPhaseWithOneLaneEachIsNotTopologicallyConnectedEndToEnd) { + const api::RoadPosition first_start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition first_end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange first_lane_s_range(kLane_1_0_1, api::SRange(0, kLaneLength)); + const routing::Phase first_phase(0 /*index*/, kLinearTolerance, {first_start_position}, {first_end_position}, + {first_lane_s_range}, road_network_.get()); + const api::RoadPosition last_start_position{lane_2_0_m1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition last_end_position{lane_2_0_m1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange last_lane_s_range(kLane_2_0_m1, api::SRange(0., kLaneLength)); + const routing::Phase last_phase(1 /*index*/, kLinearTolerance, {last_start_position}, {last_end_position}, + {last_lane_s_range}, road_network_.get()); + const routing::Route dut({first_phase, last_phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_FALSE(errors.empty()) << SerializeErrors(errors); +} + +// Asserts that adjacent routing::Phases have the same number of end and start positions constraint is checked. +TEST_F(MultiPhaseTest, MultiPhaseWithDifferentNumberOfPositionsAtInterfaceIsNotConnectedEndToEnd) { + const api::RoadPosition first_start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition first_end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange first_lane_s_range(kLane_1_0_1, api::SRange(0, kLaneLength)); + const routing::Phase first_phase(0 /*index*/, kLinearTolerance, {first_start_position}, {first_end_position}, + {first_lane_s_range}, road_network_.get()); + const api::RoadPosition last_start_position_1{lane_2_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition last_start_position_m1{lane_2_0_m1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition last_end_position{lane_2_0_m1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange last_lane_s_range_1(kLane_2_0_1, api::SRange(0., kLaneLength)); + const api::LaneSRange last_lane_s_range_m1(kLane_2_0_m1, api::SRange(0., kLaneLength)); + const routing::Phase last_phase(1 /*index*/, kLinearTolerance, {last_start_position_1, last_start_position_m1}, + {last_end_position}, {last_lane_s_range_1, last_lane_s_range_m1}, + road_network_.get()); + const routing::Route dut({first_phase, last_phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_FALSE(errors.empty()) << SerializeErrors(errors); +} + +TEST_F(MultiPhaseTest, MultiPhaseWithinSameLaneIsConnectedEndToEnd) { + const api::RoadPosition first_start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition first_end_position{lane_1_0_1_, api::LanePosition{kLaneLength / 2., 0., 0.}}; + const api::LaneSRange first_lane_s_range(kLane_1_0_1, api::SRange(0, kLaneLength / 2.)); + const routing::Phase first_phase(0 /*index*/, kLinearTolerance, {first_start_position}, {first_end_position}, + {first_lane_s_range}, road_network_.get()); + const api::RoadPosition last_start_position{lane_1_0_1_, api::LanePosition{kLaneLength / 2., 0., 0.}}; + const api::RoadPosition last_end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange last_lane_s_range(kLane_1_0_1, api::SRange(kLaneLength / 2., kLaneLength)); + const routing::Phase last_phase(1 /*index*/, kLinearTolerance, {last_start_position}, {last_end_position}, + {last_lane_s_range}, road_network_.get()); + const routing::Route dut({first_phase, last_phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_TRUE(errors.empty()) << SerializeErrors(errors); +} + +// Asserts that adjacent routing::Phases whose api::LaneSRanges belong to the same api::Lane have end and +// start geometrically matching positions. +TEST_F(MultiPhaseTest, MultiPhaseWithinSameLaneIsNotGeometricallyConnectedEndToEnd) { + const api::RoadPosition first_start_position{lane_1_0_1_, api::LanePosition{0., 0., 0.}}; + const api::RoadPosition first_end_position{lane_1_0_1_, api::LanePosition{kLaneLength / 2., 0., 0.}}; + const api::LaneSRange first_lane_s_range(kLane_1_0_1, api::SRange(0, kLaneLength / 2.)); + const routing::Phase first_phase(0 /*index*/, kLinearTolerance, {first_start_position}, {first_end_position}, + {first_lane_s_range}, road_network_.get()); + const api::RoadPosition last_start_position{lane_1_0_1_, api::LanePosition{kLaneLength / 2. + 5., 0., 0.}}; + const api::RoadPosition last_end_position{lane_1_0_1_, api::LanePosition{kLaneLength, 0., 0.}}; + const api::LaneSRange last_lane_s_range(kLane_1_0_1, api::SRange(kLaneLength / 2. + 5., kLaneLength)); + const routing::Phase last_phase(1 /*index*/, kLinearTolerance, {last_start_position}, {last_end_position}, + {last_lane_s_range}, road_network_.get()); + const routing::Route dut({first_phase, last_phase}, road_network_.get()); + + const std::vector errors = dut.ValidateEndToEndConnectivity(); + + ASSERT_FALSE(errors.empty()) << SerializeErrors(errors); +} + +} // namespace +} // namespace test +} // namespace maliput