Program Listing for File RoutingGraphContainer.h

↰ Return to documentation for file (include/lanelet2_routing/RoutingGraphContainer.h)

#pragma once

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/Lanelet.h>

#include <algorithm>
#include <unordered_set>

#include "lanelet2_routing/Route.h"
#include "lanelet2_routing/RoutingGraph.h"

namespace lanelet {
namespace routing {

class RoutingGraphContainer {
 public:
  using ConflictingInGraph = std::pair<size_t, ConstLanelets>;
  using ConflictingInGraphs = std::vector<ConflictingInGraph>;

  explicit RoutingGraphContainer(std::vector<RoutingGraphConstPtr> routingGraphs) : graphs_{std::move(routingGraphs)} {}

  explicit RoutingGraphContainer(const std::vector<RoutingGraphPtr>& routingGraphs)
      : graphs_{utils::transform(routingGraphs, [](auto& g) { return RoutingGraphConstPtr(g); })} {}

  ConstLanelets conflictingInGraph(const ConstLanelet& lanelet, size_t routingGraphId,
                                   double participantHeight = .0) const {
    if (routingGraphId >= graphs_.size()) {
      throw InvalidInputError("Routing Graph ID is higher than the number of graphs.");
    }
    auto overlaps = [lanelet, participantHeight](const ConstLanelet& ll) {
      return participantHeight != .0 ? !geometry::overlaps3d(lanelet, ll, participantHeight)
                                     : !geometry::overlaps2d(lanelet, ll);
    };
    const auto map{graphs_[routingGraphId]->passableSubmap()};
    ConstLanelets conflicting{map->laneletLayer.search(geometry::boundingBox2d(lanelet))};
    auto begin = conflicting.begin();
    auto end = conflicting.end();
    end = std::remove(begin, end, lanelet);
    end = std::remove_if(begin, end, overlaps);
    conflicting.erase(end, conflicting.end());
    return conflicting;
  }

  ConflictingInGraphs conflictingInGraphs(const ConstLanelet& lanelet, double participantHeight = .0) const {
    ConflictingInGraphs result;
    for (size_t it = 0; it < graphs_.size(); it++) {
      result.emplace_back(std::make_pair(it, conflictingInGraph(lanelet, it, participantHeight)));
    }
    return result;
  }

  ConstLanelets conflictingOfRouteInGraph(const Route* route, size_t routingGraphId,
                                          double participantHeight = .0) const {
    std::unordered_set<ConstLanelet> conflicting;
    for (const auto& it : route->laneletSubmap()->laneletLayer) {
      ConstLanelets tempConf{conflictingInGraph(it, routingGraphId, participantHeight)};
      conflicting.insert(tempConf.begin(), tempConf.end());
    }
    return ConstLanelets(conflicting.begin(), conflicting.end());
  }

  ConflictingInGraphs conflictingOfRouteInGraphs(const Route* route, double participantHeight = .0) const {
    ConflictingInGraphs result;
    for (size_t it = 0; it < graphs_.size(); it++) {
      result.emplace_back(std::make_pair(it, conflictingOfRouteInGraph(route, it, participantHeight)));
    }
    return result;
  }

  const std::vector<RoutingGraphConstPtr>& routingGraphs() const { return graphs_; }

 private:
  std::vector<RoutingGraphConstPtr> graphs_;
};

}  // namespace routing
}  // namespace lanelet