Fix topology with ending lanes

CHANGELOG.md

@@ -1,5 +1,6 @@
 ## Latest
 
+  * Fix a bug at `Map.get_topology()`, causing lanes with no successors to not be part of it.
   * Added new ConstantVelocityAgent
   * Added new parameter to the TrafficManager, `set_desired_speed`, to set a vehicle's speed.
   * Added 4 new attributes to all vehicles:

LibCarla/source/carla/road/Map.cpp

@@ -711,8 +711,18 @@ namespace road {
     for (const auto &pair : _data.GetRoads()) {
       const auto &road = pair.second;
       ForEachDrivableLane(road, [&](auto &&waypoint) {
-        for (auto &&successor : GetSuccessors(waypoint)) {
-          result.push_back({waypoint, successor});
+        auto successors = GetSuccessors(waypoint);
+        if (successors.size() == 0){
+          auto distance = static_cast<float>(GetDistanceAtEndOfLane(GetLane(waypoint)));
+          auto last_waypoint = GetWaypoint(waypoint.road_id, waypoint.lane_id, distance);
+          if (last_waypoint.has_value()){
+            result.push_back({waypoint, *last_waypoint});
+          }
+        }
+        else{
+          for (auto &&successor : GetSuccessors(waypoint)) {
+            result.push_back({waypoint, successor});
+          }
         }
       });
     }

PythonAPI/carla/agents/navigation/global_route_planner.py

@@ -110,9 +110,15 @@ class GlobalRoutePlanner(object):
                 w = wp1.next(self._sampling_resolution)[0]
                 while w.transform.location.distance(endloc) > self._sampling_resolution:
                     seg_dict['path'].append(w)
-                    w = w.next(self._sampling_resolution)[0]
+                    next_ws = w.next(self._sampling_resolution)
+                    if len(next_ws) == 0:
+                        break
+                    w = next_ws[0]
             else:
-                seg_dict['path'].append(wp1.next(self._sampling_resolution)[0])
+                next_wps = wp1.next(self._sampling_resolution)
+                if len(next_wps) == 0:
+                    break
+                seg_dict['path'].append(next_wps[0])
             self._topology.append(seg_dict)
 
     def _build_graph(self):