Lanelet2 routing

The routing module for lanelet2.

For a short version of this you can also look at the presentation. If some images do not render correctly, please clone the repository and open the html file in your browser.

This readme covers some basics. The API offers more than that.

1. Components and Vocabulary

How to create a Routing Graph

The needed components to create a routing graph are:

Routing Cost Modules: * They generically determine the routing cost for travelling along a lanelet/area * Can be e.g. length, travel time * You can dynamically select between them when querying the routing graph * You can easily plug in your own routing cost calculation * Influences the prefered path

Traffic Rules for a Specific Participant (see lanelet2_traffic_rules) * Determines which lanelets/areas are passable * Influences the possible paths

Lanelet Map: (see lanelet2_core) * Map with Lanelets, Areas, Regulatory Elements, ...

Relations

Lanelets that are part of a routing graph can have relations to each other:

The possible relations are: * left, right (reachable via lane change) * adjacent left, adjacent right (lanelets that are neighbours but not reachable via lane change) * succeeding (relation between two subsequent lanelets) * conflicting (intersecting lanelets/areas) * area (reachable area to lanelet/area relation)

Route vs Path vs Sequence

When querying data in the routing graph, you will come across the terms route, path and sequence. In contrast to a simple set of lanelets (data-wise a vector of lanelets), they have a special meaning and are data-wise different classes.

A route means all the lanelets that can be used to a destination without driving a different road. They can be connected by a generic sequence of lane changes and successors.

A path (LaneletPath or LaneletOrAreaPath) is an ordered list of Lanelets/Areas that lead to the destination. They can be connected by lane changes.

A sequence (LaneletSequence) is a sequence of subsequent Lanelets that is not separated by a lane change (think of it as a lane). It does not necessary lead to a destination, instead it ends when a lane change is required. In the example image, the lanelets A, D, B form a valid sequence (and also a valid path), while the lanelets A, D, E are a valid path, but not a valid sequence.

2. Code Usage

Create a Routing Graph

using namespace lanelet;

// Load a map
LaneletMapPtr map = load("map.osm", Origin({49, 8})); // origin has to be close to the map data in lat/lon coordinates

// Initialize traffic rules
TrafficRulesPtr trafficRules{TrafficRulesFactory::instance().create(Locations::Germany, Participants::Vehicle)};

// Optional: Initalize routing costs
double laneChangeCost = 2;
RoutingCostPtrs costPtrs{std::make_shared<RoutingCostDistance>(laneChangeCost)};

// Optional: Initialize config for routing graph:
RoutingGraph::Configuration routingGraphConf;
routingGraphConf.emplace(std::make_pair(RoutingGraph::ParticipantHeight, Attribute("2.")));

// Create routing graph
RoutingGraphPtr graph = RoutingGraph::build(map, trafficRules /*, costPtrs, routingGraphConf*/);

• The traffic rules object represents the view from which the map will be interpreted. Doing routing with vehicle traffic rules will yield different results than routing with e.g. bicycle traffic rules.
• Routing for bicycles might include lanelets that are not available to (motorized)vehicles and vice versa.

The python interface works similarly:

import lanelet2
map = lanelet2.io.load("map.osm", lanelet2.io.Origin(49, 8))
trafficRules = lanelet2.traffic_rules.create(lanelet2.traffic_rules.Locations.Germany, lanelet2.traffic_rules.Participants.Vehicle)
graph = lanelet2.routing.RoutingGraph(map, trafficRules)

Get a shortest path

Optional<routing::LaneletPath> shortestPath = graph->shortestPath(fromLanelet, toLanelet);

• Optional will be uninitialized (false) if there's no path
• there's also shortestPathWithVia

in python:

shortestPath = graph.shortestPath(fromLanelet, toLanelet)

In python, shortestPath simply returns None if there is no path.

Get and write a route

    Optional<Route> route = graph->getRoute(fromLanelet, toLanelet, routingCostId);
    if (route) {
        LaneletSubmapConstPtr routeMap = route->laneletSubmap();
        write("route.osm", *routeMap->laneletMap(), Origin({49, 8}));
    }

• Optional will be uninitialized (false) if there's no route

Note that there is a semantic difference between a LaneletSubmap and a LaneletMap. While a LaneletSubmap only contains the things you explicitly added, the LaneletMap also contains all the things referred by them (the Points, Linestrings, things referred by RegulatoryElements). The LaneletSubmap in this case only contains the Lanelets of the route. But since this is not sufficient for writing, you need to transform it into a regular LaneletMap first.

The written map will therefore not only contain the Lanelets but also their RegulatoryElements. If these RegulatoryElements contain other Lanelets, these Lanelets will be part of the written map as well, even if they are not on the route.

in python:

route = graph.getRoute(fromLanelet, toLanelet, routingCostId)
if route:
    laneletSubmap = route.laneletSubmap()
    lanelet2.io.write("route.osm", laneletSubmap.laneletMap(), lanelet2.io.Origin(49, 8)))

Get a reachable set of lanelets

double maxRoutingCost{100};
ConstLanelets reachableSet = graph->reachableSet(lanelet, maxRoutingCost, routingCostId);

Left, Right, Following Lanelets

// Get routable left lanelet if it exists
Optional<ConstLanelet> left{graph->left(fromLanelet)};
// Get non-routable left lanelet if it exists
Optional<ConstLanelet> adjacentLeft{graph->adjacentLeft(fromLanelet)};
// Get following lanelets
ConstLanelets following{graph->following(fromLanelet)};

• Also available: right, adjacentRight, lefts, rights, conflicting

Alternatively: or queries that return relations:

// Get relations to all left lanelets
LaneletRelations leftRelations = graph->leftRelations(
                                                fromLanelet);

There's leftRelations that returns a vector of pairs of LaneletRelations whereas RelationType can be 'left' or 'adjacentLeft' in this case

More

This is just a quick walkthrough. Advanced examples can be found in lanelet2_examples.

3. Export and Debugging Routing Graphs

LaneletMap with Routing Information

LaneletMapConstPtr debugLaneletMap = graph->getDebugLaneletMap(RoutingCostId(0));
write(std::string("routing_graph.osm"), *debugLaneletMap);

This one is best viewed in JOSM and using a custom map style css which is to be found in res/routing.mapcss. This gif shows, how to add a style to JOSM, except that one needs to press the + button in the configuration menu and specify the file.

Most of the information is to be found in the attributes. The line strings that connect lanelets do have a direction. The name of the forth-direction is generally to be found left/above the line and the reverse relation right/under the string.

DOT (GraphViz) and GraphML (xml-based) file export

graph->exportGraphViz("~/graph.gv");
graph->exportGraphML("~/graph.graphml");

These can then be viewed with a graph viewer like Gephi. The downside compared to the laneletMap export is, that the lanelets aren't localized.

4. Routes

Example route through Oststadtkreisel:

Output of getDebugLaneletMap() function:

Example Relational Queries on Routes:

// Get left lanelet of example lanelet 'll'
Optional<ConstLaneletRelation> left = route->leftRelation(ll);
// Get conflicting lanelets of 'll'
ConstLanelets conflicting = route->conflictingInRoute(ll);

Note that a route just returns relations to lanelets that can be used to reach the goal.

Other example queries:

// Get underlying shortest path
Optional<routing::LaneletPath> shortestPath = route->shortestPath();
// Get the full lane of a given lanelet 'll'
LaneletSequence fullLane = route->fullLane(ll);
// Get remaining lane of a given lanelet 'll'
LaneletSequence remainingLane = route->remainingLane(ll);

5. Interconnect Routing Graphs of Different Participants

A RoutingGraphContainer can be used to connect graphs of different participants to get information about conflicting lanelets.

Create a RoutingGraphContainer

std::vector<RoutingGraphPtr> graphs;
graphs.emplace_back(vehicleGraphLaneletMap);
graphs.emplace_back(pedestrianGraphLaneletMap);
RoutingGraphContainer container(graphs);

Example Queries

The last parameter participantHeight is optional and decides whether conflicting lanelets are determined in 2D or 3D.

Query for a single lanelet

double heightClearance{4.}; // Height of the traffic participant
// Query a single graph for conflicting lanelets
size_t routingGraphId{0};   // E.g. 0 for the first graph
ConstLanelets conflictingVehicle{container->conflictingInGraph(bridgeLanelet, routingGraphId, heightClearance)};
// Query all graphs for conflicting lanelets
RoutingGraphContainer::ConflictingInGraphs conflicting{container->conflictingInGraphs(bridgeLanelet, heightClearance)};

Query for a Whole Route

// Conflicting lanelets of a route in a single graph
ConstLanelets conflictingVehicle{container->conflictingOfRouteInGraph(routePtr, routingGraphId)};
// Conflicting lanelets of a route in all graphs
RoutingGraphContainer::ConflictingInGraphs result{container->conflictingOfRouteInGraphs(routePtr, heightClearance)};

Changelog for package lanelet2_routing

1.2.1 (2023-05-10)

• Update thirdparty deps, rework projected point, enable python 3.10/3.11 (#300)
• Contributors: poggenhans

1.2.0 (2023-01-30)

• Add CI using GitHub Actions (#256)
• Use TYPED_TEST_SUITE over deprecated TYPED_TEST_CASE in unit tests
• Add missing include for boost 1.69
• Contributors: Fabian Poggenhans, Fabian Immel

1.1.1 (2020-09-14)

1.1.0 (2020-09-06)

• Implement more flexible configuration for obtaining possible paths. Still lacking proper tests
• Add parameter to left/right/adjacentLeft/adjacentRight so that they can be queried based on routing cost id
• Fix routing for lane changes, add tests for it
• Fix wrong route in circles
• Fix unittests that rely on a non-writable root directory
• Add experimental support for building with colcon on ros2 and ament_cmake
• Fix the documentation about how to create a routing graph
• Removing extra semicolons causing warnings with wpedantic.
• Making all includes in lanelet2_routing consistent.
• Updating package.xml files to format 3.
• Fix use of equals in older boost versions
• Arbitrary lanelet and area adjacencies in LaneletPaths
• Add functionality to create the bounding polygon from a Path
• Contributors: Fabian Poggenhans, Johannes Janosovits, Joshua Whitley

1.0.1 (2020-03-24)

• Mention laneletSubmap in README
• Make sure lanelet2 buildtool_export_depends on mrt_cmake_modules
• Add changelogs
• Fix clang-tidy warnings
• Contributors: Fabian Poggenhans

1.0.0 (2020-03-03)

• Bump version to 1.0
• Routing: Add shortest path search based on areas
• Fix default values for lane changes in RoutingGraph
• RoutingGraph and Route now use the new LaneletSubmap to store the lanelets they are using their member functions laneletMap() and passableMap() are now deprecated and should be replaced by laneletSubmap() and passableSubmap() respectively. These functions have less overhead and are less likely to be misinterpreted as \'maps containing only the lanelets you need\'
• Edges with nonfinite costs are no longer added to the graph to avoid overflows. If a cost function returns infinite costs, no edge will be added and the connection will not be available to the routing graph
• Introduce proper namespacing for internal objects
• Update documentation
• Routing graph and route object now support queries with a custom search function Routing graph and route object now both have a function forEachSuccessor (and more) for doing more advanced queries. Added doc tests and python bindings
• Update the shortest path algorithm to use the new dijktra search
• Extended and simplified the reachablePath/Set functions by using boost graphs implementation of dijkstras shortest paths
• Refactored the internal representation of the route. Cleaned up headers that are only supposed to be used internally
• Complete rewrite of the route builder using boost graph
• Removed the \"diverging\" and \"merging\" classification from the routing graph and update the doc They are now all represented with the \"succeeding\" relation. Information about merging or diverging can now be obtained simply by querying the number of following/preceding lanelets. As a consequence, the route object no longer caches queried \"lanes\". The responsible functions are now const.
• Fix compiler errors with gcc 5
• Fix image paths in routing doc
• Initial commit
• Contributors: Fabian Poggenhans, Johannes Janosovits, Maximilian Naumann