Expose map topology in Python

Docs/python_api.md

@@ -173,6 +173,7 @@
 - `name`
 - `get_spawn_points()`
 - `get_waypoint(location, project_to_road=True)`
+- `get_topology()`
 - `to_opendrive()`
 - `save_to_disk(path=self.name)`
 

LibCarla/source/carla/client/Map.cpp

@@ -9,6 +9,7 @@
 #include "carla/client/Waypoint.h"
 #include "carla/opendrive/OpenDrive.h"
 #include "carla/road/Map.h"
+#include "carla/road/WaypointGenerator.h"
 
 #include <sstream>
 
@@ -41,6 +42,33 @@ namespace client {
         nullptr;
   }
 
+  Map::TopologyList Map::GetTopology() const {
+    DEBUG_ASSERT(_map != nullptr);
+    namespace re = carla::road::element;
+    std::unordered_map<re::id_type, std::unordered_map<int, SharedPtr<Waypoint>>> waypoints;
+
+    auto get_or_make_waypoint = [&](const auto &waypoint) {
+      auto &waypoints_on_road = waypoints[waypoint.GetRoadId()];
+      auto it = waypoints_on_road.find(waypoint.GetLaneId());
+      if (it == waypoints_on_road.end()) {
+        it = waypoints_on_road.emplace(
+            waypoint.GetLaneId(),
+            SharedPtr<Waypoint>(new Waypoint{shared_from_this(), waypoint})).first;
+      }
+      return it->second;
+    };
+
+    TopologyList result;
+    auto topology = road::WaypointGenerator::GenerateTopology(*_map);
+    result.reserve(topology.size());
+    for (const auto &pair : topology) {
+      result.emplace_back(
+          get_or_make_waypoint(pair.first),
+          get_or_make_waypoint(pair.second));
+    }
+    return result;
+  }
+
   std::vector<road::element::LaneMarking> Map::CalculateCrossedLanes(
       const geom::Location &origin,
       const geom::Location &destination) const {

LibCarla/source/carla/client/Map.h

@@ -44,6 +44,10 @@ namespace client {
         const geom::Location &location,
         bool project_to_road = true) const;
 
+    using TopologyList = std::vector<std::pair<SharedPtr<Waypoint>, SharedPtr<Waypoint>>>;
+
+    TopologyList GetTopology() const;
+
     std::vector<road::element::LaneMarking> CalculateCrossedLanes(
         const geom::Location &origin,
         const geom::Location &destination) const;

PythonAPI/source/libcarla/Map.cpp

@@ -39,6 +39,16 @@ static void SaveOpenDriveToDisk(const carla::client::Map &self, std::string path
   out << self.GetOpenDrive() << std::endl;
 }
 
+static auto GetTopology(const carla::client::Map &self) {
+  namespace py = boost::python;
+  auto topology = self.GetTopology();
+  py::list result;
+  for (auto &&pair : topology) {
+    result.append(py::make_tuple(pair.first, pair.second));
+  }
+  return result;
+}
+
 void export_map() {
   using namespace boost::python;
   namespace cc = carla::client;
@@ -58,6 +68,7 @@ void export_map() {
     .add_property("name", CALL_RETURNING_COPY(cc::Map, GetName))
     .def("get_spawn_points", CALL_RETURNING_COPY(cc::Map, GetRecommendedSpawnPoints))
     .def("get_waypoint", &cc::Map::GetWaypoint, (arg("location"), arg("project_to_road")=true))
+    .def("get_topology", &GetTopology)
     .def("to_opendrive", CALL_RETURNING_COPY(cc::Map, GetOpenDrive))
     .def("save_to_disk", &SaveOpenDriveToDisk, (arg("path")=""))
     .def(self_ns::str(self_ns::self))