example.cpp

/**
 * Copyright 2024 Institute of Automotive Engineering (FZD), Technical University of Darmstadt
 * SPDX-License-Identifier: MIT
 */

#include <lanelet2_io/Io.h>
#include <lanelet2_routing/Route.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_traffic_rules/TrafficRulesFactory.h>

#include <iostream>

#include "ExampleFileLoader.h"
#include "MyOwnCost.h"
#include "capbasedrouting/logging/Logger.h"
#include "capbasedrouting/matching/MatchingCriteria.h"

int main() {
    using namespace capbasedrouting;
    using namespace capbasedrouting::logging;
    using namespace lanelet;
    using namespace lanelet::traffic_rules;
    using namespace lanelet::routing;

    // Define the path of the map file
    auto const file = "../example/maps/DA_City_network_Lanelet2_BSSD.osm";

    /**
     * Load BSSD with the extended example file loader which patches BSSD errors on the fly
     * If your map is well-defined you can use the default FileLoader class
     */
    ExampleFileLoader loader;
    loader.load(file);

    // Load the proofs
    std::list<matching::MatchingCriteriaProof> proofs = {
        matching::load("../example/proofs/DC_reserv_1.json"),
        matching::load("../example/proofs/DC_reserv_2.json"),
        matching::load("../example/proofs/DC_reserv_3.json"),
        matching::load("../example/proofs/DC_reserv_4.json"),
    };

    // Load the lanelet map
    LaneletMapPtr const map = load(file, Origin({49, 8}));

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

    /**
     * Initialize the example cost function which overrides the transition event.
     * If you do not want to use any event, you can use the default MatchingCost class
     */
    auto cost = std::make_shared<MyOwnCost>(10.0, 0.0, map, loader, proofs, LogLevel::NONE);

    /**
     * If the map contains lanelets with no associated behavior space,
     * we can set a default BSSD cost for traversing them.
     * Here, we set the default BSSD cost to 0 which results in the
     * conventional distance-based routing cost for these lanelets.
     */
    cost->setDefaultCost(0.0);
    RoutingCostPtrs const costPtrs{cost};

    // Build the routing graph
    RoutingGraphPtr const graph = RoutingGraph::build(*map, *trafficRules, costPtrs);

    // Write the graph to a file (e.g. for debugging)
    LaneletMapConstPtr debugLaneletMap = graph->getDebugLaneletMap(RoutingCostId(0));
    write(std::string("routing_graph.osm"), *debugLaneletMap, Origin({49, 8}));

    // Define the start and end lanelets for which we want to find a route
    auto const from = map.get()->laneletLayer.get(7021);
    auto const to   = map.get()->laneletLayer.get(4544);

    // Calculate the route
    Optional<Route> route = graph->getRoute(from, to, 0);

    // Check if a route was found. If not, print a message and return.
    if (!route) {
        std::cout << "No route found!" << std::endl;
        return 0;
    }

    // Write the route to a file
    LaneletSubmapConstPtr const routeMap = route->laneletSubmap();
    write("route.osm", *routeMap->laneletMap(), Origin({49, 8}));
}