Fix route calculation when using nonzero cost id

lanelet2_routing/include/lanelet2_routing/Forward.h

@@ -55,7 +55,7 @@ using LaneletOrAreaPaths = std::vector<LaneletOrAreaPath>;
 //! symmetry. When a is left of b, b can be either right or adjacent right to b.
 enum class RelationType : uint8_t {
   None = 0,                 //!< No relation
-  Successor = 0b1,          //!< A (the only) direct, reachable successor. Not merging and not diverging.
+  Successor = 0b1,          //!< A direct, reachable successor
   Left = 0b10,              //!< (the only) directly adjacent, reachable left neighbour
   Right = 0b100,            //!< (the only) directly adjacent, reachable right neighbour
   AdjacentLeft = 0b1000,    //!< directly adjacent, unreachable left neighbor

lanelet2_routing/include/lanelet2_routing/internal/GraphUtils.h

@@ -237,11 +237,17 @@ class ConflictingSectionFilter {
         return;
       }
       auto type = (*g_)[edge].relation;
-      auto neighbourTypes = RelationType::Left | RelationType::Right;
-      auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight;
+      const auto neighbourTypes = RelationType::Left | RelationType::Right;
+      const auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight;
       if ((type & (neighbourTypes)) != RelationType::None) {
-        auto outEdge = boost::edge(boost::target(edge, *g_), boost::source(edge, *g_), *g_);
-        auto reverseIsNeigh = outEdge.second && ((*g_)[outEdge.first].relation & neighbourTypes) != RelationType::None;
+        // The following is basically equivalent to interating over reverse edges like below, but this doesn't compile
+        // due to a boost::graph bug:
+        // boost::edge_range(boost::target(edge, *g_), boost::source(edge, *g_), *g_);
+        auto outEdges = boost::out_edges(boost::target(edge, *g_), *g_);
+        auto reverseIsNeigh = std::any_of(outEdges.first, outEdges.second, [&](auto outEdge) {
+          return boost::target(outEdge, *g_) == boost::source(edge, *g_) &&
+                 ((*g_)[outEdge].relation & neighbourTypes) != RelationType::None;
+        });
         isNeighbour |= reverseIsNeigh;
         isConflicting |= !isNeighbour;
       }

lanelet2_routing/src/RouteBuilder.cpp

@@ -307,7 +307,9 @@ class RouteConstructionVisitor : public boost::default_bfs_visitor {
       // In this case we have to adjust lane Ids
       setLaneIdFromTo(routeGraph()[*destVertex].laneId, routeGraph()[sourceElem_].laneId);
     }
-    routeGraph_->addEdge(sourceElem_, *destVertex, g[e]);
+    auto edgeInfo = g[e];
+    edgeInfo.costId = 0;
+    routeGraph_->addEdge(sourceElem_, *destVertex, edgeInfo);
   }
   void finish_vertex(LaneletVertexId v, const OnRouteGraph& g) {  // NOLINT
     // now that all neighbours are known, now is the time to add lanelets that conflict in the whole graph

lanelet2_routing/test/test_route.cpp

@@ -11,7 +11,7 @@ using namespace lanelet::routing::tests;
 template <Id FromId, Id ToId, Id... ViaIds>
 class TestRoute : public GermanVehicleGraph {
  public:
-  explicit TestRoute(uint16_t routingCostId = 0, bool withLaneChanges = true) {
+  explicit TestRoute(uint16_t routingCostId = 0, bool withLaneChanges = true) : withLaneChanges{withLaneChanges} {
     Ids viaIds{ViaIds...};
     start = lanelets.at(From);
     end = lanelets.at(To);
@@ -21,6 +21,7 @@ class TestRoute : public GermanVehicleGraph {
     EXPECT_TRUE(!!optRoute);
     route = std::move(*optRoute);
   }
+  bool withLaneChanges{true};
   ConstLanelet start;
   ConstLanelet end;
   ConstLanelets via;
@@ -68,6 +69,15 @@ using AllRoutes =
 TYPED_TEST_SUITE(AllRoutesTest, AllRoutes);
 
 TYPED_TEST(AllRoutesTest, CheckValidity) { EXPECT_NO_THROW(this->route.checkValidity()); }  // NOLINT
+TYPED_TEST(AllRoutesTest, CheckEquality) {
+  // This ensures that the same route is computed no matter the routing cost id
+  auto route2 = this->graph->getRouteVia(this->start, this->via, this->end, 2, this->withLaneChanges);
+  EXPECT_TRUE(!!route2);
+  auto map0 = this->route.laneletSubmap();
+  auto map2 = route2->laneletSubmap();
+  EXPECT_TRUE(std::equal(map0->laneletLayer.begin(), map0->laneletLayer.end(), map2->laneletLayer.begin(),
+                         map2->laneletLayer.end()));
+}
 
 TYPED_TEST(AllRoutesTest, ShortestMapInDebugMap) {
   const auto map{this->route.getDebugLaneletMap()};

lanelet2_routing/test/test_routing_map.h

@@ -7,6 +7,7 @@
 #include <memory>
 #include <utility>
 
+#include "RoutingCost.h"
 #include "lanelet2_routing/Forward.h"
 #include "lanelet2_routing/RoutingGraph.h"
 
@@ -21,7 +22,8 @@ inline RoutingGraphPtr setUpGermanVehicleGraph(LaneletMap& map, double laneChang
   traffic_rules::TrafficRulesPtr trafficRules{traffic_rules::TrafficRulesFactory::create(
       Locations::Germany, Participants::Vehicle, traffic_rules::TrafficRules::Configuration())};
   RoutingCostPtrs costPtrs{std::make_shared<RoutingCostDistance>(laneChangeCost, minLaneChangeLength),
-                           std::make_shared<RoutingCostTravelTime>(laneChangeCost)};
+                           std::make_shared<RoutingCostTravelTime>(laneChangeCost),
+                           std::make_shared<RoutingCostDistance>(laneChangeCost, minLaneChangeLength)};
   RoutingGraph::Configuration configuration;
   configuration.insert(std::make_pair(RoutingGraph::ParticipantHeight, participantHeight));
   return RoutingGraph::build(map, *trafficRules, costPtrs, configuration);
@@ -496,7 +498,7 @@ class GermanVehicleGraph : public RoutingGraphTest {
 
  public:
   RoutingGraphConstPtr graph{testData.vehicleGraph};
-  uint8_t numCostModules{2};
+  uint8_t numCostModules{3};
 };
 
 class GermanPedestrianGraph : public RoutingGraphTest {