From 46d50df7c503723b6aad47ba14fbd7dc71f94d22 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 20 Apr 2022 16:44:12 +0200 Subject: [PATCH] Fix topology with ending lanes --- CHANGELOG.md | 1 + LibCarla/source/carla/road/Map.cpp | 14 ++++++++++++-- .../agents/navigation/global_route_planner.py | 10 ++++++++-- 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 632991032..ba2ca6b97 100644 --- a/CHANGELOG.md +++ b/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: diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index 5195ba1d8..5fcfabd7c 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/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(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}); + } } }); } diff --git a/PythonAPI/carla/agents/navigation/global_route_planner.py b/PythonAPI/carla/agents/navigation/global_route_planner.py index 56c3b6de2..5649f3e9e 100644 --- a/PythonAPI/carla/agents/navigation/global_route_planner.py +++ b/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):