Fix waypoints next and previous when connecting two lines with opposite directions (#7137)

* Update semantic labels (changed from version 0.9.13 -> 0.9.14)

* Add "Other" Label description in ref_sensors.md

* Revert "Add "Other" Label description in ref_sensors.md"

This reverts commit 3958e35.

* Revert "Update semantic labels (changed from version 0.9.13 -> 0.9.14)"

This reverts commit 6ee572e.

* Fix waypoints next and previous when connecting two lines with opposite
dirrections.

Fixing issue #7136

Probably not the best solution, but works for our case. The waypoint
that would cause conflict when calling next from it is not returned.

* Update CHANGELOG.md

* Update CHANGELOG.md

Co-authored-by: Michal Sojka <michal.sojka@cvut.cz>

---------

Co-authored-by: Gerhard Dorn <67096719+dorn-gerhard@users.noreply.github.com>
Co-authored-by: Michal Sojka <michal.sojka@cvut.cz>

Author: 3 people

CHANGELOG.md

@@ -1,4 +1,5 @@
 ## Latest Changes
+ * Fixed waypoint.next and .previous causing loops when two opposite-direction lanes follow each other in the map.
  * Fixed a bug that caused navigation information not to be loaded when switching maps
  * Prevent from segfault on failing SignalReference identification when loading OpenDrive files
  * Added vehicle doors to the recorder

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);