From 642a36d75e06cdb898ef18dea72822a4d122a6eb Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 8 Nov 2023 11:38:41 +0100 Subject: [PATCH] Fixed ending lane control bug --- LibCarla/source/carla/trafficmanager/Constants.h | 1 + LibCarla/source/carla/trafficmanager/InMemoryMap.cpp | 12 ++++++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/Constants.h b/LibCarla/source/carla/trafficmanager/Constants.h index f8052e363..0ed288a0a 100644 --- a/LibCarla/source/carla/trafficmanager/Constants.h +++ b/LibCarla/source/carla/trafficmanager/Constants.h @@ -105,6 +105,7 @@ static const float MAX_WPT_RADIANS = 0.087f; // 5ยบ static float const DELTA = 25.0f; static float const Z_DELTA = 500.0f; static float const STRAIGHT_DEG = 19.0f; +static const double MIN_LANE_WIDTH = 1.0f; } // namespace Map namespace TrafficLight { diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index e7fd3312b..a4bc9bea8 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -180,6 +180,11 @@ namespace traffic_manager { // Setting segment predecessors and successors. SegmentId waypoint_segment_id = GetSegmentId(connection.first); SegmentId successor_segment_id = GetSegmentId(connection.second); + if (waypoint_segment_id == successor_segment_id){ + // If both topology waypoints are at the same segment, ignore them. + // This happens at lanes that have either no successor or predecessor connections. + continue; + } using SegIdVectorPair = std::pair, std::vector>; SegIdVectorPair &connection_first = segment_topology[waypoint_segment_id]; SegIdVectorPair &connection_second = segment_topology[successor_segment_id]; @@ -226,7 +231,10 @@ namespace traffic_manager { assert(_world_map != nullptr && "No map reference found."); auto raw_dense_topology = _world_map->GenerateWaypoints(MAP_RESOLUTION); for (auto &waypoint_ptr: raw_dense_topology) { - segment_map[GetSegmentId(waypoint_ptr)].emplace_back(std::make_shared(waypoint_ptr)); + if (waypoint_ptr->GetLaneWidth() > MIN_LANE_WIDTH){ + // Avoid making the vehicles move through very narrow lanes + segment_map[GetSegmentId(waypoint_ptr)].emplace_back(std::make_shared(waypoint_ptr)); + } } // 3. Processing waypoints. @@ -347,7 +355,7 @@ namespace traffic_manager { if (neighbour) { swp->SetNextWaypoint(neighbour->GetNextWaypoint()); for (auto next_waypoint : neighbour->GetNextWaypoint()) { - next_waypoint->SetPreviousWaypoint({swp}); + next_waypoint->SetPreviousWaypoint({swp}); } } }