From e228aa0fff033aad1a7c6d1f7827f0cbbb1497d4 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 31 May 2022 08:59:58 +0200 Subject: [PATCH] Added map key check --- .../trafficmanager/TrafficLightStage.cpp | 88 ++++++++++--------- 1 file changed, 48 insertions(+), 40 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index 51049caae..3f677febf 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -53,22 +53,24 @@ void TrafficLightStage::Update(const unsigned long index) { } // The vehicle is currently at a non signalized junction, so handle its priorities. // Don't use the next condition as bounding boxes might switch to green - else if (vehicle_last_junction.find(ego_actor_id) != vehicle_last_junction.end()){ - + else if (vehicle_last_junction.find(ego_actor_id) != vehicle_last_junction.end()) + { if (!look_ahead_point->CheckJunction()) { // Very close to the junction exit, forget about it. RemoveActor(ego_actor_id); } else { auto junction_id = junction->GetId(); - auto& exiting_vehicles = exiting_vehicles_map.at(junction_id); - if (std::find(exiting_vehicles.begin(), exiting_vehicles.end(), ego_actor_id) != exiting_vehicles.end()) { - // The vehicle is exitting the junction. - traffic_light_hazard = false; - } - else { - // Vehicle entering the junction, handle it - traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction, waypoint_buffer, current_timestamp); + if (exiting_vehicles_map.find(junction_id) != exiting_vehicles_map.end()){ + auto& exiting_vehicles = exiting_vehicles_map.at(junction_id); + if (std::find(exiting_vehicles.begin(), exiting_vehicles.end(), ego_actor_id) != exiting_vehicles.end()) { + // The vehicle is exitting the junction. + traffic_light_hazard = false; + } + else { + // Vehicle entering the junction, handle it + traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction, waypoint_buffer, current_timestamp); + } } } } @@ -115,42 +117,48 @@ bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, bool traffic_light_hazard = false; auto junction_id = junction->GetId(); - auto& entering_vehicles = entering_vehicles_map.at(junction_id); - auto& exiting_vehicles = exiting_vehicles_map.at(junction_id); + if (entering_vehicles_map.find(junction_id) == entering_vehicles_map.end()) + { + auto& entering_vehicles = entering_vehicles_map.at(junction_id); - if (vehicle_stop_time.find(ego_actor_id) == vehicle_stop_time.end()) { - // Ensure the vehicle stops before doing anything else - if (simulation_state.GetVelocity(ego_actor_id).Length() < EPSILON_RELATIVE_SPEED) { - vehicle_stop_time.insert({ego_actor_id, timestamp}); - } - traffic_light_hazard = true; - } + if (exiting_vehicles_map.find(junction_id) == exiting_vehicles_map.end()) + { + auto& exiting_vehicles = exiting_vehicles_map.at(junction_id); - else if (entering_vehicles.front() == ego_actor_id) { - auto entry_elapsed_seconds = vehicle_stop_time.at(ego_actor_id).elapsed_seconds; - if (timestamp.elapsed_seconds - entry_elapsed_seconds < MINIMUM_STOP_TIME) { - // Wait at least the minimum amount of time before entering the junction - traffic_light_hazard = true; - } - else { - // Track the first actor until it has passed the mid-point - cg::Transform actor_transform = waypoint_buffer.front()->GetTransform(); - cg::Vector3D forward_vec = actor_transform.GetForwardVector(); - cg::Vector3D to_center_vec = junction->GetBoundingBox().location - actor_transform.location; + if (vehicle_stop_time.find(ego_actor_id) == vehicle_stop_time.end()) { + // Ensure the vehicle stops before doing anything else + if (simulation_state.GetVelocity(ego_actor_id).Length() < EPSILON_RELATIVE_SPEED) { + vehicle_stop_time.insert({ego_actor_id, timestamp}); + } + traffic_light_hazard = true; + } - if (cg::Math::Dot(forward_vec, to_center_vec) < EXIT_JUNCTION_THRESHOLD) { - // Remove it from the entry data, letting the next one enter it - entering_vehicles.pop_front(); - vehicle_stop_time.erase(ego_actor_id); - exiting_vehicles.push_back(ego_actor_id); + else if (entering_vehicles.front() == ego_actor_id) { + auto entry_elapsed_seconds = vehicle_stop_time.at(ego_actor_id).elapsed_seconds; + if (timestamp.elapsed_seconds - entry_elapsed_seconds < MINIMUM_STOP_TIME) { + // Wait at least the minimum amount of time before entering the junction + traffic_light_hazard = true; + } + else { + // Track the first actor until it has passed the mid-point + cg::Transform actor_transform = waypoint_buffer.front()->GetTransform(); + cg::Vector3D forward_vec = actor_transform.GetForwardVector(); + cg::Vector3D to_center_vec = junction->GetBoundingBox().location - actor_transform.location; + + if (cg::Math::Dot(forward_vec, to_center_vec) < EXIT_JUNCTION_THRESHOLD) { + // Remove it from the entry data, letting the next one enter it + entering_vehicles.pop_front(); + vehicle_stop_time.erase(ego_actor_id); + exiting_vehicles.push_back(ego_actor_id); + } + } + + } else { + // Only one vehicle can be entering the junction, so stop the rest. + traffic_light_hazard = true; } } - - } else { - // Only one vehicle can be entering the junction, so stop the rest. - traffic_light_hazard = true; } - return traffic_light_hazard; }