Improved GetNearestLane function

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

@@ -202,7 +202,7 @@ namespace element {
     ///   @param dist distance from the begining of the road to the point you
     ///          want to calculate the distance
     ///   @param loc point to calculate the distance
-    int GetNearestLane(double dist, const geom::Location &loc) const {
+    std::pair<int, double> GetNearestLane(double dist, const geom::Location &loc) const {
       const DirectedPoint dp_center_road = GetDirectedPointIn(dist);
       auto info = GetInfo<RoadInfoLane>(0.0);
 
@@ -224,7 +224,7 @@ namespace element {
           }
         }
       }
-      return nearest_lane_id;
+      return std::pair<int, double>(nearest_lane_id, nearest_dist);
     }
 
     const double &GetLength() const {

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

@@ -5,10 +5,11 @@
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
 #include "carla/road/element/Waypoint.h"
-
 #include "carla/Logging.h"
 #include "carla/road/Map.h"
 
+#include <algorithm>
+
 namespace carla {
 namespace road {
 namespace element {
@@ -42,18 +43,61 @@ namespace element {
   Waypoint::Waypoint(SharedPtr<const Map> m, const geom::Location &loc)
     : _map(m) {
     DEBUG_ASSERT(_map != nullptr);
-    double nearest_dist = std::numeric_limits<double>::max();
+
+    // max_nearests represents the max nearests roads
+    // where we will search for nearests lanes
+    constexpr int max_nearests = 10;
+    // in case that map has less than max_nearests lanes,
+    // we will use the maximum lanes
+    const int max_nearest_allowed = _map->GetData()._elements.size()  <
+        max_nearests ? _map->GetData()._elements.size() : max_nearests;
+
+    double nearest_dist[max_nearests];
+    std::fill(nearest_dist, nearest_dist + max_nearest_allowed,
+        std::numeric_limits<double>::max());
+
+    id_type ids[max_nearests];
+    std::fill(ids, ids + max_nearest_allowed, 0);
+
+    double dists[max_nearests];
+    std::fill(dists, dists + max_nearest_allowed, 0.0);
+
+
     for (auto &&r : _map->GetData()._elements) {
       auto current_dist = r.second->GetNearestPoint(loc);
-      if (current_dist.second < nearest_dist) {
-        nearest_dist = current_dist.second;
-        _road_id = r.first;
-        _dist = current_dist.first;
+
+      // search for nearests points
+      for (int i = 0; i < max_nearest_allowed; ++i) {
+        if (current_dist.second < nearest_dist[i]) {
+          // reorder nearest_dist
+          for (int j = max_nearest_allowed - 1; j > i; --j) {
+            nearest_dist[j] = nearest_dist[j - 1];
+            ids[j] = ids[j - 1];
+            dists[j] = dists[j - 1];
+          }
+          nearest_dist[i] = current_dist.second;
+          ids[i] = r.first;
+          dists[i] = current_dist.first;
+
+          break;
+        }
       }
     }
+
+    // search for the nearest lane in nearest_dist
+    auto nearest_lane_dist = std::numeric_limits<double>::max();
+    for (int i = 0; i < max_nearest_allowed; ++i) {
+      auto lane_dist = _map->GetData().GetRoad(ids[i])->GetNearestLane(dists[i], loc);
+
+      if (lane_dist.second < nearest_lane_dist) {
+        nearest_lane_dist = lane_dist.second;
+        _lane_id = lane_dist.first;
+        _road_id = ids[i];
+        _dist = dists[i];
+      }
+    }
+
     DEBUG_ASSERT(_dist <= _map->GetData().GetRoad(_road_id)->GetLength());
-    const auto nearest_lane = _map->GetData().GetRoad(_road_id)->GetNearestLane(_dist, loc);
-    _lane_id = nearest_lane;
   }
 
   Waypoint::Waypoint(
@@ -73,6 +117,7 @@ namespace element {
   geom::Transform Waypoint::GetTransform() const {
     road::element::DirectedPoint dp =
         _map->GetData().GetRoad(_road_id)->GetDirectedPointIn(_dist);
+
     geom::Rotation rot(0.0, geom::Math::to_degrees(dp.tangent), 0.0);
     if (_lane_id > 0) {
       rot.yaw += 180.0;
@@ -113,7 +158,8 @@ namespace element {
     }
 
     std::vector<Waypoint> result;
-    const auto &next_lanes = _lane_id < 0 ? road_segment.GetNextLane(_lane_id) : road_segment.GetPrevLane(_lane_id);
+    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);
     }