Program Listing for File RoutingGraphVisualization.h

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

#pragma once

#include <boost/graph/graphml.hpp>
#include <boost/graph/graphviz.hpp>
#include <iostream>

#include "lanelet2_routing/Exceptions.h"
#include "lanelet2_routing/Forward.h"
#include "lanelet2_routing/internal/Graph.h"

namespace lanelet {

// This one needs to be in this namepace. Otherwise it's not found.
inline std::istream& operator>>(std::istream& is, ConstLaneletOrArea& /*r*/) { return is; }

namespace routing {
inline std::ostream& operator<<(std::ostream& os, const RelationType& r) { return os << relationToString(r); }
inline std::istream& operator>>(std::istream& is, const RelationType& /*r*/) { return is; }

namespace internal {

template <class Graph>
class VertexWriterGraphViz {
 public:
  explicit VertexWriterGraphViz(const Graph* g) : graph_(g) {}
  template <class VertexOrEdge>
  void operator()(std::ostream& out, const VertexOrEdge& v) const {
    const Id id{(*graph_)[v].laneletOrArea.id()};
    out << "[label=\"" << id << "\" lanelet=\"" << id << "\"]";
  }

 private:
  const Graph* graph_;
};

template <class Graph>
class EdgeWriterGraphViz {
 public:
  explicit EdgeWriterGraphViz(const Graph* g) : graph_(g) {}
  template <class VertexOrEdge>
  void operator()(std::ostream& out, const VertexOrEdge& v) const {
    const RelationType relation{(*graph_)[v].relation};
    out << "[label=\"" << relationToString(relation) << "\" color=\"" << relationToColor(relation);
    if (relation != RelationType::AdjacentLeft && relation != RelationType::AdjacentRight &&
        relation != RelationType::Conflicting) {
      out << "\" weight=\"" << (*graph_)[v].routingCost;
    }
    out << "\" routingCostId=\"" << (*graph_)[v].costId << "\"]";
  }

 private:
  const Graph* graph_;
};

template <typename G, typename E = boost::keep_all, typename V = boost::keep_all>
inline void exportGraphVizImpl(const std::string& filename, const G& g, E edgeFilter = boost::keep_all(),
                               V vertexFilter = boost::keep_all()) {
  std::ofstream file;
  file.open(filename);
  if (!file.is_open()) {
    throw lanelet::ExportError("Could not open file at " + filename + ".");
  }

  VertexWriterGraphViz<G> vertexWriter(&g);
  EdgeWriterGraphViz<G> edgeWriter(&g);
  boost::filtered_graph<G, E, V> fg(g, edgeFilter, vertexFilter);
  boost::write_graphviz(file, fg, vertexWriter, edgeWriter);

  file.close();
}

template <typename G>
inline void exportGraphVizImpl(const std::string& filename, const G& g, const RelationType& relationTypes,
                               RoutingCostId routingCostId = 0) {
  auto edgeFilter = EdgeCostFilter<G>(g, routingCostId, relationTypes);
  exportGraphVizImpl(filename, g, edgeFilter);
}

template <typename G, typename E = boost::keep_all, typename V = boost::keep_all>
inline void exportGraphMLImpl(const std::string& filename, const G& g, E eFilter = boost::keep_all(),
                              V vFilter = boost::keep_all()) {
  std::ofstream file;
  file.open(filename);
  if (!file.is_open()) {
    throw lanelet::ExportError("Could not open file at " + filename + ".");
  }

  boost::filtered_graph<G, EdgeCostFilter<G>> filteredGraph(g, eFilter, vFilter);

  auto pmId = boost::get(&VertexInfo::laneletOrArea, filteredGraph);  // NOLINT
  auto pmRelation = boost::get(&EdgeInfo::relation, filteredGraph);
  auto pmRoutingCosts = boost::get(&EdgeInfo::routingCost, filteredGraph);

  boost::dynamic_properties dp;
  dp.property("info", pmId);
  dp.property("relation", pmRelation);
  dp.property("routingCost", pmRoutingCosts);

  boost::write_graphml(file, filteredGraph, dp, false);
}

template <typename G>
inline void exportGraphMLImpl(const std::string& filename, const G& g, const RelationType& relationTypes,
                              RoutingCostId routingCostId = 0) {
  auto edgeFilter = EdgeCostFilter<G>(g, routingCostId, relationTypes);
  exportGraphMLImpl(filename, g, edgeFilter);
}
}  // namespace internal
}  // namespace routing
}  // namespace lanelet