Add WaypointGenerator class

LibCarla/source/carla/client/Waypoint.cpp

@@ -7,6 +7,7 @@
 #include "carla/client/Waypoint.h"
 
 #include "carla/client/Map.h"
+#include "carla/road/WaypointGenerator.h"
 
 namespace carla {
 namespace client {
@@ -18,7 +19,7 @@ namespace client {
   Waypoint::~Waypoint() = default;
 
   std::vector<SharedPtr<Waypoint>> Waypoint::Next(double distance) const {
-    auto waypoints = _waypoint.Next(distance);
+    auto waypoints = road::WaypointGenerator::GetNext(_waypoint, distance);
     std::vector<SharedPtr<Waypoint>> result;
     result.reserve(waypoints.size());
     for (auto &waypoint : waypoints) {

LibCarla/source/carla/road/WaypointGenerator.cpp

@@ -0,0 +1,147 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "carla/road/WaypointGenerator.h"
+
+#include "carla/road/Map.h"
+
+namespace carla {
+namespace road {
+
+  using namespace carla::road::element;
+
+  // ===========================================================================
+  // -- Static local methods ---------------------------------------------------
+  // ===========================================================================
+
+  template <typename T>
+  static std::vector<T> ConcatVectors(std::vector<T> dst, std::vector<T> src) {
+    if (src.size() > dst.size()) {
+      return ConcatVectors(src, dst);
+    }
+    dst.insert(
+        dst.end(),
+        std::make_move_iterator(src.begin()),
+        std::make_move_iterator(src.end()));
+    return dst;
+  }
+
+  template <typename FuncT>
+  static void ForEachDrivableLane(const RoadSegment &road, double s, FuncT &&func) {
+    const auto *info = road.GetInfo<RoadInfoLane>(s);
+    DEBUG_ASSERT(info != nullptr);
+    for (auto &&lane_id : info->getLanesIDs(RoadInfoLane::which_lane_e::Both)) {
+      if (info->getLane(lane_id)->_type == "driving") {
+        func(lane_id);
+      }
+    }
+  }
+
+  // ===========================================================================
+  // -- WaypointGenerator ------------------------------------------------------
+  // ===========================================================================
+
+  std::vector<Waypoint> WaypointGenerator::GetSuccessors(const Waypoint &waypoint) {
+    auto &map = waypoint._map;
+    const auto this_lane_id = waypoint.GetLaneId();
+    const auto this_road_id = waypoint.GetRoadId();
+    const auto *this_road = map->GetData().GetRoad(this_road_id);
+    DEBUG_ASSERT(this_road != nullptr);
+
+    const auto &next_lanes =
+        this_lane_id < 0 ?
+            this_road->GetNextLane(this_lane_id) :
+            this_road->GetPrevLane(this_lane_id);
+
+    if (next_lanes.empty()) {
+      log_error("lane id =", this_lane_id, " road id=", this_road_id, ": missing next lanes");
+    }
+
+    std::vector<Waypoint> result;
+    result.reserve(next_lanes.size());
+    for (auto &&pair : next_lanes) {
+      const auto lane_id = pair.first;
+      const auto road_id = pair.second;
+      const auto *road = map->GetData().GetRoad(road_id);
+      DEBUG_ASSERT(lane_id != 0);
+      DEBUG_ASSERT(road != nullptr);
+      const auto distance = lane_id < 0 ? 0.0 : road->GetLength();
+      result.push_back(Waypoint(map, road_id, lane_id, distance));
+    }
+    return result;
+  }
+
+  std::vector<Waypoint> WaypointGenerator::GetNext(
+      const Waypoint &waypoint,
+      double distance) {
+    auto &map = waypoint._map;
+    const auto this_lane_id = waypoint.GetLaneId();
+    const auto this_road_id = waypoint.GetRoadId();
+    const auto *this_road = map->GetData().GetRoad(this_road_id);
+    DEBUG_ASSERT(this_road != nullptr);
+
+    double distance_on_next_segment;
+
+    if (this_lane_id < 0) {
+      // road goes forward.
+      const auto total_distance = waypoint._dist + distance;
+      const auto road_length = this_road->GetLength();
+      if (total_distance <= road_length) {
+        return { Waypoint(map, this_road_id, this_lane_id, total_distance) };
+      }
+      distance_on_next_segment = total_distance - road_length;
+    } else {
+      // road goes backward.
+      const auto total_distance = waypoint._dist - distance;
+      if (total_distance >= 0.0) {
+        return { Waypoint(map, this_road_id, this_lane_id, total_distance) };
+      }
+      distance_on_next_segment = std::abs(total_distance);
+    }
+
+    std::vector<Waypoint> result;
+    for (auto &&next_waypoint : GetSuccessors(waypoint)) {
+      result = ConcatVectors(result, GetNext(next_waypoint, distance_on_next_segment));
+    }
+    return result;
+  }
+
+  std::vector<Waypoint> WaypointGenerator::GenerateAll(
+      const Map &map,
+      const double distance) {
+    std::vector<Waypoint> result;
+    for (auto &&road_segment : map.GetData().GetRoadSegments()) {
+      /// @todo Should distribute them equally along the segment?
+      for (double s = 0.0; s < road_segment.GetLength(); s += distance) {
+        ForEachDrivableLane(road_segment, s, [&](auto lane_id) {
+          result.push_back(Waypoint(map.shared_from_this(), road_segment.GetId(), lane_id, s));
+        });
+      }
+    }
+    return result;
+  }
+
+  std::vector<std::pair<Waypoint, Waypoint>> WaypointGenerator::GenerateTopology(
+      const Map &map) {
+    std::vector<std::pair<Waypoint, Waypoint>> result;
+    for (auto &&road_segment : map.GetData().GetRoadSegments()) {
+      ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) {
+        auto distance = lane_id < 0 ? 0.0 : road_segment.GetLength();
+        auto this_waypoint = Waypoint(
+            map.shared_from_this(),
+            road_segment.GetId(),
+            lane_id,
+            distance);
+        for (auto &&successor : GetSuccessors(this_waypoint)) {
+          result.push_back({this_waypoint, successor});
+        }
+      });
+    }
+    return result;
+  }
+
+} // namespace road
+} // namespace carla

LibCarla/source/carla/road/WaypointGenerator.h

@@ -0,0 +1,48 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/road/element/Waypoint.h"
+
+#include <utility>
+#include <vector>
+
+namespace carla {
+namespace road {
+
+  class Map;
+
+  class WaypointGenerator {
+  public:
+
+    using Waypoint = element::Waypoint;
+
+    /// Return the list of waypoints placed at the entrance of each drivable
+    /// successor lane; i.e., the list of each waypoint in the next road segment
+    /// that a vehicle could drive from @a waypoint.
+    static std::vector<Waypoint> GetSuccessors(
+        const Waypoint &waypoint);
+
+    /// Return the list of waypoints at @a distance such that a vehicle at @a
+    /// waypoint could drive to.
+    static std::vector<Waypoint> GetNext(
+        const Waypoint &waypoint,
+        double distance);
+
+    /// Generate all the waypoints in @a map separated by @a approx_distance.
+    static std::vector<Waypoint> GenerateAll(
+        const Map &map,
+        double approx_distance);
+
+    /// Generate the minimum set of waypoints that define the topology of @a
+    /// map. The waypoints are placed at the entrance of each lane.
+    static std::vector<std::pair<Waypoint, Waypoint>> GenerateTopology(
+        const Map &map);
+  };
+
+} // namespace road
+} // namespace carla

LibCarla/source/carla/road/element/Waypoint.cpp

@@ -14,32 +14,6 @@ namespace carla {
 namespace road {
 namespace element {
 
-  // ===========================================================================
-  // -- Static local methods ---------------------------------------------------
-  // ===========================================================================
-
-  static const RoadSegment &GetRoad(const Map &map, id_type road_id) {
-    auto *road = map.GetData().GetRoad(road_id);
-    DEBUG_ASSERT(road != nullptr);
-    return *road;
-  }
-
-  template <typename T>
-  static std::vector<T> ConcatVectors(std::vector<T> dst, std::vector<T> src) {
-    if (src.size() > dst.size()) {
-      return ConcatVectors(src, dst);
-    }
-    dst.insert(
-        dst.end(),
-        std::make_move_iterator(src.begin()),
-        std::make_move_iterator(src.end()));
-    return dst;
-  }
-
-  // ===========================================================================
-  // -- Waypoint ---------------------------------------------------------------
-  // ===========================================================================
-
   Waypoint::Waypoint(SharedPtr<const Map> m, const geom::Location &loc)
     : _map(m) {
     DEBUG_ASSERT(_map != nullptr);
@@ -97,6 +71,7 @@ namespace element {
     }
 
     DEBUG_ASSERT(_dist <= _map->GetData().GetRoad(_road_id)->GetLength());
+    DEBUG_ASSERT(_lane_id != 0);
   }
 
   Waypoint::Waypoint(
@@ -109,6 +84,7 @@ namespace element {
       _lane_id(lane_id),
       _dist(distance) {
     DEBUG_ASSERT(_map != nullptr);
+    DEBUG_ASSERT(_lane_id != 0);
   }
 
   Waypoint::~Waypoint() = default;
@@ -122,7 +98,9 @@ namespace element {
       rot.yaw += 180.0;
     }
 
-    auto *info = GetRoad(*_map, _road_id).GetInfo<RoadInfoLane>(0.0);
+    const auto *road_segment = _map->GetData().GetRoad(_road_id);
+    DEBUG_ASSERT(road_segment != nullptr);
+    const auto *info = road_segment->GetInfo<RoadInfoLane>(0.0);
     DEBUG_ASSERT(info != nullptr);
 
     dp.ApplyLateralOffset(info->getLane(_lane_id)->_lane_center_offset);
@@ -133,46 +111,6 @@ namespace element {
     return RoadInfoList(_map->GetData().GetRoad(_road_id)->GetInfos(_dist));
   }
 
-  std::vector<Waypoint> Waypoint::Next(const double distance) const {
-    DEBUG_ASSERT(_lane_id != 0);
-
-    const auto &road_segment = GetRoad(*_map, _road_id);
-    double distance_on_next_segment;
-
-    if (_lane_id < 0) {
-      // road goes forward.
-      const auto total_distance = _dist + distance;
-      const auto road_length = road_segment.GetLength();
-      if (total_distance <= road_length) {
-        return { Waypoint(_map, _road_id, _lane_id, total_distance) };
-      }
-      distance_on_next_segment = total_distance - road_length;
-    } else {
-      // road goes backward.
-      const auto total_distance = _dist - distance;
-      if (total_distance >= 0.0) {
-        return { Waypoint(_map, _road_id, _lane_id, total_distance) };
-      }
-      distance_on_next_segment = std::abs(total_distance);
-    }
-
-    std::vector<Waypoint> result;
-    const auto &next_lanes = _lane_id < 0 ? road_segment.GetNextLane(_lane_id) : road_segment.GetPrevLane(
-        _lane_id);
-    if (next_lanes.empty()) {
-      log_error("no lanes!! lane id =", _lane_id, " road id=", _road_id);
-    }
-    for (auto &&pair : next_lanes) {
-      auto lane_id = pair.first;
-      auto road_id = pair.second;
-      DEBUG_ASSERT(lane_id != 0);
-      auto d = lane_id < 0 ? 0.0 : GetRoad(*_map, road_id).GetLength();
-      auto waypoint = Waypoint(_map, road_id, lane_id, d);
-      result = ConcatVectors(result, waypoint.Next(distance_on_next_segment));
-    }
-    return result;
-  }
-
 } // namespace element
 } // namespace road
 } // namespace carla

LibCarla/source/carla/road/element/Waypoint.h

@@ -15,6 +15,7 @@ namespace carla {
 namespace road {
 
   class Map;
+  class WaypointGenerator;
 
 namespace element {
 
@@ -33,13 +34,12 @@ namespace element {
       return _lane_id;
     }
 
-    std::vector<Waypoint> Next(double distance) const;
-
     RoadInfoList GetRoadInfo() const;
 
   private:
 
     friend carla::road::Map;
+    friend carla::road::WaypointGenerator;
 
     Waypoint(SharedPtr<const Map>, const geom::Location &location);
 
@@ -56,7 +56,6 @@ namespace element {
     int _lane_id = 0;
 
     double _dist = 0.0;
-
   };
 
 } // namespace element