lanelet2 routing not working on two-way lanelets

[johannes-fischer]

The example map contains a section of two-way lanelets with a corresponding attribute. The routing graph seems to be set up incorrectly for these.

Code example:

#!/usr/bin/env python
import os
import lanelet2
from lanelet2.projection import UtmProjector

example_file = os.path.join(os.path.dirname(os.path.abspath(
    __file__)), "../../lanelet2_maps/res/mapping_example.osm")
projector = UtmProjector(lanelet2.io.Origin(49, 8.4))
lanelet_map = lanelet2.io.load(example_file, projector)

traffic_rules = lanelet2.traffic_rules.create(lanelet2.traffic_rules.Locations.Germany,
                                                   lanelet2.traffic_rules.Participants.Vehicle)
graph = lanelet2.routing.RoutingGraph(lanelet_map, traffic_rules)

lanelet_in_roundabout = lanelet_map.laneletLayer[45334]
lanelet_before_roundabout = lanelet_map.laneletLayer[45356]
lanelet_on_road_before = lanelet_map.laneletLayer[45358]

assert(len(graph.following(lanelet_before_roundabout)) == 1)
assert(len(graph.following(lanelet_on_road_before)) == 1)
assert(len(graph.possiblePaths(lanelet_before_roundabout, 100, 0, True)) == 1)
assert(graph.getRoute(lanelet_before_roundabout, lanelet_in_roundabout)  == None)
assert(graph.getRoute(lanelet_on_road_before, lanelet_before_roundabout) == None)
assert(graph.getRoute(lanelet_before_roundabout, lanelet_on_road_before) != None) # route along lane
assert((lanelet_in_roundabout.id in [lanelet.id for lanelet in graph.reachableSet(lanelet_before_roundabout, 100, 0, True)]) == False)

Here lanelet_before_roundabout and lanelet_on_road_before are two-way lanelets (see image below) but the routing graph can only find routing from lanelet_before_roundabout to lanelet_on_road_before not the other direction. Also, it cannot find routing from lanelet_before_roundabout onto the roundabout. Hence, it seems there is a bug in the routing graph builder with two-way lanelets.

Furthermore, assuming the routing between lanelet_before_roundabout and lanelet_on_road_before is fixed, there might be additional issues with driving onto the from lanelet_before_roundabout to lanelet_in_roundabout, since this code compares whether the end of previous lanelet left bound and beginning of next lanelet left bound match:

template <typename Lanelet1T, typename Lanelet2T>
IfLL<Lanelet1T, IfLL<Lanelet2T, bool>> follows(const Lanelet1T& prev, const Lanelet2T& next) {
  return !prev.leftBound().empty() && !prev.rightBound().empty() && !next.leftBound().empty() &&
         !next.rightBound().empty() && prev.leftBound().back() == next.leftBound().front() &&
         prev.rightBound().back() == next.rightBound().front();
}

However, in this case of two-way lanelets, the right bound of lanelet_in_roundabout attaches to the left bound of lanelet_before_roundabout. Possibly, this can be solved by considering two-way lanelets in both directions for routing purposes.

[poggenhans]

Lanelets have an orientation. If you want to search for a route in the opposite direction, you have to search with the inverted lanelet.

In your example assert(graph.following(lanelet_on_road_before.invert()) == [lanelet_before_roundabout.invert()])

[johannes-fischer]

Oh great thanks, that works! I assumed the id is enough to identify a lanelet uniquely and since the id remains the same after lanelet.invert(), I thought the inverted lanelet is still considered the same lanelet. Now I understand that only the tuple (lanelet.id, lanelet.inverted()) uniquely defines a lanelet.

I checked out the exported routing graph files and saw that they contain the two-way lanelet ids twice as different graph nodes, once for each direction.