diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 7f8f433..d9a6a0a 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -18,9 +18,9 @@ jobs: container: image: ubuntu:20.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.6 - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@v0.3 id: action_ros_ci_step with: package-name: ${{ env.PACKAGE_NAME }} diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index a689251..b95574e 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -37,7 +37,7 @@ jobs: - uses: ros-tooling/setup-ros@v0.6 env: ACTIONS_ALLOW_UNSECURE_COMMANDS: true - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: ${{ env.ROS_WS }}/src/${{ env.PACKAGE_NAME }} - name: clang 8 install diff --git a/.github/workflows/scan_build.yml b/.github/workflows/scan_build.yml index a70d75c..fa72e70 100644 --- a/.github/workflows/scan_build.yml +++ b/.github/workflows/scan_build.yml @@ -26,7 +26,7 @@ jobs: - uses: ros-tooling/setup-ros@v0.6 env: ACTIONS_ALLOW_UNSECURE_COMMANDS: true - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: ${{ env.ROS_WS }}/src/${{ env.PACKAGE_NAME }} - name: clang 8 install diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e518050..0fa41da 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -15,6 +15,7 @@ macro(add_dependencies_to_test target) maliput_dragway::maliput_dragway maliput_dragway::maliput_dragway_test_utilities maliput_malidrive::builder + maliput_malidrive::loader maliput_multilane::maliput_multilane maliput_multilane::test_utilities) @@ -40,6 +41,7 @@ file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/data/NoBranchPointsObjContent.obj DESTINAT file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/data/StackedBranchPointsObjContent.obj 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) ament_add_gtest(find_lane_sequences_test find_lane_sequences_test.cc) ament_add_gtest(generate_obj_test generate_obj_test.cc WORKING_DIRECTORY ${TEST_PATH}) ament_add_gtest(generate_string_test generate_string_test.cc) @@ -53,6 +55,7 @@ ament_add_gtest(traffic_light_book_loader_test traffic_light_book_loader_test.cc ament_add_gtest(waypoints_test waypoints_test.cc) add_dependencies_to_test(derive_lane_s_routes_test) +add_dependencies_to_test(distance_router_test) add_dependencies_to_test(find_lane_sequences_test) add_dependencies_to_test(generate_obj_test) add_dependencies_to_test(generate_string_test) diff --git a/test/distance_router_test.cc b/test/distance_router_test.cc new file mode 100644 index 0000000..2d3d5e8 --- /dev/null +++ b/test/distance_router_test.cc @@ -0,0 +1,311 @@ +// 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 "maliput/base/distance_router.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "assert_compare.h" + +namespace maliput { +namespace test { +namespace { + +using maliput::api::IsEqual; +using maliput::api::IsLanePositionClose; + +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()); + 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(end_pos.size(), phase.end_positions().size()); + 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(lane_s_ranges.size(), phase.lane_s_ranges().size()); + 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); + } +} + +class TShapeRoadRoutingTest : public ::testing::Test { + public: + //@{ 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}; + //@} + static constexpr routing::RoutingConstraints kDefaultRoutingConstraints{}; + static constexpr routing::RoutingConstraints kSmallPhaseCostConstraint{ + true /* allow_lane_switch */, std::optional{1.} /* max_phase_cost */, std::nullopt /* max_route_cost */ + }; + static constexpr routing::RoutingConstraints kSmallRouteCostConstraint{ + true /* allow_lane_switch */, std::nullopt /* max_phase_cost */, std::optional{1.} /* max_route_cost */ + }; + const std::string kTShapeRoadFilePath{std::string(kMalidriveResourcesPath) + + std::string("/resources/odr/TShapeRoad.xodr")}; + + std::unique_ptr road_network_{}; + std::unique_ptr dut_{}; + + 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, kTShapeRoadFilePath); + 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); + + dut_ = std::make_unique(*road_network_, kLinearTolerance); + } +}; + +// 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.)); + } + + api::RoadPosition start_; + api::RoadPosition end_; +}; + +// 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 routes = dut_->ComputeRoutes(start_, end_, kDefaultRoutingConstraints); + + ASSERT_EQ(1u, routes.size()); + const routing::Route& route = routes[0]; + ASSERT_EQ(1, route.size()); + const routing::Phase& phase = route.Get(0); + CheckRoutingPhase(phase, 0, kLinearTolerance, std::vector{start_}, + std::vector{end_}, kPhaseLaneSRanges); +} + +// The maximum cost of the phase is smaller than the solution's phase cost, so no routes can be found. +TEST_F(RoutingInTheSameLaneTest, WithConstrainedPhaseCostReturnsEmpty) { + 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(RoutingInTheSameLaneTest, WithConstrainedRouteCostReturnsEmpty) { + const std::vector routes = dut_->ComputeRoutes(start_, end_, kSmallRouteCostConstraint); + + ASSERT_TRUE(routes.empty()); +} + +// Defines the test cases for the TShapeRoad where the start and end positions are on the extremes of the +// 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_; + + 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.)); + } + + 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_; +}; + +// 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); + + 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); +} + +// 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); + + 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); + + ASSERT_TRUE(routes.empty()); +} + +// Defines the test cases for the TShapeRoad where the start and end positions are on the extremes of the +// 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_; + + 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.)); + } + + 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_; +}; + +// 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); + + 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); +} + +// 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); + + 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); + + ASSERT_TRUE(routes.empty()); +} + +} // namespace +} // namespace test +} // namespace maliput diff --git a/test/find_lane_sequences_test.cc b/test/find_lane_sequences_test.cc index ce0e09e..eb62d4e 100644 --- a/test/find_lane_sequences_test.cc +++ b/test/find_lane_sequences_test.cc @@ -29,6 +29,7 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "maliput/routing/find_lane_sequences.h" +#include #include #include #include @@ -39,6 +40,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -90,6 +94,14 @@ TEST_F(DragwayBasedTest, FindLaneSequencesChangeLanes) { CheckSequences(FindLaneSequences(left_lane_, right_lane_, kLength), {}); } +TEST_F(DragwayBasedTest, FindLaneSequencesChangeLanesWithMaxLength) { + constexpr double kMaxLength{std::numeric_limits::max()}; + CheckSequences(FindLaneSequences(center_lane_, left_lane_, kMaxLength), {}); + CheckSequences(FindLaneSequences(center_lane_, right_lane_, kMaxLength), {}); + CheckSequences(FindLaneSequences(right_lane_, left_lane_, kMaxLength), {}); + CheckSequences(FindLaneSequences(left_lane_, right_lane_, kMaxLength), {}); +} + TEST_F(DragwayBasedTest, FindLaneSequencesSameLane) { for (double length : std::vector{kLength, 0.}) { CheckSequences(FindLaneSequences(center_lane_, center_lane_, length), {{center_lane_->id().string()}}); @@ -141,6 +153,7 @@ TEST_F(LoopBasedTest, FindLaneSequencesTest) { const Lane* start_lane = index_.GetLane(LaneId("l:0_0")); const Lane* end_lane = index_.GetLane(LaneId("l:4_0")); const std::vector> sequences = FindLaneSequences(start_lane, end_lane, kMaxLength); + ASSERT_EQ(sequences.size(), 5u); } @@ -180,5 +193,54 @@ GTEST_TEST(FindLaneSequencesTest, MaxLengthOmitsStartAndEndLanes) { ASSERT_EQ(FindLaneSequences(start_lane, end_lane, total_length).size(), 1u); } +static constexpr char kMalidriveResourcesPath[] = DEF_MALIDRIVE_RESOURCES; + +// Loads TShapeRoad, and evaluates FindLaneSequences() for a large distance that will not +// filter out results, and shows the behavior when removing and leaving U-turns. +class TShapeRoadFindLaneSequencesTest : public ::testing::Test { + public: + //@{ 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}; + static constexpr double kDistance{std::numeric_limits::max()}; + static constexpr bool kDontRemoveUTurns{false}; + static constexpr bool kRemoveUTurns{true}; + //@} + const std::string kTShapeRoadFilePath{std::string(kMalidriveResourcesPath) + + std::string("/resources/odr/TShapeRoad.xodr")}; + + const api::LaneId kStartLaneId{"1_0_1"}; + const api::LaneId kEndLaneId{"0_0_1"}; + const Lane* start_lane_; + const Lane* end_lane_; + + void SetUp() override { + std::map configuration; + configuration.emplace(malidrive::builder::params::kRoadGeometryId, "malidrive_rg"); + configuration.emplace(malidrive::builder::params::kOpendriveFile, kTShapeRoadFilePath); + configuration.emplace(malidrive::builder::params::kLinearTolerance, std::to_string(kLinearTolerance)); + configuration.emplace(malidrive::builder::params::kAngularTolerance, std::to_string(kAngularTolerance)); + configuration.emplace(malidrive::builder::params::kScaleLength, std::to_string(kScaleLength)); + configuration.emplace(malidrive::builder::params::kInertialToBackendFrameTranslation, "{0., 0., 0.}"); + road_network_ = malidrive::loader::Load(configuration); + start_lane_ = road_network_->road_geometry()->ById().GetLane(kStartLaneId); + end_lane_ = road_network_->road_geometry()->ById().GetLane(kEndLaneId); + } + + std::unique_ptr road_network_{}; +}; + +TEST_F(TShapeRoadFindLaneSequencesTest, NotRemovingUTurnYieldsTwoSequences) { + CheckSequences( + FindLaneSequences(start_lane_, end_lane_, kDistance, kDontRemoveUTurns), + {{"1_0_1", "6_0_-1", "2_0_1", "9_0_-1", "0_0_-1", "5_0_-1", "1_0_-1", "7_0_-1", "2_0_-1", "8_0_-1", "0_0_1"}, + {"1_0_1", "4_0_1", "0_0_1"}}); +} + +TEST_F(TShapeRoadFindLaneSequencesTest, RemovingUTurnYieldsOneSequence) { + CheckSequences(FindLaneSequences(start_lane_, end_lane_, kDistance, kRemoveUTurns), {{"1_0_1", "4_0_1", "0_0_1"}}); +} + } // namespace routing } // namespace maliput