Fix waypoints next and previous when connecting two lines with opposite directions

CHANGELOG.md

@@ -1,3 +1,5 @@
+  * Fixed waypoint.next and .previous causing loops when two opposite-direction lanes follow each other in the map.
+
 ## CARLA 0.9.15
 
   * Added Digital Twins feature version 0.1. Now you can create your own map based on OpenStreetMaps

LibCarla/source/carla/road/Map.cpp

@@ -582,10 +582,23 @@ namespace road {
           successor.road_id != waypoint.road_id ||
           successor.section_id != waypoint.section_id ||
           successor.lane_id != waypoint.lane_id);
-      result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
-    }
+      // Fix situations, when next waypoint is in the opposite dirrection and
+      // this waypoint is his successor, so this function would end up in a loop
+      bool is_broken = false;
+      for (const auto &future_succcessor : GetSuccessors(successor)) {
+          if (future_succcessor.road_id == waypoint.road_id 
+               && future_succcessor.lane_id == waypoint.lane_id
+               && future_succcessor.section_id == waypoint.section_id){
+            is_broken = true;
+            break;
+            }
+      }  // end inner for
+      if (!is_broken){
+        result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
+      }
+    }  // end outer for
     return result;
-  }
+  }  // end GetNext
 
   std::vector<Waypoint> Map::GetPrevious(
       const Waypoint waypoint,
@@ -618,10 +631,23 @@ namespace road {
           successor.road_id != waypoint.road_id ||
           successor.section_id != waypoint.section_id ||
           successor.lane_id != waypoint.lane_id);
-      result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
-    }
+      // Fix situations, when next waypoint is in the opposite dirrection and
+      // this waypoint is his predeccessor, so this function would end up in a loop
+      bool is_broken = false;
+      for (const auto &future_predecessor : GetPredecessors(successor)) {
+          if (future_predecessor.road_id == waypoint.road_id 
+               && future_predecessor.lane_id == waypoint.lane_id
+               && future_predecessor.section_id == waypoint.section_id){
+            is_broken = true;
+            break;
+            }
+      }  // end inner for
+      if (!is_broken){
+        result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
+      }
+    }  // end outer for
     return result;
-  }
+  }  // end GetPrevious
 
   boost::optional<Waypoint> Map::GetRight(Waypoint waypoint) const {
     RELEASE_ASSERT(waypoint.lane_id != 0);