From 1e98335808ee3695ff8ff975f03f4e77121903e3 Mon Sep 17 00:00:00 2001 From: Praveen Kumar <35625166+pravinblaze@users.noreply.github.com> Date: Fri, 5 Jun 2020 03:32:10 +0530 Subject: [PATCH] Traffic manager 2.0 (#2833) * Traffic Manager 2.0 * WIP: new class structure for TM 2.0 Yet to use the new classes in TrafficManagerLocal * WIP: new class structure integration * Fix for python api build. * Fix for hybrid mode crash. * Fixed incorrect collision cache. Fixed crash upon map change. Minor reformatting. * Fixed collisions with unregistered actors. Fixed collisions among multiple traffic manager instances. Fixed vehicle destruction upon being stuck for too long. * Fixed vehicle wobble on steep roads * Waypoint buffer extension at junction entrance. * WIP: Revamped intersection anticipation. * Implemented waypoint occupancy tracking. * * considering buffer independent waypoints for determining blocked junction exit. * considering longitudinal extreme points for localizing unregistered actors. * Removed debug statements * Intersection anticipation for vehicles moving across path. * Fixed intersection anticipation in Town03, Town04. * Safe interval length check for intersection anticipation. * Distance check from safe interval for blocked exit. * Removed debug statements * * Intersection anticipation exception for roundabout in Town03. * Updated comments. * codacity fix * more codacity fixes * formatting and minor changes * bad indentation * Removed debug statements and updated comments. * fixes unused variable error in make examples * Increased minimum obstacle distance for lane change to avoid partial lane changes. * Removing crude stopping logic at junction entrance for blocked junction case. * Ignoring small intersection segments for intersection anticipation. * Fixed issue in unblocking mechanism due to incorrect clock initialization. * Fixing intersection entrance identification. * Fixed incorrect safe space after junction detection. * Fixed collision negotiation conditions to work well inside intersections and avoid deadlocks in roundabout turns. * Replaced in file constants of InMemoryMap with definitions from Constants.h Updated comments * Added a check to avoid collision considerations in case of traffic light hazard in motion planner. * Re-organised include statements for ALSM.h/.cpp files. Fixed a double initialization precision. * WIP: Refactoring ALSM::Update * Refactored ALSM::Update into shorter functions. * ALSM: Corrected numeric comparision precision and container access optimizations. * Initial review changes * CollisionStage: addressed pr comments. * Constants.h, DataStructures.h: addressed pr review comments. * InMemorMap: review comment addressal. * LocalizationStage: Review comment addressal * New class for random generation instead of rand() * Removed unused code * MotionPlanStage: re-organized include statements. * MotionPlanStage: review comment addressal. * SimulationState, DataStructures: minor refactoring. * SnippetProfiler, TrackTraffic: Review comment addressal. * Refactored include statements for SimpleWaypoint, SnippetProfiler, TrackTraffic files. * TrafficLightStage: review comment addressal. * Using sleep instead of continue to time hybrid mode. * Changing fixed array allocation with dynamic resizing. * Refactored include statements for TrafficManager * Removed clamp macro * Added const to Networking constants Co-authored-by: Jacopo Bartiromo Co-authored-by: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com> --- LibCarla/source/carla/client/Vehicle.h | 2 + LibCarla/source/carla/trafficmanager/ALSM.cpp | 357 +++++ LibCarla/source/carla/trafficmanager/ALSM.h | 106 ++ .../carla/trafficmanager/AtomicActorSet.h | 25 +- .../source/carla/trafficmanager/AtomicMap.h | 6 +- .../trafficmanager/BatchControlStage.cpp | 135 -- .../carla/trafficmanager/BatchControlStage.h | 76 - .../carla/trafficmanager/CollisionStage.cpp | 807 +++++----- .../carla/trafficmanager/CollisionStage.h | 204 +-- .../source/carla/trafficmanager/Constants.h | 132 ++ .../carla/trafficmanager/DataStructures.h | 71 + .../carla/trafficmanager/InMemoryMap.cpp | 96 +- .../source/carla/trafficmanager/InMemoryMap.h | 26 +- .../trafficmanager/LocalizationStage.cpp | 1308 +++++------------ .../carla/trafficmanager/LocalizationStage.h | 230 +-- .../trafficmanager/LocalizationUtils.cpp | 300 +--- .../carla/trafficmanager/LocalizationUtils.h | 60 +- .../source/carla/trafficmanager/Messenger.h | 96 -- .../trafficmanager/MessengerAndDataTypes.h | 113 -- .../carla/trafficmanager/MotionPlanStage.cpp | 290 ++++ .../carla/trafficmanager/MotionPlanStage.h | 72 + .../trafficmanager/MotionPlannerStage.cpp | 318 ---- .../carla/trafficmanager/MotionPlannerStage.h | 107 -- .../carla/trafficmanager/PIDController.cpp | 99 -- .../carla/trafficmanager/PIDController.h | 107 +- .../carla/trafficmanager/Parameters.cpp | 215 ++- .../source/carla/trafficmanager/Parameters.h | 229 +-- .../trafficmanager/PerformanceDiagnostics.cpp | 48 - .../trafficmanager/PerformanceDiagnostics.h | 102 -- .../carla/trafficmanager/PipelineStage.cpp | 60 - .../carla/trafficmanager/PipelineStage.h | 77 - .../carla/trafficmanager/RandomGenerator.h | 20 + .../carla/trafficmanager/SimpleWaypoint.cpp | 2 + .../carla/trafficmanager/SimpleWaypoint.h | 4 +- .../carla/trafficmanager/SimulationState.cpp | 83 ++ .../carla/trafficmanager/SimulationState.h | 96 ++ .../carla/trafficmanager/SnippetProfiler.h | 80 + LibCarla/source/carla/trafficmanager/Stage.h | 29 + .../carla/trafficmanager/TrackTraffic.cpp | 172 +++ .../carla/trafficmanager/TrackTraffic.h | 56 + .../trafficmanager/TrafficLightStage.cpp | 276 ++-- .../carla/trafficmanager/TrafficLightStage.h | 110 +- .../carla/trafficmanager/TrafficManager.cpp | 2 + .../carla/trafficmanager/TrafficManager.h | 3 + .../carla/trafficmanager/TrafficManagerBase.h | 3 - .../trafficmanager/TrafficManagerClient.h | 10 +- .../trafficmanager/TrafficManagerLocal.cpp | 363 +++-- .../trafficmanager/TrafficManagerLocal.h | 281 ++-- .../trafficmanager/TrafficManagerRemote.cpp | 5 +- .../trafficmanager/TrafficManagerRemote.h | 4 - .../trafficmanager/TrafficManagerServer.h | 3 + PythonAPI/carla/source/libcarla/Actor.cpp | 4 +- PythonAPI/carla/source/libcarla/Client.cpp | 4 +- 53 files changed, 3372 insertions(+), 4112 deletions(-) create mode 100644 LibCarla/source/carla/trafficmanager/ALSM.cpp create mode 100644 LibCarla/source/carla/trafficmanager/ALSM.h delete mode 100644 LibCarla/source/carla/trafficmanager/BatchControlStage.cpp delete mode 100644 LibCarla/source/carla/trafficmanager/BatchControlStage.h create mode 100644 LibCarla/source/carla/trafficmanager/Constants.h create mode 100644 LibCarla/source/carla/trafficmanager/DataStructures.h delete mode 100644 LibCarla/source/carla/trafficmanager/Messenger.h delete mode 100644 LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h create mode 100644 LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp create mode 100644 LibCarla/source/carla/trafficmanager/MotionPlanStage.h delete mode 100644 LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp delete mode 100644 LibCarla/source/carla/trafficmanager/MotionPlannerStage.h delete mode 100644 LibCarla/source/carla/trafficmanager/PIDController.cpp delete mode 100644 LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp delete mode 100644 LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h delete mode 100644 LibCarla/source/carla/trafficmanager/PipelineStage.cpp delete mode 100644 LibCarla/source/carla/trafficmanager/PipelineStage.h create mode 100644 LibCarla/source/carla/trafficmanager/RandomGenerator.h create mode 100644 LibCarla/source/carla/trafficmanager/SimulationState.cpp create mode 100644 LibCarla/source/carla/trafficmanager/SimulationState.h create mode 100644 LibCarla/source/carla/trafficmanager/SnippetProfiler.h create mode 100644 LibCarla/source/carla/trafficmanager/Stage.h create mode 100644 LibCarla/source/carla/trafficmanager/TrackTraffic.cpp create mode 100644 LibCarla/source/carla/trafficmanager/TrackTraffic.h diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index cd2dff58e..9e2f0efcd 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/LibCarla/source/carla/client/Vehicle.h @@ -13,6 +13,8 @@ #include "carla/rpc/VehiclePhysicsControl.h" #include "carla/trafficmanager/TrafficManager.h" +using carla::traffic_manager::constants::Networking::TM_DEFAULT_PORT; + namespace carla { namespace traffic_manager { diff --git a/LibCarla/source/carla/trafficmanager/ALSM.cpp b/LibCarla/source/carla/trafficmanager/ALSM.cpp new file mode 100644 index 000000000..52afc7872 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/ALSM.cpp @@ -0,0 +1,357 @@ + +#include "boost/pointer_cast.hpp" + +#include "carla/client/Actor.h" +#include "carla/client/Vehicle.h" +#include "carla/client/Walker.h" + +#include "carla/trafficmanager/Constants.h" +#include "carla/trafficmanager/LocalizationUtils.h" +#include "carla/trafficmanager/SimpleWaypoint.h" + +#include "carla/trafficmanager/ALSM.h" + +namespace carla { +namespace traffic_manager { + +ALSM::ALSM( + AtomicActorSet ®istered_vehicles, + BufferMap &buffer_map, + TrackTraffic &track_traffic, + const Parameters ¶meters, + const cc::World &world, + const LocalMapPtr &local_map, + SimulationState &simulation_state, + LocalizationStage &localization_stage, + CollisionStage &collision_stage, + TrafficLightStage &traffic_light_stage, + MotionPlanStage &motion_plan_stage) + : registered_vehicles(registered_vehicles), + buffer_map(buffer_map), + track_traffic(track_traffic), + parameters(parameters), + world(world), + local_map(local_map), + simulation_state(simulation_state), + localization_stage(localization_stage), + collision_stage(collision_stage), + traffic_light_stage(traffic_light_stage), + motion_plan_stage(motion_plan_stage) {} + +void ALSM::Update() { + + bool hybrid_physics_mode = parameters.GetHybridPhysicsMode(); + + std::set world_vehicle_ids; + std::set world_pedestrian_ids; + std::vector unregistered_list_to_be_deleted; + + current_timestamp = world.GetSnapshot().GetTimestamp(); + ActorList world_actors = world.GetActors(); + + // Find destroyed actors and perform clean up. + const ALSM::DestroyeddActors destroyed_actors = IdentifyDestroyedActors(world_actors); + + const ActorIdSet &destroyed_registered = destroyed_actors.first; + for (const auto &deletion_id: destroyed_registered) { + RemoveActor(deletion_id, true); + } + + const ActorIdSet &destroyed_unregistered = destroyed_actors.second; + for (auto deletion_id : destroyed_unregistered) { + RemoveActor(deletion_id, false); + } + + // Invalidate hero actor if it is not alive anymore. + if (hybrid_physics_mode && hero_actors.size() != 0u) { + ActorIdSet hero_actors_to_delete; + for (auto &hero_actor_info: hero_actors) { + if(destroyed_unregistered.find(hero_actor_info.first) != destroyed_unregistered.end()) { + hero_actors_to_delete.insert(hero_actor_info.first); + } + } + + for (auto &deletion_id: hero_actors_to_delete) { + hero_actors.erase(deletion_id); + } + } + + // Scan for new unregistered actors. + ALSM::ActorVector new_actors = IdentifyNewActors(world_actors); + for (const ActorPtr &actor: new_actors) { + unregistered_actors.insert({actor->GetId(), actor}); + + // Identify any new hero vehicle if the system is in hybrid physics mode. + if (hybrid_physics_mode) { + if (actor->GetTypeId().front() == 'v') { + ActorId hero_actor_id = actor->GetId(); + for (auto&& attribute: actor->GetAttributes()) { + if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") { + hero_actors.insert({hero_actor_id, actor}); + } + } + } + } + } + + // Update dynamic state and static attributes for all registered vehicles. + ALSM::IdleInfo max_idle_time = std::make_pair(0u, current_timestamp.elapsed_seconds); + UpdateRegisteredActorsData(hybrid_physics_mode, max_idle_time); + + // Destroy registered vehicle if stuck at a location for too long. + if (IsVehicleStuck(max_idle_time.first) + && (current_timestamp.elapsed_seconds - elapsed_last_actor_destruction) > DELTA_TIME_BETWEEN_DESTRUCTIONS) { + registered_vehicles.Destroy(max_idle_time.first); + RemoveActor(max_idle_time.first, true); + elapsed_last_actor_destruction = current_timestamp.elapsed_seconds; + } + + // Update dynamic state and static attributes for unregistered actors. + UpdateUnregisteredActorsData(); +} + +std::vector ALSM::IdentifyNewActors(const ActorList &actor_list) { + std::vector new_actors; + for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) { + ActorPtr actor = *iter; + ActorId actor_id = actor->GetId(); + if (!registered_vehicles.Contains(actor_id) + && unregistered_actors.find(actor_id) == unregistered_actors.end()) { + new_actors.push_back(actor); + } + } + return new_actors; +} + +ALSM::DestroyeddActors ALSM::IdentifyDestroyedActors(const ActorList &actor_list) { + + ALSM::DestroyeddActors destroyed_actors; + ActorIdSet &deleted_registered = destroyed_actors.first; + ActorIdSet &deleted_unregistered = destroyed_actors.second; + + // Building hash set of actors present in current frame. + ActorIdSet current_actors; + for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) { + current_actors.insert((*iter)->GetId()); + } + + // Searching for destroyed registered actors. + std::vector registered_ids = registered_vehicles.GetIDList(); + for (const ActorId &actor_id : registered_ids) { + if (current_actors.find(actor_id) == current_actors.end()) { + deleted_registered.insert(actor_id); + } + } + + // Searching for destroyed unregistered actors. + for (const auto &actor_info: unregistered_actors) { + const ActorId &actor_id = actor_info.first; + if (current_actors.find(actor_id) == current_actors.end() + || registered_vehicles.Contains(actor_id)) { + deleted_unregistered.insert(actor_id); + } + } + + return destroyed_actors; +} + +void ALSM::UpdateRegisteredActorsData(const bool hybrid_physics_mode, ALSM::IdleInfo &max_idle_time) { + + std::vector vehicle_list = registered_vehicles.GetList(); + bool hero_actor_present = hybrid_physics_mode && hero_actors.size() != 0u; + float physics_radius = parameters.GetHybridPhysicsRadius(); + float physics_radius_square = SQUARE(physics_radius); + for (const Actor &vehicle : vehicle_list) { + ActorId actor_id = vehicle->GetId(); + cg::Transform vehicle_transform = vehicle->GetTransform(); + cg::Location vehicle_location = vehicle_transform.location; + cg::Rotation vehicle_rotation = vehicle_transform.rotation; + cg::Vector3D vehicle_velocity = vehicle->GetVelocity(); + + // Initializing idle times. + if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) { + idle_time.insert({actor_id, current_timestamp.elapsed_seconds}); + } + + // Check if current actor is in range of hero actor and enable physics in hybrid mode. + bool in_range_of_hero_actor = false; + if (hero_actor_present) { + for (auto &hero_actor_info: hero_actors) { + const ActorId &hero_actor_id = hero_actor_info.first; + if (simulation_state.ContainsActor(hero_actor_id)) { + const cg::Location &hero_location = simulation_state.GetLocation(hero_actor_id); + if (cg::Math::DistanceSquared(vehicle_location, hero_location) < physics_radius_square) { + in_range_of_hero_actor = true; + break; + } + } + } + } + bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true; + vehicle->SetSimulatePhysics(enable_physics); + + bool state_entry_present = simulation_state.ContainsActor(actor_id); + // If physics is disabled, calculate velocity based on change in position. + if (!enable_physics) { + cg::Location previous_location; + if (state_entry_present) { + previous_location = simulation_state.GetLocation(actor_id); + } else { + previous_location = vehicle_location; + } + cg::Vector3D displacement = (vehicle_location - previous_location); + vehicle_velocity = displacement * INV_HYBRID_DT; + } + + // Updated kinematic state object. + auto vehicle_ptr = boost::static_pointer_cast(vehicle); + KinematicState kinematic_state{vehicle_location, vehicle_rotation, + vehicle_velocity, vehicle_ptr->GetSpeedLimit(), enable_physics}; + + // Updated traffic light state object. + TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()}; + + // Update simulation state. + if (state_entry_present) { + simulation_state.UpdateKinematicState(actor_id, kinematic_state); + simulation_state.UpdateTrafficLightState(actor_id, tl_state); + } + else { + cg::Vector3D dimensions = vehicle_ptr->GetBoundingBox().extent; + StaticAttributes attributes{ActorType::Vehicle, dimensions.x, dimensions.y, dimensions.z}; + + simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state); + } + + // Updating idle time when necessary. + UpdateIdleTime(max_idle_time, actor_id); + } +} + +void ALSM::UpdateUnregisteredActorsData() { + for (auto &actor_info: unregistered_actors) { + + const ActorId actor_id = actor_info.first; + const ActorPtr actor_ptr = actor_info.second; + const std::string type_id = actor_ptr->GetTypeId(); + + const cg::Transform actor_transform = actor_ptr->GetTransform(); + const cg::Location actor_location = actor_transform.location; + const cg::Rotation actor_rotation = actor_transform.rotation; + const cg::Vector3D actor_velocity = actor_ptr->GetVelocity(); + KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true}; + + TrafficLightState tl_state; + ActorType actor_type = ActorType::Any; + cg::Vector3D dimensions; + std::vector nearest_waypoints; + + bool state_entry_not_present = !simulation_state.ContainsActor(actor_id); + if (type_id.front() == 'v') { + auto vehicle_ptr = boost::static_pointer_cast(actor_ptr); + kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit(); + + tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()}; + + if (state_entry_not_present) { + dimensions = vehicle_ptr->GetBoundingBox().extent; + actor_type = ActorType::Vehicle; + StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z}; + + simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state); + } else { + simulation_state.UpdateKinematicState(actor_id, kinematic_state); + simulation_state.UpdateTrafficLightState(actor_id, tl_state); + } + + // Identify occupied waypoints. + cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent; + cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector(); + std::vector corners = {actor_location + cg::Location(extent.x * heading_vector), + actor_location, + actor_location + cg::Location(-extent.x * heading_vector)}; + for (cg::Location &vertex: corners) { + SimpleWaypointPtr nearest_waypoint = local_map->GetWaypointInVicinity(vertex); + if (nearest_waypoint == nullptr) {nearest_waypoint = local_map->GetPedWaypoint(vertex);} + if (nearest_waypoint == nullptr) {nearest_waypoint = local_map->GetWaypoint(actor_location);} + nearest_waypoints.push_back(nearest_waypoint); + } + } + else if (type_id.front() == 'w') { + auto walker_ptr = boost::static_pointer_cast(actor_ptr); + + if (state_entry_not_present) { + dimensions = walker_ptr->GetBoundingBox().extent; + actor_type = ActorType::Pedestrian; + StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z}; + + simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state); + } else { + simulation_state.UpdateKinematicState(actor_id, kinematic_state); + } + + // Identify occupied waypoints. + SimpleWaypointPtr nearest_waypoint = local_map->GetPedWaypoint(actor_location); + if (nearest_waypoint == nullptr) {nearest_waypoint = local_map->GetWaypoint(actor_location);} + nearest_waypoints.push_back(nearest_waypoint); + } + + track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints); + } +} + +void ALSM::UpdateIdleTime(std::pair& max_idle_time, const ActorId& actor_id) { + if (idle_time.find(actor_id) != idle_time.end()) { + double &idle_duration = idle_time.at(actor_id); + TrafficLightState tl_state = simulation_state.GetTLS(actor_id); + if (simulation_state.GetVelocity(actor_id).SquaredLength() > SQUARE(STOPPED_VELOCITY_THRESHOLD) + || (tl_state.at_traffic_light && tl_state.tl_state != TLS::Green)) { + idle_duration = current_timestamp.elapsed_seconds; + } + + // Checking maximum idle time. + if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) { + max_idle_time = std::make_pair(actor_id, idle_duration); + } + } +} + +bool ALSM::IsVehicleStuck(const ActorId& actor_id) { + if (idle_time.find(actor_id) != idle_time.end()) { + double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id); + if (delta_idle_time >= BLOCKED_TIME_THRESHOLD) { + return true; + } + } + return false; +} + +void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) { + if (registered_actor) { + registered_vehicles.Remove({actor_id}); + buffer_map.erase(actor_id); + idle_time.erase(actor_id); + localization_stage.RemoveActor(actor_id); + collision_stage.RemoveActor(actor_id); + traffic_light_stage.RemoveActor(actor_id); + motion_plan_stage.RemoveActor(actor_id); + } + else { + unregistered_actors.erase(actor_id); + hero_actors.erase(actor_id); + } + + track_traffic.DeleteActor(actor_id); + simulation_state.RemoveActor(actor_id); +} + +void ALSM::Reset() { + unregistered_actors.clear(); + idle_time.clear(); + hero_actors.clear(); + elapsed_last_actor_destruction = 0.0; + current_timestamp = world.GetSnapshot().GetTimestamp(); +} + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/ALSM.h b/LibCarla/source/carla/trafficmanager/ALSM.h new file mode 100644 index 000000000..146831e34 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/ALSM.h @@ -0,0 +1,106 @@ + +#pragma once + +#include + +#include "carla/client/ActorList.h" +#include "carla/client/Timestamp.h" +#include "carla/client/World.h" +#include "carla/Memory.h" + +#include "carla/trafficmanager/AtomicActorSet.h" +#include "carla/trafficmanager/CollisionStage.h" +#include "carla/trafficmanager/DataStructures.h" +#include "carla/trafficmanager/InMemoryMap.h" +#include "carla/trafficmanager/LocalizationStage.h" +#include "carla/trafficmanager/MotionPlanStage.h" +#include "carla/trafficmanager/Parameters.h" +#include "carla/trafficmanager/SimulationState.h" +#include "carla/trafficmanager/TrafficLightStage.h" + +namespace carla { +namespace traffic_manager { + +using namespace constants::HybridMode; +using namespace constants::VehicleRemoval; + +namespace chr = std::chrono; +namespace cg = carla::geom; +namespace cc = carla::client; + +using ActorList = carla::SharedPtr; +using ActorMap = std::unordered_map; +using IdleTimeMap = std::unordered_map; +using LocalMapPtr = std::shared_ptr; + +/// ALSM: Agent Lifecycle and State Managerment +/// This class has functionality to update the local cache of kinematic states +/// and manage memory and cleanup for varying number of vehicles in the simulation. +class ALSM { + +private: + AtomicActorSet ®istered_vehicles; + // Structure containing vehicles in the simulator not registered with the traffic manager. + ActorMap unregistered_actors; + BufferMap &buffer_map; + // Structure keeping track of duration of vehicles stuck in a location. + IdleTimeMap idle_time; + // Structure containing vehicles with attribute role_name with value hero. + ActorMap hero_actors; + TrackTraffic &track_traffic; + const Parameters ¶meters; + const cc::World &world; + const LocalMapPtr &local_map; + SimulationState &simulation_state; + LocalizationStage &localization_stage; + CollisionStage &collision_stage; + TrafficLightStage &traffic_light_stage; + MotionPlanStage &motion_plan_stage; + // Time elapsed since last vehicle destruction due to being idle for too long. + double elapsed_last_actor_destruction {0.0}; + cc::Timestamp current_timestamp; + + // Updates the duration for which a registered vehicle is stuck at a location. + void UpdateIdleTime(std::pair& max_idle_time, const ActorId& actor_id); + + // Method to determine if a vehicle is stuck at a place for too long. + bool IsVehicleStuck(const ActorId& actor_id); + + // Removes an actor from traffic manager and performs clean up of associated data + // from various stages tracking the said vehicle. + void RemoveActor(const ActorId actor_id, const bool registered_actor); + + using ActorVector = std::vector; + // Method to identify actors newly spawned in the simulation since last tick. + ActorVector IdentifyNewActors(const ActorList &actor_list); + + using DestroyeddActors = std::pair; + // Method to identify actors deleted in the last frame. + // Arrays of registered and unregistered actors are returned separately. + DestroyeddActors IdentifyDestroyedActors(const ActorList &actor_list); + + using IdleInfo = std::pair; + void UpdateRegisteredActorsData(const bool hybrid_physics_mode, IdleInfo &max_idle_time); + + void UpdateUnregisteredActorsData(); + +public: + ALSM(AtomicActorSet ®istered_vehicles, + BufferMap &buffer_map, + TrackTraffic &track_traffic, + const Parameters ¶meters, + const cc::World &world, + const LocalMapPtr &local_map, + SimulationState &simulation_state, + LocalizationStage &localization_stage, + CollisionStage &collision_stage, + TrafficLightStage &traffic_light_stage, + MotionPlanStage &motion_plan_stage); + + void Update(); + + void Reset(); +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h index 4fc8adf40..d7d4a1892 100644 --- a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h +++ b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h @@ -61,21 +61,26 @@ namespace traffic_manager { ++state_counter; } - void Remove(std::vector actor_list) { + void Remove(std::vector actor_id_list) { std::lock_guard lock(modification_mutex); - for (auto& actor: actor_list) { - actor_set.erase(actor->GetId()); + for (auto& actor_id: actor_id_list) { + if (actor_set.find(actor_id) != actor_set.end()){ + actor_set.erase(actor_id); + } } ++state_counter; } - void Destroy(ActorPtr actor) { + void Destroy(ActorId actor_id) { std::lock_guard lock(modification_mutex); - actor_set.erase(actor->GetId()); - actor->Destroy(); - ++state_counter; + if (actor_set.find(actor_id) != actor_set.end()) { + ActorPtr actor = actor_set.at(actor_id); + actor->Destroy(); + actor_set.erase(actor_id); + ++state_counter; + } } int GetState() { @@ -96,6 +101,12 @@ namespace traffic_manager { return actor_set.size(); } + void Clear() { + + std::lock_guard lock(modification_mutex); + return actor_set.clear(); + } + }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/AtomicMap.h b/LibCarla/source/carla/trafficmanager/AtomicMap.h index e28e37829..e2c5a9f08 100644 --- a/LibCarla/source/carla/trafficmanager/AtomicMap.h +++ b/LibCarla/source/carla/trafficmanager/AtomicMap.h @@ -17,7 +17,7 @@ namespace traffic_manager { private: - std::mutex map_mutex; + mutable std::mutex map_mutex; std::unordered_map map; public: @@ -30,13 +30,13 @@ namespace traffic_manager { map.insert(entry); } - bool Contains(const Key &key) { + bool Contains(const Key &key) const { std::lock_guard lock(map_mutex); return map.find(key) != map.end(); } - Value &GetValue(const Key &key) { + const Value &GetValue(const Key &key) const { std::lock_guard lock(map_mutex); return map.at(key); diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp b/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp deleted file mode 100644 index 3d9aec9f2..000000000 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "carla/trafficmanager/BatchControlStage.h" - -namespace carla { -namespace traffic_manager { - -BatchControlStage::BatchControlStage( - std::string stage_name, - std::shared_ptr messenger, - carla::client::detail::EpisodeProxy &episode_proxy, - Parameters ¶meters) - : PipelineStage(stage_name), - messenger(messenger), - episode_proxy_bcs(episode_proxy), - parameters(parameters) { - - // Initializing number of vehicles to zero in the beginning. - number_of_vehicles = 0u; -} - -BatchControlStage::~BatchControlStage() {} - -void BatchControlStage::Action() { - - // Looping over registered actors. - for (uint64_t i = 0u; i < number_of_vehicles && data_frame != nullptr; ++i) { - - carla::rpc::VehicleControl vehicle_control; - - const PlannerToControlData &element = data_frame->at(i); - if (!element.actor || !element.actor->IsAlive()) { - continue; - } - const carla::ActorId actor_id = element.actor->GetId(); - - // Apply actuation from controller if physics enabled. - if (element.physics_enabled) { - vehicle_control.throttle = element.throttle; - vehicle_control.brake = element.brake; - vehicle_control.steer = element.steer; - - commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control); - } - // Apply target transform for physics-less vehicles. - else { - - const cg::Transform vehicle_transform = element.actor->GetTransform(); - const cg::Location vehicle_location = vehicle_transform.location; - const cg::Vector3D vehicle_heading = vehicle_transform.GetForwardVector(); - const cg::Transform &teleportation_transform = element.transform; - const cg::Location &teleportation_location_candidate = teleportation_transform.location; - const cg::Vector3D teleprotation_relative_position = teleportation_location_candidate - vehicle_location; - - cg::Transform corrected_teleportation_transform = teleportation_transform; - // Validate if teleportation transform is ahead of the vehicle's current position. - // If not, then teleport to vehicle's current position. - if (cg::Math::Dot(teleprotation_relative_position, vehicle_heading) < 0.0f) { - corrected_teleportation_transform = vehicle_transform; - } - - commands->at(i) = carla::rpc::Command::ApplyTransform(actor_id, corrected_teleportation_transform); - } - } -} - -void BatchControlStage::DataReceiver() { - - data_frame = messenger->Peek(); - - // Allocating new containers for the changed number of registered vehicles. - if (data_frame != nullptr && number_of_vehicles != (*data_frame.get()).size()) { - - number_of_vehicles = static_cast((*data_frame.get()).size()); - - // Allocating array for command batching. - commands = std::make_shared>(number_of_vehicles); - } -} - -void BatchControlStage::DataSender() { - - messenger->Pop(); - bool synch_mode = parameters.GetSynchronousMode(); - - // Asynchronous mode. - if (!synch_mode) { - // Apply batch command through an asynchronous RPC call. - if (commands != nullptr) { - episode_proxy_bcs.Lock()->ApplyBatch(*commands.get(), false); - } - // Applying an infinitesimal sleep statement for providing a system cancellation point. - std::this_thread::sleep_for(1us); - } - // Synchronous mode. - else { - std::unique_lock lock(step_execution_mutex); - // TODO: Re-introduce timeout while waiting for RunStep() call. - while (!run_step.load()) { - step_execution_notifier.wait_for(lock, 1ms, [this]() {return run_step.load();}); - } - // Apply batch command in synchronous RPC call. - if (commands != nullptr) { - episode_proxy_bcs.Lock()->ApplyBatchSync(*commands.get(), true); - } - // Set flag to false, unblock RunStep() call and release mutex lock. - run_step.store(false); - step_complete_notifier.notify_one(); - lock.unlock(); - } -} - - -bool BatchControlStage::RunStep() { - - bool synch_mode = parameters.GetSynchronousMode(); - if (synch_mode) { - std::unique_lock lock(step_execution_mutex); - // Set flag to true, notify DataSender and wait for a return notification - run_step.store(true); - step_execution_notifier.notify_one(); - while (run_step.load()) { - step_complete_notifier.wait_for(lock, 1ms, [this]() {return !run_step.load();}); - } - } - - return true; -} - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/BatchControlStage.h b/LibCarla/source/carla/trafficmanager/BatchControlStage.h deleted file mode 100644 index 4e0a9ced5..000000000 --- a/LibCarla/source/carla/trafficmanager/BatchControlStage.h +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include -#include -#include -#include - -#include "carla/client/detail/EpisodeProxy.h" -#include "carla/client/detail/Simulator.h" -#include "carla/Logging.h" -#include "carla/rpc/ActorId.h" -#include "carla/rpc/Command.h" -#include "carla/rpc/VehicleControl.h" - -#include "carla/trafficmanager/MessengerAndDataTypes.h" -#include "carla/trafficmanager/Parameters.h" -#include "carla/trafficmanager/PipelineStage.h" - -namespace carla { -namespace traffic_manager { - -using namespace std::literals::chrono_literals; - -/// This class receives actuation signals (throttle, brake, steer) -/// from Motion Planner Stage class and communicates these signals to -/// the simulator in batches to control vehicles' movement. -class BatchControlStage : public PipelineStage { - -private: - - /// Pointer to frame received from Motion Planner. - std::shared_ptr data_frame; - /// Pointer to a messenger from Motion Planner. - std::shared_ptr messenger; - /// Reference to CARLA client connection object. - carla::client::detail::EpisodeProxy episode_proxy_bcs; - /// Array to hold command batch. - std::shared_ptr> commands; - /// Number of vehicles registered with the traffic manager. - uint64_t number_of_vehicles; - /// Parameter object for changing synchronous behaviour. - Parameters ¶meters; - /// Step runner flag. - std::atomic run_step {false}; - /// Mutex for progressing synchronous execution. - std::mutex step_execution_mutex; - /// Condition variables for progressing synchronous execution. - std::condition_variable step_execution_notifier; - std::condition_variable step_complete_notifier; - -public: - - BatchControlStage(std::string stage_name, - std::shared_ptr messenger, - carla::client::detail::EpisodeProxy &episode_proxy, - Parameters ¶meters); - - ~BatchControlStage(); - - void DataReceiver() override; - - void Action() override; - - void DataSender() override; - - bool RunStep(); -}; - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 6ce8c2f53..37cdc2b69 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -1,382 +1,205 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . -#include "CollisionStage.h" +#include "carla/geom/Math.h" -#define MAX(__a, __b) ((__a) > (__b) ? (__a) : (__b)) +#include "carla/trafficmanager/Constants.h" +#include "carla/trafficmanager/LocalizationUtils.h" + +#include "carla/trafficmanager/CollisionStage.h" namespace carla { namespace traffic_manager { -namespace CollisionStageConstants { +using Point2D = bg::model::point; +using TLS = carla::rpc::TrafficLightState; - static const float VERTICAL_OVERLAP_THRESHOLD = 4.0f; - static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f; - static const float BOUNDARY_EXTENSION_MAXIMUM = 50.0f; - static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f; - static const float LOCKING_DISTANCE_PADDING = 4.0f; - static const float MAX_LOCKING_EXTENSION = 10.0f; - static const float MAX_COLLISION_RADIUS = 100.0f; - static const float MIN_COLLISION_RADIUS = 15.0f; - static const float WALKER_TIME_EXTENSION = 1.5f; - static const float EPSILON_VELOCITY = 0.1f; - static const float INTER_BBOX_DISTANCE_THRESHOLD = 0.3f; - static const float SQUARE_ROOT_OF_TWO = 1.414f; - static const float COS_10_DEGREES = 0.9848f; -} // namespace CollisionStageConstants +using namespace constants::Collision; +using constants::WaypointSelection::JUNCTION_LOOK_AHEAD; - using namespace CollisionStageConstants; +CollisionStage::CollisionStage( + const std::vector &vehicle_id_list, + const SimulationState &simulation_state, + const BufferMap &buffer_map, + const TrackTraffic &track_traffic, + const Parameters ¶meters, + CollisionFrame &output_array, + cc::DebugHelper &debug_helper) + : vehicle_id_list(vehicle_id_list), + simulation_state(simulation_state), + buffer_map(buffer_map), + track_traffic(track_traffic), + parameters(parameters), + output_array(output_array), + debug_helper(debug_helper) {} - CollisionStage::CollisionStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr planner_messenger, - Parameters ¶meters, - cc::DebugHelper &debug_helper) - : PipelineStage(stage_name), - localization_messenger(localization_messenger), - planner_messenger(planner_messenger), - parameters(parameters), - debug_helper(debug_helper) { +void CollisionStage::Update(const unsigned long index) { + ActorId obstacle_id = 0u; + bool collision_hazard = false; + float available_distance_margin = std::numeric_limits::infinity(); - // Initializing clock for checking unregistered actors periodically. - last_world_actors_pass_instance = chr::system_clock::now(); - // Initializing output array selector. - frame_selector = true; - // Initializing the number of vehicles to zero in the beginning. - number_of_vehicles = 0u; - // Initializing srand. - srand(static_cast(time(NULL))); + const ActorId ego_actor_id = vehicle_id_list.at(index); + if (simulation_state.ContainsActor(ego_actor_id)) { + const cg::Location ego_location = simulation_state.GetLocation(ego_actor_id); + const Buffer &ego_buffer = buffer_map.at(ego_actor_id); + const unsigned long look_ahead_index = GetTargetWaypoint(ego_buffer, JUNCTION_LOOK_AHEAD).second; - } + ActorIdSet overlapping_actors = track_traffic.GetOverlappingVehicles(ego_actor_id); + std::vector collision_candidate_ids; - CollisionStage::~CollisionStage() {} - - void CollisionStage::Action() { - - // Clearing the old chache. - vehicle_cache.clear(); - const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; - - // Looping over registered actors. - for (uint64_t i = 0u; i < number_of_vehicles && localization_frame != nullptr; ++i) { - - LocalizationToCollisionData &data = localization_frame->at(i); - if (!data.actor->IsAlive()) { - continue; + // Run through vehicles with overlapping paths and filter them; + float collision_radius_square = SQUARE(MAX_COLLISION_RADIUS); + for (ActorId overlapping_actor_id : overlapping_actors) { + // If actor is within maximum collision avoidance and vertical overlap range. + const cg::Location &overlapping_actor_location = simulation_state.GetLocation(overlapping_actor_id); + if (overlapping_actor_id != ego_actor_id + && cg::Math::DistanceSquared(overlapping_actor_location, ego_location) < collision_radius_square + && std::abs(ego_location.z - overlapping_actor_location.z) < VERTICAL_OVERLAP_THRESHOLD) { + collision_candidate_ids.push_back(overlapping_actor_id); } + } - const Actor ego_actor = data.actor; - const ActorId ego_actor_id = ego_actor->GetId(); - const cg::Location ego_location = ego_actor->GetLocation(); - const cg::Vector3D ego_velocity = data.velocity; + // Sorting collision candidates in accending order of distance to current vehicle. + std::sort(collision_candidate_ids.begin(), collision_candidate_ids.end(), + [this, &ego_location](const ActorId &a_id_1, const ActorId &a_id_2) { + const cg::Location &e_loc = ego_location; + const cg::Location &loc_1 = simulation_state.GetLocation(a_id_1); + const cg::Location &loc_2 = simulation_state.GetLocation(a_id_2); + return (cg::Math::DistanceSquared(e_loc, loc_1) < cg::Math::DistanceSquared(e_loc, loc_2)); + }); - const SimpleWaypointPtr& closest_point = data.closest_waypoint; - const SimpleWaypointPtr& junction_look_ahead = data.junction_look_ahead_waypoint; - using OverlappingActorInfo = std::vector>; - const OverlappingActorInfo &collision_candidates = data.overlapping_actors; + // Check every actor in the vicinity if it poses a collision hazard. + for (auto iter = collision_candidate_ids.begin(); + iter != collision_candidate_ids.end() && !collision_hazard; + ++iter) { + const ActorId other_actor_id = *iter; + const ActorType other_actor_type = simulation_state.GetType(other_actor_id); - bool collision_hazard = false; - float available_distance_margin = std::numeric_limits::infinity(); - cg::Vector3D obstacle_velocity; - const SimpleWaypointPtr safe_point_junction = data.safe_point_after_junction; - - try { - // Check every actor in the vicinity if it poses a collision hazard. - for (auto actor_info = collision_candidates.begin(); - actor_info != collision_candidates.end() && !collision_hazard; - ++actor_info) { - - ActorId other_actor_id; - Actor other_actor; - cg::Vector3D other_velocity; - std::tie(other_actor_id, other_actor, other_velocity) = *actor_info; - - if (!other_actor->IsAlive()) continue; - const auto other_actor_type = other_actor->GetTypeId(); - const cg::Location other_location = other_actor->GetLocation(); - - // Collision checks increase with speed - float collision_distance = std::pow(floor(ego_velocity.Length()*3.6f/10.0f),2.0f); - collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS); - - // Temporary fix to (0,0,0) bug - if (!(other_location.x == 0 && other_location.y == 0 && other_location.z == 0)) { - - if (other_actor_id != ego_actor_id && - (cg::Math::DistanceSquared(ego_location, other_location) - < std::pow(MAX_COLLISION_RADIUS, 2)) && - (std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) { - - if (parameters.GetCollisionDetection(ego_actor, other_actor)) { - - std::pair negotiation_result = NegotiateCollision(ego_actor, other_actor, ego_location, - other_location, closest_point, junction_look_ahead, - ego_velocity, other_velocity); - if ((safe_point_junction != nullptr - && !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction, - other_location, other_velocity)) - || negotiation_result.first) - { - - if ((other_actor_type[0] == 'v' && parameters.GetPercentageIgnoreVehicles(ego_actor) <= (rand() % 101)) || - (other_actor_type[0] == 'w' && parameters.GetPercentageIgnoreWalkers(ego_actor) <= (rand() % 101))) - { - collision_hazard = true; - available_distance_margin = negotiation_result.second; - obstacle_velocity = other_velocity; - } - } - } - } + if (parameters.GetCollisionDetection(ego_actor_id, other_actor_id) + && buffer_map.find(ego_actor_id) != buffer_map.end() + && simulation_state.ContainsActor(other_actor_id)) { + std::pair negotiation_result = NegotiateCollision(ego_actor_id, + other_actor_id, + look_ahead_index); + if (negotiation_result.first) { + if ((other_actor_type == ActorType::Vehicle + && parameters.GetPercentageIgnoreVehicles(ego_actor_id) <= pgen.next()) + || (other_actor_type == ActorType::Pedestrian + && parameters.GetPercentageIgnoreWalkers(ego_actor_id) <= pgen.next())) { + collision_hazard = true; + obstacle_id = other_actor_id; + available_distance_margin = negotiation_result.second; } } - - } catch (const std::exception&) { - carla::log_info("Actor might not be alive \n"); } - - CollisionToPlannerData &message = current_planner_frame->at(i); - message.hazard = collision_hazard; - message.distance_to_other_vehicle = available_distance_margin; - message.other_vehicle_velocity = obstacle_velocity; } } - void CollisionStage::DataReceiver() { + CollisionHazardData &output_element = output_array.at(index); + output_element.hazard_actor_id = obstacle_id; + output_element.hazard = collision_hazard; + output_element.available_distance_margin = available_distance_margin; +} - localization_frame = localization_messenger->Peek(); +void CollisionStage::RemoveActor(const ActorId actor_id) { + collision_locks.erase(actor_id); +} - if (localization_frame != nullptr) { +void CollisionStage::Reset() { + collision_locks.clear(); +} - // Connecting actor ids to their position indices on data arrays. - // This map also provides us the additional benefit of being - // able to quickly identify if a vehicle id is registered - // with the traffic manager or not. - vehicle_id_to_index.clear(); - uint64_t index = 0u; - for (auto &element: *localization_frame.get()) { - vehicle_id_to_index.insert({element.actor->GetId(), index++}); - } +float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id) { - // Allocating new containers for the changed number - // of registered vehicles. - if (number_of_vehicles != (*localization_frame.get()).size()) { - - number_of_vehicles = static_cast((*localization_frame.get()).size()); - - // Allocating output arrays to be shared with motion planner stage. - planner_frame_a = std::make_shared(number_of_vehicles); - planner_frame_b = std::make_shared(number_of_vehicles); - } + const float velocity = cg::Math::Dot(simulation_state.GetVelocity(actor_id), simulation_state.GetHeading(actor_id)); + float bbox_extension; + // Using a linear function to calculate boundary length. + bbox_extension = BOUNDARY_EXTENSION_RATE * velocity + BOUNDARY_EXTENSION_MINIMUM; + // If a valid collision lock present, change boundary length to maintain lock. + if (collision_locks.find(actor_id) != collision_locks.end()) { + const CollisionLock &lock = collision_locks.at(actor_id); + float lock_boundary_length = static_cast(lock.distance_to_lead_vehicle + LOCKING_DISTANCE_PADDING); + // Only extend boundary track vehicle if the leading vehicle + // if it is not further than velocity dependent extension by MAX_LOCKING_EXTENSION. + if ((lock_boundary_length - lock.initial_lock_distance) < MAX_LOCKING_EXTENSION) { + bbox_extension = lock_boundary_length; } - - // Cleaning geodesic boundaries from the last iteration. - geodesic_boundaries.clear(); - } - void CollisionStage::DataSender() { + return bbox_extension; +} - localization_messenger->Pop(); +LocationVector CollisionStage::GetBoundary(const ActorId actor_id) { + const ActorType actor_type = simulation_state.GetType(actor_id); + const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id); - planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b); - frame_selector = !frame_selector; + float forward_extension = 0.0f; + if (actor_type == ActorType::Pedestrian) { + // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions. + forward_extension = simulation_state.GetVelocity(actor_id).Length() * WALKER_TIME_EXTENSION; } - std::pair CollisionStage::NegotiateCollision(const Actor &reference_vehicle, const Actor &other_vehicle, - const cg::Location &reference_location, - const cg::Location &other_location, - const SimpleWaypointPtr &closest_point, - const SimpleWaypointPtr &junction_look_ahead, - const cg::Vector3D reference_velocity, - const cg::Vector3D other_velocity) { + cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id); - // Vehicle IDs - const ActorId reference_vehicle_id = reference_vehicle->GetId(); - const ActorId other_vehicle_id = other_vehicle->GetId(); + float bbox_x = dimensions.x; + float bbox_y = dimensions.y; - // Output variables for the method. - bool hazard = false; - float available_distance_margin = std::numeric_limits::infinity(); + const cg::Vector3D x_boundary_vector = heading_vector * (bbox_x + forward_extension); + const auto perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f).MakeUnitVector(); + const cg::Vector3D y_boundary_vector = perpendicular_vector * (bbox_y + forward_extension); - // Ego and other vehicle heading. - const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector(); - // Vector from ego position to position of the other vehicle. - const float vector_magnitude_epsilon = 2 * std::numeric_limits::epsilon(); - cg::Vector3D reference_to_other = other_location - reference_location; - reference_to_other = reference_to_other.MakeSafeUnitVector(vector_magnitude_epsilon); + // Four corners of the vehicle in top view clockwise order (left-handed system). + const cg::Location location = simulation_state.GetLocation(actor_id); + LocationVector bbox_boundary = { + location + cg::Location(x_boundary_vector - y_boundary_vector), + location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector), + location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector), + location + cg::Location(x_boundary_vector + y_boundary_vector), + }; - // Other vehicle heading. - const cg::Vector3D other_heading = other_vehicle->GetTransform().GetForwardVector(); - // Vector from other vehicle position to ego position. - cg::Vector3D other_to_reference = reference_location - other_location; - other_to_reference = other_to_reference.MakeSafeUnitVector(vector_magnitude_epsilon); + return bbox_boundary; +} - // Obtain cc::Vehicle pointers and calculate half diagonal length of vehicle bounding box. - const auto reference_vehicle_ptr = boost::static_pointer_cast(reference_vehicle); - const auto other_vehicle_ptr = boost::static_pointer_cast(other_vehicle); - float reference_vehicle_length = reference_vehicle_ptr->GetBoundingBox().extent.x * SQUARE_ROOT_OF_TWO; - float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * SQUARE_ROOT_OF_TWO; +LocationVector CollisionStage::GetGeodesicBoundary(const ActorId actor_id) { + LocationVector geodesic_boundary; - float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location); - float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id, - reference_velocity, reference_heading); - float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle_id, other_velocity, other_heading); - // Calculate minimum distance between vehicle to consider collision negotiation. - float inter_vehicle_length = reference_vehicle_length + other_vehicle_length; - float ego_detection_range = std::pow(ego_bounding_box_extension + inter_vehicle_length, 2.0f); - float cross_detection_range = std::pow(ego_bounding_box_extension + inter_vehicle_length - + other_bounding_box_extension, 2.0f); + if (geodesic_boundary_map.find(actor_id) != geodesic_boundary_map.end()) { + geodesic_boundary = geodesic_boundary_map.at(actor_id); + } else { + const LocationVector bbox = GetBoundary(actor_id); - // Conditions to consider collision negotiation. - bool other_vehicle_in_ego_range = inter_vehicle_distance < ego_detection_range; - bool other_vehicles_in_cross_detection_range = inter_vehicle_distance < cross_detection_range; - bool other_vehicle_in_front = cg::Math::Dot(reference_heading, reference_to_other) > 0; - bool ego_inside_junction = closest_point->CheckJunction(); - bool ego_at_traffic_light = reference_vehicle_ptr->IsAtTrafficLight(); - bool ego_stopped_by_light = reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green; - bool ego_at_junction_entrance = !closest_point->CheckJunction() && junction_look_ahead->CheckJunction(); + if (buffer_map.find(actor_id) != buffer_map.end()) { + float bbox_extension = GetBoundingBoxExtention(actor_id); + const float specific_lead_distance = parameters.GetDistanceToLeadingVehicle(actor_id); + bbox_extension = std::max(specific_lead_distance, bbox_extension); + const float bbox_extension_square = SQUARE(bbox_extension); - // Conditions to consider collision negotiation. - if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light) - && ((ego_inside_junction && other_vehicles_in_cross_detection_range) - || (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) { + LocationVector left_boundary; + LocationVector right_boundary; + cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id); + const float width = dimensions.y; + const float length = dimensions.x; - GeometryComparisonCache cache = GetGeometryBetweenActors(reference_vehicle, other_vehicle, - reference_location, other_location, - reference_velocity, other_velocity); + const Buffer &waypoint_buffer = buffer_map.at(actor_id); + const TargetWPInfo target_wp_info = GetTargetWaypoint(waypoint_buffer, length); + const SimpleWaypointPtr boundary_start = target_wp_info.first; + const uint64_t boundary_start_index = target_wp_info.second; - // Conditions for collision negotiation. - bool geodesic_path_bbox_touching = cache.inter_geodesic_distance < 0.1; - bool vehicle_bbox_touching = cache.inter_bbox_distance < 0.1; - bool ego_path_clear = cache.other_vehicle_to_reference_geodesic > 0.1; - bool other_path_clear = cache.reference_vehicle_to_other_geodesic > 0.1; - bool ego_path_priority = cache.reference_vehicle_to_other_geodesic < cache.other_vehicle_to_reference_geodesic; - bool ego_angular_priority = cg::Math::Dot(reference_heading, reference_to_other) - < cg::Math::Dot(other_heading, other_to_reference); - - // Whichever vehicle's path is farthest away from the other vehicle gets priority to move. - if (geodesic_path_bbox_touching - && ((!vehicle_bbox_touching - && (!ego_path_clear || (ego_path_clear && other_path_clear && !ego_angular_priority && !ego_path_priority))) - || (vehicle_bbox_touching && !ego_angular_priority && !ego_path_priority))) { - hazard = true; - - const float specific_distance_margin = MAX(parameters.GetDistanceToLeadingVehicle(reference_vehicle), - BOUNDARY_EXTENSION_MINIMUM); - available_distance_margin = static_cast(MAX(cache.reference_vehicle_to_other_geodesic - - specific_distance_margin, 0.0)); - - ///////////////////////////////////// Collision locking mechanism ///////////////////////////////// - // The idea is, when encountering a possible collision, - // we should ensure that the bounding box extension doesn't decrease too fast and loose collision tracking. - // This enables us to smoothly approach the lead vehicle. - - // When possible collision found, check if an entry for collision lock present. - if (collision_locks.find(reference_vehicle_id) != collision_locks.end()) { - CollisionLock &lock = collision_locks.at(reference_vehicle_id); - // Check if the same vehicle is under lock. - if (other_vehicle_id == lock.lead_vehicle_id) { - // If the body of the lead vehicle is touching the reference vehicle bounding box. - if (cache.other_vehicle_to_reference_geodesic < 0.1) { - // Distance between the bodies of the vehicles. - lock.distance_to_lead_vehicle = cache.inter_bbox_distance; - } else { - // Distance from reference vehicle body to other vehicle path polygon. - lock.distance_to_lead_vehicle = cache.reference_vehicle_to_other_geodesic; - } - } else { - // If possible collision with a new vehicle, re-initialize with new lock entry. - lock = {other_vehicle_id, cache.inter_bbox_distance, cache.inter_bbox_distance}; - } - } else { - // Insert and initialize lock entry if not present. - collision_locks.insert({reference_vehicle_id, - {other_vehicle_id, cache.inter_bbox_distance, cache.inter_bbox_distance}}); - } - } - } - - // If no collision hazard detected, then flush collision lock held by the vehicle. - if (!hazard && collision_locks.find(reference_vehicle_id) != collision_locks.end()) { - collision_locks.erase(reference_vehicle_id); - } - - return {hazard, available_distance_margin}; - } - - traffic_manager::Polygon CollisionStage::GetPolygon(const LocationList &boundary) { - - traffic_manager::Polygon boundary_polygon; - for (const cg::Location &location: boundary) { - bg::append(boundary_polygon.outer(), Point2D(location.x, location.y)); - } - bg::append(boundary_polygon.outer(), Point2D(boundary.front().x, boundary.front().y)); - - return boundary_polygon; - } - - LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location, - const cg::Vector3D velocity) { - - const ActorId actor_id = actor->GetId(); - - if (geodesic_boundaries.find(actor_id) != geodesic_boundaries.end()) { - return geodesic_boundaries.at(actor_id); - } - - const LocationList bbox = GetBoundary(actor, vehicle_location, velocity); - - if (vehicle_id_to_index.find(actor_id) != vehicle_id_to_index.end()) { - - float bbox_extension = GetBoundingBoxExtention(actor_id, velocity, actor->GetTransform().GetForwardVector()); - - const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor); - if (specific_distance_margin > 0.0f) { - bbox_extension = MAX(specific_distance_margin, bbox_extension); - } - - const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor_id)).buffer; - - LocationList left_boundary; - LocationList right_boundary; - const auto vehicle = boost::static_pointer_cast(actor); - const float width = vehicle->GetBoundingBox().extent.y; - const float length_squared = std::pow(vehicle->GetBoundingBox().extent.x*2, 2.0f); - - SimpleWaypointPtr boundary_start = waypoint_buffer.front(); - SimpleWaypointPtr front_waypoint = waypoint_buffer.front(); - uint64_t boundary_start_index = 0u; - while (boundary_start->DistanceSquared(front_waypoint) < length_squared && - boundary_start_index < waypoint_buffer.size() -1) { - boundary_start = waypoint_buffer.at(boundary_start_index); - ++boundary_start_index; - } - SimpleWaypointPtr boundary_end = nullptr; - SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index); - - const auto vehicle_reference = boost::static_pointer_cast(actor); // At non-signalized junctions, we extend the boundary across the junction // and in all other situations, boundary length is velocity-dependent. + SimpleWaypointPtr boundary_end = nullptr; + SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index); bool reached_distance = false; for (uint64_t j = boundary_start_index; !reached_distance && (j < waypoint_buffer.size()); ++j) { - - if (boundary_start->DistanceSquared(current_point) > std::pow(bbox_extension, 2) - || j == waypoint_buffer.size() - 1) { + if (boundary_start->DistanceSquared(current_point) > bbox_extension_square || j == waypoint_buffer.size() - 1) { reached_distance = true; } - if (boundary_end == nullptr || cg::Math::Dot(boundary_end->GetForwardVector(), current_point->GetForwardVector()) < COS_10_DEGREES || reached_distance) { const cg::Vector3D heading_vector = current_point->GetForwardVector(); const cg::Location location = current_point->GetLocation(); - cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, heading_vector.z); + cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f); + perpendicular_vector = perpendicular_vector.MakeUnitVector(); // Direction determined for the left-handed system. const cg::Vector3D scaled_perpendicular = perpendicular_vector * width; left_boundary.push_back(location + cg::Location(scaled_perpendicular)); @@ -388,8 +211,6 @@ namespace CollisionStageConstants { current_point = waypoint_buffer.at(j); } - // Connecting the geodesic path boundary with the vehicle bounding box. - LocationList geodesic_boundary; // Reversing right boundary to construct clockwise (left-hand system) // boundary. This is so because both left and right boundary vectors have // the closest point to the vehicle at their starting index for the right @@ -399,169 +220,209 @@ namespace CollisionStageConstants { geodesic_boundary.insert(geodesic_boundary.end(), right_boundary.begin(), right_boundary.end()); geodesic_boundary.insert(geodesic_boundary.end(), bbox.begin(), bbox.end()); geodesic_boundary.insert(geodesic_boundary.end(), left_boundary.begin(), left_boundary.end()); - - geodesic_boundaries.insert({actor->GetId(), geodesic_boundary}); - return geodesic_boundary; } else { - geodesic_boundaries.insert({actor->GetId(), bbox}); - return bbox; + geodesic_boundary = bbox; } + geodesic_boundary_map.insert({actor_id, geodesic_boundary}); } - float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id, - const cg::Vector3D velocity_vector, - const cg::Vector3D heading_vector) { + return geodesic_boundary; +} - const float velocity = cg::Math::Dot(velocity_vector, heading_vector); - float bbox_extension; - // Using a linear function to calculate boundary length. - // min speed : 0kmph -> BOUNDARY_EXTENSION_MINIMUM - // max speed : 100kmph -> BOUNDARY_EXTENSION_MAXIMUM - float slope = (BOUNDARY_EXTENSION_MAXIMUM - BOUNDARY_EXTENSION_MINIMUM) / ARBITRARY_MAX_SPEED; - bbox_extension = slope * velocity + BOUNDARY_EXTENSION_MINIMUM; - // If a valid collision lock present, change boundary length to maintain lock. - if (collision_locks.find(actor_id) != collision_locks.end()) - { - CollisionLock &lock = collision_locks.at(actor_id); - float lock_boundary_length = static_cast(lock.distance_to_lead_vehicle - + LOCKING_DISTANCE_PADDING); - // Only extend boundary track vehicle if the leading vehicle - // if it is not further than velocity dependent extension by MAX_LOCKING_EXTENSION. - if ((lock_boundary_length - lock.initial_lock_distance) < MAX_LOCKING_EXTENSION) - { - bbox_extension = lock_boundary_length; +Polygon CollisionStage::GetPolygon(const LocationVector &boundary) { + + traffic_manager::Polygon boundary_polygon; + for (const cg::Location &location : boundary) { + bg::append(boundary_polygon.outer(), Point2D(location.x, location.y)); + } + bg::append(boundary_polygon.outer(), Point2D(boundary.front().x, boundary.front().y)); + + return boundary_polygon; +} + +GeometryComparison CollisionStage::GetGeometryBetweenActors(const ActorId reference_vehicle_id, + const ActorId other_actor_id) { + + + std::pair key_parts; + if (reference_vehicle_id < other_actor_id) { + key_parts = {reference_vehicle_id, other_actor_id}; + } else { + key_parts = {other_actor_id, reference_vehicle_id}; + } + + uint64_t actor_id_key = 0u; + actor_id_key |= key_parts.first; + actor_id_key <<= 32; + actor_id_key |= key_parts.second; + + GeometryComparison comparision_result{-1.0, -1.0, -1.0, -1.0}; + + if (geometry_cache.find(actor_id_key) != geometry_cache.end()) { + + comparision_result = geometry_cache.at(actor_id_key); + double mref_veh_other = comparision_result.reference_vehicle_to_other_geodesic; + comparision_result.reference_vehicle_to_other_geodesic = comparision_result.other_vehicle_to_reference_geodesic; + comparision_result.other_vehicle_to_reference_geodesic = mref_veh_other; + } else { + + const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle_id)); + const Polygon other_polygon = GetPolygon(GetBoundary(other_actor_id)); + + const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle_id)); + + const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_actor_id)); + + const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon); + const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon); + const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon); + const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); + + comparision_result = {reference_vehicle_to_other_geodesic, + other_vehicle_to_reference_geodesic, + inter_geodesic_distance, + inter_bbox_distance}; + + geometry_cache.insert({actor_id_key, comparision_result}); + } + + return comparision_result; +} + +std::pair CollisionStage::NegotiateCollision(const ActorId reference_vehicle_id, + const ActorId other_actor_id, + const uint64_t reference_junction_look_ahead_index) { + // Output variables for the method. + bool hazard = false; + float available_distance_margin = std::numeric_limits::infinity(); + + const cg::Location reference_location = simulation_state.GetLocation(reference_vehicle_id); + const cg::Location other_location = simulation_state.GetLocation(other_actor_id); + + // Ego and other vehicle heading. + const cg::Vector3D reference_heading = simulation_state.GetHeading(reference_vehicle_id); + // Vector from ego position to position of the other vehicle. + const float vector_magnitude_epsilon = 2.0f * std::numeric_limits::epsilon(); + cg::Vector3D reference_to_other = other_location - reference_location; + reference_to_other = reference_to_other.MakeSafeUnitVector(vector_magnitude_epsilon); + + // Other vehicle heading. + const cg::Vector3D other_heading = simulation_state.GetHeading(other_actor_id); + // Vector from other vehicle position to ego position. + cg::Vector3D other_to_reference = reference_location - other_location; + other_to_reference = other_to_reference.MakeSafeUnitVector(vector_magnitude_epsilon); + + float reference_vehicle_length = simulation_state.GetDimensions(reference_vehicle_id).x * SQUARE_ROOT_OF_TWO; + float other_vehicle_length = simulation_state.GetDimensions(other_actor_id).x * SQUARE_ROOT_OF_TWO; + + float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location); + float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id); + float other_bounding_box_extension = GetBoundingBoxExtention(other_actor_id); + // Calculate minimum distance between vehicle to consider collision negotiation. + float inter_vehicle_length = reference_vehicle_length + other_vehicle_length; + float ego_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length); + float cross_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length + other_bounding_box_extension); + + // Conditions to consider collision negotiation. + bool other_vehicle_in_ego_range = inter_vehicle_distance < ego_detection_range; + bool other_vehicles_in_cross_detection_range = inter_vehicle_distance < cross_detection_range; + float reference_heading_to_other_dot = cg::Math::Dot(reference_heading, reference_to_other); + bool other_vehicle_in_front = reference_heading_to_other_dot > 0; + const Buffer &reference_vehicle_buffer = buffer_map.at(reference_vehicle_id); + SimpleWaypointPtr closest_point = reference_vehicle_buffer.front(); + bool ego_inside_junction = closest_point->CheckJunction(); + TrafficLightState reference_tl_state = simulation_state.GetTLS(reference_vehicle_id); + bool ego_at_traffic_light = reference_tl_state.at_traffic_light; + bool ego_stopped_by_light = reference_tl_state.tl_state != TLS::Green; + SimpleWaypointPtr look_ahead_point = reference_vehicle_buffer.at(reference_junction_look_ahead_index); + bool ego_at_junction_entrance = !closest_point->CheckJunction() && look_ahead_point->CheckJunction(); + + // Conditions to consider collision negotiation. + if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light) + && ((ego_inside_junction && other_vehicles_in_cross_detection_range) + || (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) { + GeometryComparison geometry_comparison = GetGeometryBetweenActors(reference_vehicle_id, other_actor_id); + + // Conditions for collision negotiation. + bool geodesic_path_bbox_touching = geometry_comparison.inter_geodesic_distance < 0.1; + bool vehicle_bbox_touching = geometry_comparison.inter_bbox_distance < 0.1; + bool ego_path_clear = geometry_comparison.other_vehicle_to_reference_geodesic > 0.1; + bool other_path_clear = geometry_comparison.reference_vehicle_to_other_geodesic > 0.1; + bool ego_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic < geometry_comparison.other_vehicle_to_reference_geodesic; + bool other_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic > geometry_comparison.other_vehicle_to_reference_geodesic; + bool ego_angular_priority = reference_heading_to_other_dot< cg::Math::Dot(other_heading, other_to_reference); + + // Whichever vehicle's path is farthest away from the other vehicle gets priority to move. + bool lower_priority = !ego_path_priority && (other_path_priority || !ego_angular_priority); + bool blocked_by_other_or_lower_priority = !ego_path_clear || (other_path_clear && lower_priority); + bool yield_pre_crash = !vehicle_bbox_touching && blocked_by_other_or_lower_priority; + bool yield_post_crash = vehicle_bbox_touching && !ego_angular_priority; + + if (geodesic_path_bbox_touching && (yield_pre_crash || yield_post_crash)) { + + hazard = true; + + const float reference_lead_distance = parameters.GetDistanceToLeadingVehicle(reference_vehicle_id); + const float specific_distance_margin = std::max(reference_lead_distance, BOUNDARY_EXTENSION_MINIMUM); + available_distance_margin = static_cast(std::max(geometry_comparison.reference_vehicle_to_other_geodesic + - static_cast(specific_distance_margin), 0.0)); + + ///////////////////////////////////// Collision locking mechanism ///////////////////////////////// + // The idea is, when encountering a possible collision, + // we should ensure that the bounding box extension doesn't decrease too fast and loose collision tracking. + // This enables us to smoothly approach the lead vehicle. + + // When possible collision found, check if an entry for collision lock present. + if (collision_locks.find(reference_vehicle_id) != collision_locks.end()) { + CollisionLock &lock = collision_locks.at(reference_vehicle_id); + // Check if the same vehicle is under lock. + if (other_actor_id == lock.lead_vehicle_id) { + // If the body of the lead vehicle is touching the reference vehicle bounding box. + if (geometry_comparison.other_vehicle_to_reference_geodesic < 0.1) { + // Distance between the bodies of the vehicles. + lock.distance_to_lead_vehicle = geometry_comparison.inter_bbox_distance; + } else { + // Distance from reference vehicle body to other vehicle path polygon. + lock.distance_to_lead_vehicle = geometry_comparison.reference_vehicle_to_other_geodesic; + } + } else { + // If possible collision with a new vehicle, re-initialize with new lock entry. + lock = {geometry_comparison.inter_bbox_distance, geometry_comparison.inter_bbox_distance, other_actor_id}; + } + } else { + // Insert and initialize lock entry if not present. + collision_locks.insert({reference_vehicle_id, + {geometry_comparison.inter_bbox_distance, + geometry_comparison.inter_bbox_distance, + other_actor_id}}); } } - - return bbox_extension; } - LocationList CollisionStage::GetBoundary(const Actor &actor, - const cg::Location &location, - const cg::Vector3D velocity) { - - const auto actor_type = actor->GetTypeId(); - cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); - - cg::BoundingBox bbox; - float forward_extension = 0.0f; - if (actor_type[0] == 'v') { - const auto vehicle = boost::static_pointer_cast(actor); - bbox = vehicle->GetBoundingBox(); - } else if (actor_type[0] == 'w') { - const auto walker = boost::static_pointer_cast(actor); - bbox = walker->GetBoundingBox(); - // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions. - forward_extension = velocity.Length() * WALKER_TIME_EXTENSION; - } - - const cg::Vector3D extent = bbox.extent; - const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, heading_vector.z); - - const cg::Vector3D x_boundary_vector = heading_vector * (extent.x + forward_extension); - const cg::Vector3D y_boundary_vector = perpendicular_vector * (extent.y + forward_extension); - - // Four corners of the vehicle in top view clockwise order (left-handed - // system). - LocationList bbox_boundary = { - location + cg::Location(x_boundary_vector - y_boundary_vector), - location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector), - location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector), - location + cg::Location(x_boundary_vector + y_boundary_vector), - }; - - return bbox_boundary; + // If no collision hazard detected, then flush collision lock held by the vehicle. + if (!hazard && collision_locks.find(reference_vehicle_id) != collision_locks.end()) { + collision_locks.erase(reference_vehicle_id); } - bool CollisionStage::IsLocationAfterJunctionSafe(const Actor &ego_actor, - const Actor &other_actor, - const SimpleWaypointPtr safe_point, - const cg::Location &other_location, - const cg::Vector3D other_velocity){ + return {hazard, available_distance_margin}; +} - bool safe_junction = true; +void CollisionStage::ClearCycleCache() { + geodesic_boundary_map.clear(); + geometry_cache.clear(); +} - if (other_velocity.Length() < EPSILON_VELOCITY){ - - cg::Location safe_location = safe_point->GetLocation(); - cg::Vector3D heading_vector = safe_point->GetForwardVector(); - - cg::BoundingBox bbox; - const auto vehicle = boost::static_pointer_cast(ego_actor); - bbox = vehicle->GetBoundingBox(); - const cg::Vector3D extent = bbox.extent; - - const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f); - - const cg::Vector3D x_boundary_vector = heading_vector * extent.x; - const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y; - - LocationList ego_actor_boundary = { - safe_location + cg::Location(x_boundary_vector - y_boundary_vector), - safe_location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector), - safe_location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector), - safe_location + cg::Location(x_boundary_vector + y_boundary_vector), - }; - - const Polygon reference_polygon = GetPolygon(ego_actor_boundary); - const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location, other_velocity)); - - const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); - if (inter_bbox_distance < INTER_BBOX_DISTANCE_THRESHOLD){ - safe_junction = false; - } - } - - return safe_junction; - } - - void CollisionStage::DrawBoundary(const LocationList &boundary) { - for (uint64_t i = 0u; i < boundary.size(); ++i) { - debug_helper.DrawLine( - boundary[i] + cg::Location(0.0f, 0.0f, 1.0f), - boundary[(i + 1) % boundary.size()] + cg::Location(0.0f, 0.0f, 1.0f), - 0.1f, {255u, 255u, 0u}, 0.05f); - } - } - -GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle, - const cg::Location &reference_location, const cg::Location &other_location, - const cg::Vector3D reference_velocity, const cg::Vector3D other_velocity) { - - std::string actor_id_key = (reference_vehicle->GetId() < other_vehicle->GetId()) ? - std::to_string(reference_vehicle->GetId()) + "|" + std::to_string(other_vehicle->GetId()) - : std::to_string(other_vehicle->GetId()) +"|"+ std::to_string(reference_vehicle->GetId()); - GeometryComparisonCache mCache{-1,-1,-1,-1}; - - if (vehicle_cache.find(actor_id_key) != vehicle_cache.end()) { - mCache = vehicle_cache.at(actor_id_key); - double mref_veh_other = mCache.reference_vehicle_to_other_geodesic; - mCache.reference_vehicle_to_other_geodesic = mCache.other_vehicle_to_reference_geodesic; - mCache.other_vehicle_to_reference_geodesic = mref_veh_other; - return mCache; - } - - const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location, reference_velocity)); - const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location, other_velocity)); - const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location, reference_velocity)); - const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location, other_velocity)); - - const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon); - const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon); - const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon); - const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon); - - GeometryComparisonCache mRetCache = {reference_vehicle_to_other_geodesic, - other_vehicle_to_reference_geodesic, - inter_geodesic_distance, - inter_bbox_distance}; - - vehicle_cache.insert({actor_id_key, mRetCache}); - - return mRetCache; +void CollisionStage::DrawBoundary(const LocationVector &boundary) { + cg::Location one_meter_up(0.0f, 0.0f, 1.0f); + for (uint64_t i = 0u; i < boundary.size(); ++i) { + debug_helper.DrawLine( + boundary[i] + one_meter_up, + boundary[i+1 == boundary.size()? 0: i+1] + one_meter_up, + 0.1f, {255u, 255u, 0u}, 0.05f); } +} } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index b0c92bf89..f3c9b31d8 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -1,167 +1,109 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include #include "boost/geometry.hpp" #include "boost/geometry/geometries/geometries.hpp" #include "boost/geometry/geometries/point_xy.hpp" #include "boost/geometry/geometries/polygon.hpp" -#include "boost/pointer_cast.hpp" -#include "carla/client/ActorList.h" -#include "carla/client/Vehicle.h" -#include "carla/client/Walker.h" -#include "carla/client/World.h" -#include "carla/geom/Location.h" -#include "carla/geom/Math.h" -#include "carla/geom/Vector3D.h" -#include "carla/Logging.h" -#include "carla/rpc/ActorId.h" -#include "carla/rpc/TrafficLightState.h" -#include "carla/trafficmanager/MessengerAndDataTypes.h" +#include "carla/client/DebugHelper.h" + +#include "carla/trafficmanager/DataStructures.h" #include "carla/trafficmanager/Parameters.h" -#include "carla/trafficmanager/PipelineStage.h" +#include "carla/trafficmanager/RandomGenerator.h" +#include "carla/trafficmanager/SimulationState.h" +#include "carla/trafficmanager/Stage.h" namespace carla { namespace traffic_manager { - namespace cc = carla::client; - namespace cg = carla::geom; - namespace chr = std::chrono; - namespace bg = boost::geometry; +struct GeometryComparison { + double reference_vehicle_to_other_geodesic; + double other_vehicle_to_reference_geodesic; + double inter_geodesic_distance; + double inter_bbox_distance; +}; - using ActorId = carla::ActorId; - using Actor = carla::SharedPtr; - using Point2D = bg::model::point; - using Polygon = bg::model::polygon>; - using LocationList = std::vector; - using SimpleWaypointPtr = std::shared_ptr; - using TLS = carla::rpc::TrafficLightState; +struct CollisionLock { + double distance_to_lead_vehicle; + double initial_lock_distance; + ActorId lead_vehicle_id; +}; +using CollisionLockMap = std::unordered_map; +namespace cc = carla::client; +namespace bg = boost::geometry; - /// Structure to hold the Geometry of reference vehicle to other vehicle. - struct GeometryComparisonCache { - - double reference_vehicle_to_other_geodesic; - double other_vehicle_to_reference_geodesic; - double inter_geodesic_distance; - double inter_bbox_distance; - }; - - /// Structure to hold data about collision locking with a lead vehicle. - struct CollisionLock { - - ActorId lead_vehicle_id; - double distance_to_lead_vehicle; - double initial_lock_distance; - }; - -/// This class is the thread executable for the collision detection stage -/// and is responsible for checking possible collisions with other -/// cars along the vehicle's trajectory. -class CollisionStage : public PipelineStage { +using Buffer = std::deque>; +using BufferMap = std::unordered_map; +using LocationVector = std::vector; +using GeodesicBoundaryMap = std::unordered_map; +using GeometryComparisonMap = std::unordered_map; +using Polygon = bg::model::polygon>; +/// This class has functionality to detect potential collision with a nearby actor. +class CollisionStage : Stage { private: - - /// Geometry data for the vehicle - std::unordered_map vehicle_cache; - /// Selection key for switching between output frames. - bool frame_selector; - /// Pointer to data received from localization stage. - std::shared_ptr localization_frame; - /// Pointers to output frames to be shared with motion planner stage. - std::shared_ptr planner_frame_a; - std::shared_ptr planner_frame_b; - /// Pointers to messenger objects. - std::shared_ptr localization_messenger; - std::shared_ptr planner_messenger; - /// Runtime parameterization object. - Parameters ¶meters; - /// Reference to Carla's debug helper object. + const std::vector &vehicle_id_list; + const SimulationState &simulation_state; + const BufferMap &buffer_map; + const TrackTraffic &track_traffic; + const Parameters ¶meters; + CollisionFrame &output_array; cc::DebugHelper &debug_helper; - /// The map used to connect actor ids to the array index of data frames. - std::unordered_map vehicle_id_to_index; - /// An object used to keep track of time between checking for all world - /// actors. - chr::time_point last_world_actors_pass_instance; - /// Number of vehicles registered with the traffic manager. - uint64_t number_of_vehicles; - /// Structure to hold the geodesic boundaries during one iteration. - std::unordered_map geodesic_boundaries; - /// Structure to keep track of collision locking. - std::unordered_map collision_locks; - /// Snippet profiler for measuring execution time. - SnippetProfiler snippet_profiler; + // Structure keeping track of blocking lead vehicles. + CollisionLockMap collision_locks; + // Structures to cache geodesic boundaries of vehicle and + // comparision between vehicle boundaries + // to avoid repeated computation within a cycle. + GeometryComparisonMap geometry_cache; + GeodesicBoundaryMap geodesic_boundary_map; + RandomGenerator<> pgen; - /// Returns the bounding box corners of the vehicle passed to the method. - LocationList GetBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity); + // Method to determine if a vehicle is on a collision path to another. + std::pair NegotiateCollision(const ActorId reference_vehicle_id, + const ActorId other_actor_id, + const uint64_t reference_junction_look_ahead_index); - /// Returns the extrapolated bounding box of the vehicle along its - /// trajectory. - LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity); + // Method to calculate bounding box extention length ahead of the vehicle. + float GetBoundingBoxExtention(const ActorId actor_id); - /// Method to construct a boost polygon object. - Polygon GetPolygon(const LocationList &boundary); + // Method to calculate polygon points around the vehicle's bounding box. + LocationVector GetBoundary(const ActorId actor_id); - /// The method returns true if ego_vehicle should stop and wait for - /// other_vehicle to pass. - std::pair NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle, - const cg::Location &reference_location, - const cg::Location &other_location, - const SimpleWaypointPtr& closest_point, - const SimpleWaypointPtr& junction_look_ahead, - const cg::Vector3D reference_velocity, - const cg::Vector3D other_velocity); + // Method to construct polygon points around the path boundary of the vehicle. + LocationVector GetGeodesicBoundary(const ActorId actor_id); - /// Method to calculate the speed dependent bounding box extention for a vehicle. - float GetBoundingBoxExtention(const ActorId actor_id, - const cg::Vector3D velocity_vector, - const cg::Vector3D heading_vector); + Polygon GetPolygon(const LocationVector &boundary); - /// At intersections, used to see if there is space after the junction - bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor, - const SimpleWaypointPtr safe_point, const cg::Location &other_location, - const cg::Vector3D other_velocity); + // Method to compare path boundaries, bounding boxes of vehicles + // and cache the results for reuse in current update cycle. + GeometryComparison GetGeometryBetweenActors(const ActorId reference_vehicle_id, + const ActorId other_actor_id); - /// A simple method used to draw bounding boxes around vehicles - void DrawBoundary(const LocationList &boundary); - - /// Method to compute Geometry result between two vehicles - GeometryComparisonCache GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle, - const cg::Location &reference_location, const cg::Location &other_location, - const cg::Vector3D reference_velocity,const cg::Vector3D other_velocity); + // Method to draw path boundary. + void DrawBoundary(const LocationVector &boundary); public: + CollisionStage(const std::vector &vehicle_id_list, + const SimulationState &simulation_state, + const BufferMap &buffer_map, + const TrackTraffic &track_traffic, + const Parameters ¶meters, + CollisionFrame &output_array, + cc::DebugHelper& debug_helper); -CollisionStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr planner_messenger, - Parameters ¶meters, - cc::DebugHelper &debug_helper); + void Update (const unsigned long index) override; - ~CollisionStage(); + void RemoveActor(const ActorId actor_id) override; - void DataReceiver() override; + void Reset() override; - void Action() override; - - void DataSender() override; - - }; + // Method to flush cache for current update cycle. + void ClearCycleCache(); +}; } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/Constants.h b/LibCarla/source/carla/trafficmanager/Constants.h new file mode 100644 index 000000000..4e6fca74a --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/Constants.h @@ -0,0 +1,132 @@ + +/// This file contains various constants used in traffic manager +/// arranged into sensible namespaces for re-usability across +/// different files. + +#pragma once + +#include +#include + +#define SQUARE(a) ((a) * (a)) +#define RATE(MaxY, MinY, DiffX) (((MaxY) - (MinY)) / (DiffX)) + +namespace carla { +namespace traffic_manager { +namespace constants { + +namespace Networking { +static const uint64_t MIN_TRY_COUNT = 20u; +static const unsigned short TM_DEFAULT_PORT = 8000u; +static const int64_t TM_TIMEOUT = 2000; // ms +} // namespace Networking + +namespace VehicleRemoval { +static const float STOPPED_VELOCITY_THRESHOLD = 0.8f; +static const double BLOCKED_TIME_THRESHOLD = 90.0; +static const double DELTA_TIME_BETWEEN_DESTRUCTIONS = 10.0; +} // namespace VehicleRemoval + +namespace HybridMode { +static const float HYBRID_MODE_DT = 0.05f; +static const float INV_HYBRID_DT = 1.0f / HYBRID_MODE_DT; +static const float PHYSICS_RADIUS = 50.0f; +} // namespace HybridMode + +namespace SpeedThreshold { +static const float HIGHWAY_SPEED = 50.0f / 3.6f; +static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f; +static const float AFTER_JUNCTION_MIN_SPEED = 5.0f / 3.6f; +} // namespace SpeedThreshold + +namespace PathBufferUpdate { +static const float MAX_START_DISTANCE = 30.0f; +static const float MINIMUM_HORIZON_LENGTH = 30.0f; +static const float MAXIMUM_HORIZON_LENGTH = 60.0f; +static const float HORIZON_RATE = RATE(MAXIMUM_HORIZON_LENGTH, + MINIMUM_HORIZON_LENGTH, + SpeedThreshold::ARBITRARY_MAX_SPEED); +} // namespace PathBufferUpdate + +namespace WaypointSelection { +static const float TARGET_WAYPOINT_TIME_HORIZON = 1.0f; +static const float TARGET_WAYPOINT_HORIZON_LENGTH = 2.5f; +static const float JUNCTION_LOOK_AHEAD = 10.0f; +static const float SAFE_DISTANCE_AFTER_JUNCTION = 10.0f; +static const float MIN_JUNCTION_LENGTH = 5.0f; +static const float MIN_SAFE_INTERVAL_LENGTH = 0.9f * SAFE_DISTANCE_AFTER_JUNCTION; +} // namespace WaypointSelection + +namespace LaneChange { +static const float MINIMUM_LANE_CHANGE_DISTANCE = 15.0f; +static const float MAXIMUM_LANE_OBSTACLE_DISTANCE = 50.0f; +static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f; +static const float INTER_LANE_CHANGE_DISTANCE = 10.0f; +} // namespace LaneChange + +namespace Collision { +static const float BOUNDARY_EXTENSION_MAXIMUM = 50.0f; +static const float BOUNDARY_EXTENSION_MINIMUM = 2.0f; +static const float BOUNDARY_EXTENSION_RATE = RATE(BOUNDARY_EXTENSION_MAXIMUM, + BOUNDARY_EXTENSION_MINIMUM, + SpeedThreshold::ARBITRARY_MAX_SPEED); +static const float COS_10_DEGREES = 0.9848f; +static const float EPSILON_VELOCITY = 0.1f; +static const float LOCKING_DISTANCE_PADDING = 4.0f; +static const float MAX_COLLISION_RADIUS = 100.0f; +static const float MAX_LOCKING_EXTENSION = 10.0f; +static const float WALKER_TIME_EXTENSION = 1.5f; +static const float SQUARE_ROOT_OF_TWO = 1.414f; +static const float VERTICAL_OVERLAP_THRESHOLD = 4.0f; +} // namespace Collision + +namespace FrameMemory { +static const uint64_t INITIAL_SIZE = 50u; +static const uint64_t GROWTH_STEP_SIZE = 50u; +static const float INV_GROWTH_STEP_SIZE = 1.0f / static_cast(GROWTH_STEP_SIZE); +} // namespace FrameMemory + +namespace Map { +static const float INFINITE_DISTANCE = std::numeric_limits::max(); +static const float GRID_SIZE = 4.0f; +static const float PED_GRID_SIZE = 10.0f; +static const float MAX_GEODESIC_GRID_LENGTH = 20.0f; +static const double MAP_RESOLUTION = 0.1; +static const float INV_MAP_RESOLUTION = 10.0f; +} // namespace Map + +namespace TrafficLight { +static const uint64_t NO_SIGNAL_PASSTHROUGH_INTERVAL = 5u; +static const double DOUBLE_NO_SIGNAL_PASSTHROUGH_INTERVAL = 5.0; +} // namespace TrafficLight + +namespace MotionPlan { +static const float RELATIVE_APPROACH_SPEED = 10.0f / 3.6f; +static const float MIN_FOLLOW_LEAD_DISTANCE = 5.0f; +static const float MAX_FOLLOW_LEAD_DISTANCE = 10.0f; +static const float FOLLOW_DISTANCE_RATE = RATE(MAX_FOLLOW_LEAD_DISTANCE, + MIN_FOLLOW_LEAD_DISTANCE, + SpeedThreshold::ARBITRARY_MAX_SPEED); +static const float CRITICAL_BRAKING_MARGIN = 0.25f; +static const float EPSILON_RELATIVE_SPEED = 0.001f; +static const float MAX_JUNCTION_BLOCK_DISTANCE = 0.5f * WaypointSelection::SAFE_DISTANCE_AFTER_JUNCTION; +} // namespace MotionPlan + +namespace PID { +static const float MAX_THROTTLE = 0.7f; +static const float MAX_BRAKE = 1.0f; +static const float VELOCITY_INTEGRAL_MAX = 5.0f; +static const float VELOCITY_INTEGRAL_MIN = -5.0f; +static const float DT = 0.05f; +static const float INV_DT = 1.0f / DT; +} // namespace PID + +namespace TrackTraffic { +static const uint64_t BUFFER_STEP_THROUGH = 10; +static const float INV_BUFFER_STEP_THROUGH = 0.1f; +} // namespace TrackTraffic + + +} // namespace constants +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/DataStructures.h b/LibCarla/source/carla/trafficmanager/DataStructures.h new file mode 100644 index 000000000..9e11d6635 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/DataStructures.h @@ -0,0 +1,71 @@ + +/// This file contains definitions of common data structures used in traffic manager. + +#pragma once + +#include +#include +#include + +#include "carla/client/Actor.h" +#include "carla/geom/Location.h" +#include "carla/geom/Rotation.h" +#include "carla/geom/Vector3D.h" +#include "carla/rpc/ActorId.h" +#include "carla/rpc/Command.h" +#include "carla/rpc/TrafficLightState.h" + +#include "carla/trafficmanager/SimpleWaypoint.h" + +namespace carla { +namespace traffic_manager { + +namespace chr = std::chrono; +namespace cc = carla::client; +namespace cg = carla::geom; + +using ActorId = carla::ActorId; +using ActorPtr = carla::SharedPtr; +using JunctionID = carla::road::JuncId; +using SimpleWaypointPtr = std::shared_ptr; +using Buffer = std::deque; +using BufferMap = std::unordered_map; +using TimeInstance = chr::time_point; +using TLS = carla::rpc::TrafficLightState; + +struct LocalizationData { + SimpleWaypointPtr junction_end_point; + SimpleWaypointPtr safe_point; + bool is_at_junction_entrance; +}; +using LocalizationFrame = std::vector; + +struct CollisionHazardData { + float available_distance_margin; + ActorId hazard_actor_id; + bool hazard; +}; +using CollisionFrame = std::vector; + +using ControlFrame = std::vector; + +using TLFrame = std::vector; + +/// Structure to hold the actuation signals. +struct ActuationSignal { + float throttle; + float brake; + float steer; +}; + +/// Structure to hold the controller state. +struct StateEntry { + TimeInstance time_instance; + float deviation; + float velocity; + float deviation_integral; + float velocity_integral; +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index 8a5480c90..9da0d8e4d 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -4,22 +4,18 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . +#include "carla/trafficmanager/Constants.h" #include "carla/trafficmanager/InMemoryMap.h" namespace carla { namespace traffic_manager { -namespace MapConstants { - - static const float INFINITE_DISTANCE = std::numeric_limits::max(); - static const float GRID_SIZE = 4.0f; - static const float PED_GRID_SIZE = 10.0f; - static const float MAX_GEODESIC_GRID_LENGTH = 20.0f; - static const float TOPOLOGY_DISTANCE = 0.1f; -} // namespace MapConstants namespace cg = carla::geom; - using namespace MapConstants; + using namespace constants::Map; + + using TopologyList = std::vector>; + using RawNodeList = std::vector; InMemoryMap::InMemoryMap(WorldMap world_map) : _world_map(world_map) {} InMemoryMap::~InMemoryMap() {} @@ -71,6 +67,7 @@ namespace MapConstants { void InMemoryMap::SetUp() { // 1. Building segment topology (i.e., defining set of segment predecessors and successors) + assert(_world_map != nullptr && "No map reference found."); auto waypoint_topology = _world_map->GetTopology(); SegmentTopology segment_topology; @@ -84,39 +81,51 @@ namespace MapConstants { // Setting segment predecessors and successors. SegmentId waypoint_segment_id = GetSegmentId(connection.first); SegmentId successor_segment_id = GetSegmentId(connection.second); - segment_topology[waypoint_segment_id].second.push_back(successor_segment_id); - segment_topology[successor_segment_id].first.push_back(waypoint_segment_id); + using SegIdVectorPair = std::pair, std::vector>; + SegIdVectorPair &connection_first = segment_topology[waypoint_segment_id]; + SegIdVectorPair &connection_second = segment_topology[successor_segment_id]; + connection_first.second.push_back(successor_segment_id); + connection_second.first.push_back(waypoint_segment_id); // From path to standard road. - if (waypoint->IsJunction() && !successor->IsJunction()) { + bool waypoint_is_junction = waypoint->IsJunction(); + bool successor_is_junction = successor->IsJunction(); + if (waypoint_is_junction && !successor_is_junction) { crd::RoadId path_id = waypoint->GetRoadId(); int64_t std_road_id = static_cast(successor->GetRoadId()); std_road_id = (successor->GetLaneId() < 0) ? -1 * std_road_id : std_road_id; - std_road_connectivity[std_road_id].first.insert(path_id); - auto &in_paths = std_road_connectivity[std_road_id].first; + std::set &in_paths = std_road_connectivity[std_road_id].first; + in_paths.insert(path_id); + if (in_paths.size() >= 2) { - std::for_each(in_paths.begin(), in_paths.end(), [&is_real_junction](crd::RoadId id) {is_real_junction[id] = true;}); + for (auto &in_path_id: in_paths) { + is_real_junction[in_path_id] = true; + } } } // From standard road to path. - if (!waypoint->IsJunction() && successor->IsJunction()) { + if (!waypoint_is_junction && successor_is_junction) { crd::RoadId path_id = successor->GetRoadId(); int64_t std_road_id = static_cast(waypoint->GetRoadId()); std_road_id = (waypoint->GetLaneId() < 0) ? -1 * std_road_id : std_road_id; - std_road_connectivity[std_road_id].second.insert(path_id); - auto &out_paths = std_road_connectivity[std_road_id].second; + std::set &out_paths = std_road_connectivity[std_road_id].second; + out_paths.insert(path_id); + if (out_paths.size() >= 2) { - std::for_each(out_paths.begin(), out_paths.end(), [&is_real_junction](crd::RoadId id) {is_real_junction[id] = true;}); + for (auto &out_path_id: out_paths) { + is_real_junction[out_path_id] = true; + } } } } // 2. Consuming the raw dense topology from cc::Map into SimpleWaypoints. SegmentMap segment_map; - auto raw_dense_topology = _world_map->GenerateWaypoints(TOPOLOGY_DISTANCE); + 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)); } @@ -180,13 +189,13 @@ namespace MapConstants { for (auto &simple_waypoint: dense_topology) { if (simple_waypoint != nullptr) { const cg::Location loc = simple_waypoint->GetLocation(); - const std::string grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, true)); + const int64_t grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, true)); if (waypoint_grid.find(grid_key) == waypoint_grid.end()) { waypoint_grid.insert({grid_key, {simple_waypoint}}); } else { waypoint_grid.at(grid_key).insert(simple_waypoint); } - const std::string ped_grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, false)); + const int64_t ped_grid_key = MakeGridKey(MakeGridId(loc.x, loc.y, false)); if (ped_waypoint_grid.find(ped_grid_key) == ped_waypoint_grid.end()) { ped_waypoint_grid.insert({ped_grid_key, {simple_waypoint}}); } else { @@ -230,8 +239,6 @@ namespace MapConstants { } } } - - MakeGeodesiGridCenters(); } std::pair InMemoryMap::MakeGridId(float x, float y, bool vehicle_or_pedestrian) { @@ -243,8 +250,14 @@ namespace MapConstants { } } - std::string InMemoryMap::MakeGridKey(std::pair grid_key) { - return std::to_string(grid_key.first) + "#" + std::to_string(grid_key.second); + int64_t InMemoryMap::MakeGridKey(std::pair grid_id) { + + int64_t grid_key = 0; + grid_key |= grid_id.first; + grid_key <<= 32; + grid_key |= grid_id.second; + + return grid_key; } SimpleWaypointPtr InMemoryMap::GetWaypointInVicinity(cg::Location location) { @@ -257,7 +270,7 @@ namespace MapConstants { for (int i = -1; i <= 1; ++i) { for (int j = -1; j <= 1; ++j) { - const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j}); + const int64_t grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j}); if (waypoint_grid.find(grid_key) != waypoint_grid.end()) { const auto &waypoint_set = waypoint_grid.at(grid_key); @@ -267,7 +280,7 @@ namespace MapConstants { for (auto &simple_waypoint: waypoint_set) { - if (simple_waypoint->DistanceSquared(location) < std::pow(closest_distance, 2)) { + if (simple_waypoint->DistanceSquared(location) < SQUARE(closest_distance)) { closest_waypoint = simple_waypoint; closest_distance = simple_waypoint->DistanceSquared(location); } @@ -278,7 +291,7 @@ namespace MapConstants { // Return the closest waypoint in the surrounding grids // only if it is in the same horizontal plane as the requested location. - if (closest_waypoint != nullptr && std::abs(closest_waypoint->GetLocation().z - location.z) > 1.0) { + if (closest_waypoint != nullptr && std::abs(closest_waypoint->GetLocation().z - location.z) > 1.0f) { closest_waypoint = nullptr; } @@ -295,7 +308,7 @@ namespace MapConstants { for (int i = -1; i <= 1; ++i) { for (int j = -1; j <= 1; ++j) { - const std::string grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j}); + const int64_t grid_key = MakeGridKey({grid_ids.first + i, grid_ids.second + j}); if (ped_waypoint_grid.find(grid_key) != ped_waypoint_grid.end()) { const auto &waypoint_set = ped_waypoint_grid.at(grid_key); @@ -417,26 +430,9 @@ namespace MapConstants { } } - void InMemoryMap::MakeGeodesiGridCenters() { - for (auto &swp: dense_topology) { - GeoGridId ggid = swp->CheckJunction() ? swp->GetJunctionId() : swp->GetGeodesicGridId(); - if (geodesic_grid_center.find(ggid) == geodesic_grid_center.end()) { - geodesic_grid_center.insert({ggid, swp->GetLocation()}); - } else { - cg::Location &grid_loc = geodesic_grid_center.at(ggid); - grid_loc = (grid_loc + swp->GetLocation()) / 2; - } - } - } - - cg::Location InMemoryMap::GetGeodesicGridCenter(GeoGridId ggid) { - cg::Location grid_center; - if (geodesic_grid_center.find(ggid) != geodesic_grid_center.end()) { - grid_center = geodesic_grid_center.at(ggid); - } else { - grid_center = cg::Location(); - } - return grid_center; + std::string InMemoryMap::GetMapName() { + assert(_world_map != nullptr && "No map reference found."); + return _world_map->GetName(); } } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.h b/LibCarla/source/carla/trafficmanager/InMemoryMap.h index a27f444f9..aaa21e6c4 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.h +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.h @@ -6,19 +6,15 @@ #pragma once -#include -#include -#include #include -#include #include +#include #include "carla/client/Map.h" #include "carla/client/Waypoint.h" #include "carla/geom/Location.h" #include "carla/geom/Math.h" #include "carla/Memory.h" -#include "carla/road/Lane.h" #include "carla/road/RoadTypes.h" #include "carla/trafficmanager/SimpleWaypoint.h" @@ -31,13 +27,11 @@ namespace cc = carla::client; namespace crd = carla::road; using WaypointPtr = carla::SharedPtr; - using TopologyList = std::vector>; using SimpleWaypointPtr = std::shared_ptr; using NodeList = std::vector; - using RawNodeList = std::vector; using GeoGridId = crd::JuncId; using WorldMap = carla::SharedPtr; - using WaypointGrid = std::unordered_map>; + using WaypointGrid = std::unordered_map>; using SegmentId = std::tuple; using SegmentTopology = std::map, std::vector>>; @@ -71,12 +65,6 @@ namespace crd = carla::road; /// sampling_resolution. void SetUp(); - /// Computes the segment id of a given waypoint. - /// - /// The Id takes into account OpenDrive's road Id, lane Id and Section Id. - SegmentId GetSegmentId(const WaypointPtr &wp) const; - SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const; - /// This method returns the closest waypoint to a given location on the map. SimpleWaypointPtr GetWaypoint(const cg::Location &location) const; @@ -88,8 +76,7 @@ namespace crd = carla::road; /// local cache. NodeList GetDenseTopology() const; - void MakeGeodesiGridCenters(); - cg::Location GetGeodesicGridCenter(GeoGridId ggid); + std::string GetMapName(); private: @@ -97,7 +84,7 @@ namespace crd = carla::road; std::pair MakeGridId(float x, float y, bool vehicle_or_pedestrian); /// Method to generate map key for waypoint_grid. - std::string MakeGridKey(std::pair gird_id); + int64_t MakeGridKey(std::pair gird_id); /// This method is used to find and place lane change links. void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint); @@ -106,6 +93,11 @@ namespace crd = carla::road; const SegmentTopology &segment_topology, const SegmentMap &segment_map); std::vector GetPredecessors(const SegmentId segment_id, const SegmentTopology &segment_topology, const SegmentMap &segment_map); + + /// Computes the segment id of a given waypoint. + /// The Id takes into account OpenDrive's road Id, lane Id and Section Id. + SegmentId GetSegmentId(const WaypointPtr &wp) const; + SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const; }; } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index ec55694b9..608cab68c 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -1,1024 +1,410 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . + +#include "carla/trafficmanager/Constants.h" #include "carla/trafficmanager/LocalizationStage.h" -#include "carla/client/DebugHelper.h" namespace carla { namespace traffic_manager { -namespace LocalizationConstants { +using namespace constants::PathBufferUpdate; +using namespace constants::LaneChange; +using namespace constants::WaypointSelection; - static const float MINIMUM_HORIZON_LENGTH = 30.0f; - static const float MAXIMUM_HORIZON_LENGTH = 60.0f; - static const float TARGET_WAYPOINT_TIME_HORIZON = 1.0f; - static const float TARGET_WAYPOINT_HORIZON_LENGTH = 5.0f; - static const float MINIMUM_JUNCTION_LOOK_AHEAD = 10.0f; - static const float HIGHWAY_SPEED = 50.0f / 3.6f; - static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f; - static const float HORIZON_RATE = (MAXIMUM_HORIZON_LENGTH - MINIMUM_HORIZON_LENGTH) / ARBITRARY_MAX_SPEED; - static const float MINIMUM_LANE_CHANGE_DISTANCE = 10.0f; - static const float MAXIMUM_LANE_OBSTACLE_DISTANCE = 50.0f; - static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f; - static const uint64_t UNREGISTERED_ACTORS_SCAN_INTERVAL = 10; - static const float BLOCKED_TIME_THRESHOLD = 90.0f; - static const float DELTA_TIME_BETWEEN_DESTRUCTIONS = 10.0f; - static const float STOPPED_VELOCITY_THRESHOLD = 0.8f; // meters per second. - static const float INTER_LANE_CHANGE_DISTANCE = 10.0f; - static const float MAX_COLLISION_RADIUS = 100.0f; - static const float POSITION_WINDOW_SIZE = 2.1f; - static const float HYBRID_MODE_DT = 0.05f; -} // namespace LocalizationConstants +LocalizationStage::LocalizationStage( + const std::vector &vehicle_id_list, + BufferMap &buffer_map, + const SimulationState &simulation_state, + TrackTraffic &track_traffic, + const LocalMapPtr &local_map, + Parameters ¶meters, + LocalizationFrame &output_array, + cc::DebugHelper &debug_helper) + : vehicle_id_list(vehicle_id_list), + buffer_map(buffer_map), + simulation_state(simulation_state), + track_traffic(track_traffic), + local_map(local_map), + parameters(parameters), + output_array(output_array), + debug_helper(debug_helper) {} - using namespace LocalizationConstants; +void LocalizationStage::Update(const unsigned long index) { - LocalizationStage::LocalizationStage( - std::string stage_name, - std::shared_ptr planner_messenger, - std::shared_ptr collision_messenger, - std::shared_ptr traffic_light_messenger, - AtomicActorSet ®istered_actors, - InMemoryMap &local_map, - Parameters ¶meters, - carla::client::DebugHelper &debug_helper, - carla::client::detail::EpisodeProxy &episodeProxy) - : PipelineStage(stage_name), - planner_messenger(planner_messenger), - collision_messenger(collision_messenger), - traffic_light_messenger(traffic_light_messenger), - registered_actors(registered_actors), - local_map(local_map), - parameters(parameters), - debug_helper(debug_helper), - episode_proxy_ls(episodeProxy) { + const ActorId actor_id = vehicle_id_list.at(index); + const cg::Location vehicle_location = simulation_state.GetLocation(actor_id); + const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id); + const cg::Vector3D vehicle_velocity_vector = simulation_state.GetVelocity(actor_id); + const float vehicle_speed = vehicle_velocity_vector.Length(); - // Initializing various output frame selectors. - planner_frame_selector = true; - collision_frame_selector = true; - traffic_light_frame_selector = true; - // Initializing the number of vehicles to zero in the begining. - number_of_vehicles = 0u; - // Initializing the registered actors container state. - registered_actors_state = -1; - // Initializing buffer lists. - buffer_list = std::make_shared(); - // Initializing maximum idle time to null. - maximum_idle_time = std::make_pair(nullptr, 0.0); - // Initializing srand. - srand(static_cast(time(NULL))); - // Initializing kinematic update clock. - previous_update_instance = chr::system_clock::now(); + // Speed dependent waypoint horizon length. + float horizon_length = std::min(vehicle_speed * HORIZON_RATE + MINIMUM_HORIZON_LENGTH, MAXIMUM_HORIZON_LENGTH); + const float horizon_square = SQUARE(horizon_length); + + if (buffer_map.find(actor_id) == buffer_map.end()) { + buffer_map.insert({actor_id, Buffer()}); } - LocalizationStage::~LocalizationStage() {} + Buffer &waypoint_buffer = buffer_map.at(actor_id); - void LocalizationStage::Action() { + // Clear buffer if vehicle is too far from the first waypoint in the buffer. + if (!waypoint_buffer.empty() && + cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), + vehicle_location) > SQUARE(MAX_START_DISTANCE)) { - ScanUnregisteredVehicles(); + auto number_of_pops = waypoint_buffer.size(); + for (uint64_t j = 0u; j < number_of_pops; ++j) { + PopWaypoint(actor_id, track_traffic, waypoint_buffer); + } + } - UpdateSwarmVelocities(); + bool is_at_junction_entrance = false; - // Selecting output frames based on selector keys. - const auto current_planner_frame = planner_frame_selector ? planner_frame_a : planner_frame_b; - const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b; - const auto current_traffic_light_frame = - traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b; - - // Selecting current timestamp from the world snapshot. - current_timestamp = episode_proxy_ls.Lock()->GetWorldSnapshot().GetTimestamp(); - - // Looping over registered actors. - for (uint64_t i = 0u; i < actor_list.size(); ++i) { - - const Actor vehicle = actor_list.at(i); - const ActorId actor_id = vehicle->GetId(); - const cg::Location vehicle_location = vehicle->GetLocation(); - const cg::Vector3D vehicle_velocity_vector = GetVelocity(actor_id); - const float vehicle_velocity = vehicle_velocity_vector.Length(); - - // Initializing idle times. - if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0) { - idle_time[actor_id] = current_timestamp.elapsed_seconds; - } - - // Speed dependent waypoint horizon length. - const float horizon_square = std::min(std::pow(vehicle_velocity * HORIZON_RATE + MINIMUM_HORIZON_LENGTH, 2.0f), - std::pow(MAXIMUM_HORIZON_LENGTH, 2.0f)); - - if (buffer_list->find(actor_id) == buffer_list->end()) { - buffer_list->insert({actor_id, Buffer()}); - } - - Buffer &waypoint_buffer = buffer_list->at(actor_id); - - // Clear buffer if vehicle is too far from the first waypoint in the buffer. - if (!waypoint_buffer.empty() && - cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), vehicle_location) > std::pow(30.0f, 2)) { - - auto number_of_pops = waypoint_buffer.size(); - for (uint64_t j = 0u; j < number_of_pops; ++j) { - PopWaypoint(waypoint_buffer, actor_id); - } - } + if (!waypoint_buffer.empty()) { + // Purge passed waypoints. + float dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation()); + while (dot_product <= 0.0f && !waypoint_buffer.empty()) { + PopWaypoint(actor_id, track_traffic, waypoint_buffer); if (!waypoint_buffer.empty()) { - // Purge passed waypoints. - float dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation()); - while (dot_product <= 0.0f && !waypoint_buffer.empty()) { + dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation()); + } + } - PopWaypoint(waypoint_buffer, actor_id); - if (!waypoint_buffer.empty()) { - dot_product = DeviationDotProduct(vehicle, vehicle_location, waypoint_buffer.front()->GetLocation()); - } + if (!waypoint_buffer.empty()) { + // Determine if the vehicle is at the entrance of a junction. + SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first; + is_at_junction_entrance = !waypoint_buffer.front()->CheckJunction() && look_ahead_point->CheckJunction(); + if (is_at_junction_entrance + // Exception for roundabout in Town03. + && local_map->GetMapName() == "Town03" + && vehicle_location.SquaredLength() < SQUARE(30)) { + is_at_junction_entrance = false; + } + } + + // Purge waypoints too far from the front of the buffer. + while (!is_at_junction_entrance + && !waypoint_buffer.empty() + && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square) { + PopWaypoint(actor_id, track_traffic, waypoint_buffer, false); + } + } + + // Initializing buffer if it is empty. + if (waypoint_buffer.empty()) { + SimpleWaypointPtr closest_waypoint = local_map->GetWaypointInVicinity(vehicle_location); + if (closest_waypoint == nullptr) { + closest_waypoint = local_map->GetWaypoint(vehicle_location); + } + PushWaypoint(actor_id, track_traffic, waypoint_buffer, closest_waypoint); + } + + // Assign a lane change. + const ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(actor_id); + bool force_lane_change = lane_change_info.change_lane; + bool lane_change_direction = lane_change_info.direction; + + if (!force_lane_change) { + float perc_keep_right = parameters.GetKeepRightPercentage(actor_id); + if (perc_keep_right >= 0.0f && perc_keep_right >= pgen.next()) { + force_lane_change = true; + lane_change_direction = true; + } + } + + const SimpleWaypointPtr front_waypoint = waypoint_buffer.front(); + const float lane_change_distance = SQUARE(std::max(10.0f * vehicle_speed, INTER_LANE_CHANGE_DISTANCE)); + + bool recently_not_executed_lane_change = last_lane_change_location.find(actor_id) == last_lane_change_location.end(); + bool done_with_previous_lane_change = true; + if (!recently_not_executed_lane_change) { + float distance_frm_previous = cg::Math::DistanceSquared(last_lane_change_location.at(actor_id), vehicle_location); + done_with_previous_lane_change = distance_frm_previous > lane_change_distance; + } + bool auto_or_force_lane_change = parameters.GetAutoLaneChange(actor_id) || force_lane_change; + bool front_waypoint_not_junction = !front_waypoint->CheckJunction(); + + if (auto_or_force_lane_change + && front_waypoint_not_junction + && (recently_not_executed_lane_change || done_with_previous_lane_change)) { + + SimpleWaypointPtr change_over_point = AssignLaneChange(actor_id, vehicle_location, vehicle_speed, + force_lane_change, lane_change_direction); + + if (change_over_point != nullptr) { + if (last_lane_change_location.find(actor_id) != last_lane_change_location.end()) { + last_lane_change_location.at(actor_id) = vehicle_location; + } else { + last_lane_change_location.insert({actor_id, vehicle_location}); + } + auto number_of_pops = waypoint_buffer.size(); + for (uint64_t j = 0u; j < number_of_pops; ++j) { + PopWaypoint(actor_id, track_traffic, waypoint_buffer); + } + PushWaypoint(actor_id, track_traffic, waypoint_buffer, change_over_point); + } + } + + // Populating the buffer. + while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) { + + std::vector next_waypoints = waypoint_buffer.back()->GetNextWaypoint(); + uint64_t selection_index = 0u; + // Pseudo-randomized path selection if found more than one choice. + if (next_waypoints.size() > 1) { + selection_index = static_cast(pgen.next()) % next_waypoints.size(); + } + SimpleWaypointPtr next_wp = next_waypoints.at(selection_index); + if (next_wp == nullptr) { + for (auto &wp : next_waypoints) { + if (wp != nullptr) { + next_wp = wp; + break; } + } + } + PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp); + } - // Purge waypoints too far from the front of the buffer. - while (!waypoint_buffer.empty() - && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square) { - PopWaypoint(waypoint_buffer, actor_id, false); + ExtendAndFindSafeSpace(actor_id, is_at_junction_entrance, waypoint_buffer); + + // Editing output array + LocalizationData &output = output_array.at(index); + output.is_at_junction_entrance = is_at_junction_entrance; + + if (is_at_junction_entrance) { + const SimpleWaypointPair &safe_space_end_points = vehicles_at_junction_entrance.at(actor_id); + output.junction_end_point = safe_space_end_points.first; + output.safe_point = safe_space_end_points.second; + } else { + output.junction_end_point = nullptr; + output.safe_point = nullptr; + } + + // Updating geodesic grid position for actor. + track_traffic.UpdateGridPosition(actor_id, waypoint_buffer); +} + +void LocalizationStage::ExtendAndFindSafeSpace(const ActorId actor_id, + const bool is_at_junction_entrance, + Buffer &waypoint_buffer) { + + SimpleWaypointPtr junction_end_point = nullptr; + SimpleWaypointPtr safe_point_after_junction = nullptr; + + if (is_at_junction_entrance + && vehicles_at_junction_entrance.find(actor_id) == vehicles_at_junction_entrance.end()) { + + bool entered_junction = false; + bool past_junction = false; + bool safe_point_found = false; + SimpleWaypointPtr current_waypoint = nullptr; + SimpleWaypointPtr junction_begin_point = nullptr; + float safe_distance_squared = SQUARE(SAFE_DISTANCE_AFTER_JUNCTION); + + // Scanning existing buffer points. + for (unsigned long i = 0u; i < waypoint_buffer.size() && !safe_point_found; ++i) { + current_waypoint = waypoint_buffer.at(i); + if (!entered_junction && current_waypoint->CheckJunction()) { + entered_junction = true; + junction_begin_point = current_waypoint; + } + if (entered_junction && !past_junction && !current_waypoint->CheckJunction()) { + past_junction = true; + junction_end_point = current_waypoint; + } + if (past_junction && junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared) { + safe_point_found = true; + safe_point_after_junction = current_waypoint; + } + } + + // Extend buffer if safe point not found. + if (!safe_point_found) { + while (!past_junction) { + current_waypoint = current_waypoint->GetNextWaypoint().front(); + PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint); + if (!current_waypoint->CheckJunction()) { + past_junction = true; + junction_end_point = current_waypoint; } } - // Initializing buffer if it is empty. - if (waypoint_buffer.empty()) { - SimpleWaypointPtr closest_waypoint = local_map.GetWaypointInVicinity(vehicle_location); - if (closest_waypoint == nullptr) { - closest_waypoint = local_map.GetWaypoint(vehicle_location); - } - PushWaypoint(waypoint_buffer, actor_id, closest_waypoint); - } + while (!safe_point_found) { + std::vector next_waypoints = current_waypoint->GetNextWaypoint(); + if ((junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared) + || next_waypoints.size() > 1 + || current_waypoint->CheckJunction()) { - // Assign a lane change. - - const ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(vehicle); - bool force_lane_change = lane_change_info.change_lane; - bool lane_change_direction = lane_change_info.direction; - - if (!force_lane_change) { - float perc_keep_right = parameters.GetKeepRightPercentage(vehicle); - if (perc_keep_right >= 0.0f && perc_keep_right >= (rand() % 101)) { - force_lane_change = true; - lane_change_direction = true; - } - } - - const SimpleWaypointPtr front_waypoint = waypoint_buffer.front(); - const double lane_change_distance = std::pow(std::max(10.0f * vehicle_velocity, INTER_LANE_CHANGE_DISTANCE), 2); - - if (((parameters.GetAutoLaneChange(vehicle) || force_lane_change) && !front_waypoint->CheckJunction()) - && (last_lane_change_location.find(actor_id) == last_lane_change_location.end() - || cg::Math::DistanceSquared(last_lane_change_location.at(actor_id), vehicle_location) - > lane_change_distance )) { - - SimpleWaypointPtr change_over_point = AssignLaneChange( - vehicle, vehicle_location, force_lane_change, lane_change_direction); - - if (change_over_point != nullptr) { - if (last_lane_change_location.find(actor_id) != last_lane_change_location.end()) { - last_lane_change_location.at(actor_id) = vehicle_location; - } else { - last_lane_change_location.insert({actor_id, vehicle_location}); - } - auto number_of_pops = waypoint_buffer.size(); - for (uint64_t j = 0u; j < number_of_pops; ++j) { - PopWaypoint(waypoint_buffer, actor_id); - } - PushWaypoint(waypoint_buffer, actor_id, change_over_point); - } - } - - // Populating the buffer. - while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) { - - std::vector next_waypoints = waypoint_buffer.back()->GetNextWaypoint(); - uint64_t selection_index = 0u; - // Pseudo-randomized path selection if found more than one choice. - if (next_waypoints.size() > 1) { - selection_index = static_cast(rand()) % next_waypoints.size(); - } - SimpleWaypointPtr next_wp = next_waypoints.at(selection_index); - if (next_wp == nullptr) { - for (auto& wp: next_waypoints) { - if (wp != nullptr) { - next_wp = wp; - break; - } - } - } - PushWaypoint(waypoint_buffer, actor_id, next_wp); - } - - // Begining point of the waypoint buffer; - const SimpleWaypointPtr& updated_front_waypoint = waypoint_buffer.front(); - - // Updating geodesic grid position for actor. - track_traffic.UpdateGridPosition(actor_id, waypoint_buffer); - // WayPoint Binning Changes - // Generating output. - const float target_point_distance = std::max(std::ceil(vehicle_velocity * TARGET_WAYPOINT_TIME_HORIZON), - TARGET_WAYPOINT_HORIZON_LENGTH); - - std::pair target_waypoint_index_pair = track_traffic.GetTargetWaypoint(waypoint_buffer, target_point_distance); - SimpleWaypointPtr &target_waypoint = target_waypoint_index_pair.first; - const cg::Location target_location = target_waypoint->GetLocation(); - float dot_product = DeviationDotProduct(vehicle, vehicle_location, target_location); - float cross_product = DeviationCrossProduct(vehicle, vehicle_location, target_location); - dot_product = 1.0f - dot_product; - if (cross_product < 0.0f) { - dot_product *= -1.0f; - } - - float distance = 0.0f; // TODO: use in PID - - // Filtering out false junctions on highways: - // on highways, if there is only one possible path and the section is - // marked as intersection, ignore it. - const auto vehicle_reference = boost::static_pointer_cast(vehicle); - const float speed_limit = vehicle_reference->GetSpeedLimit(); - const float look_ahead_distance = std::max(2.0f * vehicle_velocity, MINIMUM_JUNCTION_LOOK_AHEAD); - - std::pair look_ahead_point_index_pair = track_traffic.GetTargetWaypoint(waypoint_buffer, look_ahead_distance); - SimpleWaypointPtr &look_ahead_point = look_ahead_point_index_pair.first; - uint64_t &look_ahead_index = look_ahead_point_index_pair.second; - bool approaching_junction = false; - if (look_ahead_point->CheckJunction() && !(updated_front_waypoint->CheckJunction())) { - if (speed_limit*3.6f > HIGHWAY_SPEED) { - for (uint64_t j = 0u; (j < look_ahead_index) && !approaching_junction; ++j) { - SimpleWaypointPtr swp = waypoint_buffer.at(j); - if (swp->GetNextWaypoint().size() > 1) { - approaching_junction = true; - } - } + safe_point_found = true; + safe_point_after_junction = current_waypoint; } else { - approaching_junction = true; + current_waypoint = next_waypoints.front(); + PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint); } } + } - // Reset the variables when no longer approaching a junction. - if (!approaching_junction && approached[actor_id]){ - final_safe_points[actor_id] = nullptr; - approached[actor_id] = false; - } + if (junction_begin_point->DistanceSquared(junction_end_point) < SQUARE(MIN_JUNCTION_LENGTH)) { + junction_end_point = nullptr; + safe_point_after_junction = nullptr; + } - // Only do once, when the junction has just been seen. - else if (approaching_junction && !approached[actor_id]){ + vehicles_at_junction_entrance.insert({actor_id, {junction_end_point, safe_point_after_junction}}); + } + else if (!is_at_junction_entrance + && vehicles_at_junction_entrance.find(actor_id) != vehicles_at_junction_entrance.end()) { - SimpleWaypointPtr final_point = nullptr; - final_point = GetSafeLocationAfterJunction(vehicle_reference, waypoint_buffer); - if(final_point != nullptr){ - final_safe_points[actor_id] = final_point; - approaching_junction = false; - approached[actor_id] = true; - } - } + vehicles_at_junction_entrance.erase(actor_id); + } +} - // Determining possible collision candidates around the ego vehicle. - ActorIdSet overlapping_actor_set = track_traffic.GetOverlappingVehicles(actor_id); - using ActorInfoMap = std::unordered_map>; - ActorInfoMap overlapping_actor_info; - std::vector collision_candidate_ids; +void LocalizationStage::RemoveActor(ActorId actor_id) { + last_lane_change_location.erase(actor_id); + vehicles_at_junction.erase(actor_id); +} - // Run through vehicles with overlapping paths, obtain actor reference, - // and filter them based on distance to ego vehicle. - Actor overlapping_actor_ptr = nullptr; - for (ActorId overlapping_actor_id: overlapping_actor_set) { +void LocalizationStage::Reset() { + last_lane_change_location.clear(); + vehicles_at_junction.clear(); +} - // If actor is part of the registered actors. - if (vehicle_id_to_index.find(overlapping_actor_id) != vehicle_id_to_index.end()) { - overlapping_actor_ptr = actor_list.at(vehicle_id_to_index.at(overlapping_actor_id)); - } - // If actor is part of the unregistered actors. - else if (unregistered_actors.find(overlapping_actor_id) != unregistered_actors.end()) { - overlapping_actor_ptr = unregistered_actors.at(overlapping_actor_id); - } - // If actor is within maximum collision avoidance range. - if (overlapping_actor_ptr!=nullptr && overlapping_actor_ptr->IsAlive() - && cg::Math::DistanceSquared(overlapping_actor_ptr->GetLocation(), - vehicle_location) < std::pow(MAX_COLLISION_RADIUS, 2)) - { - overlapping_actor_info.insert({overlapping_actor_id, - {overlapping_actor_ptr, overlapping_actor_ptr->GetLocation()}}); - collision_candidate_ids.push_back(overlapping_actor_id); - } - } +SimpleWaypointPtr LocalizationStage::AssignLaneChange(const ActorId actor_id, + const cg::Location vehicle_location, + const float vehicle_speed, + bool force, bool direction) { - // Sorting collision candidates in accending order of distance to current vehicle. - std::sort(collision_candidate_ids.begin(), collision_candidate_ids.end(), - [&overlapping_actor_info, &vehicle_location] (const ActorId& a_id_1, const ActorId& a_id_2) { - const cg::Location& e_loc = vehicle_location; - const cg::Location& loc_1 = overlapping_actor_info.at(a_id_1).second; - const cg::Location& loc_2 = overlapping_actor_info.at(a_id_2).second; - return (cg::Math::DistanceSquared(e_loc, loc_1) < cg::Math::DistanceSquared(e_loc, loc_2)); - }); + // Waypoint representing the new starting point for the waypoint buffer + // due to lane change. Remains nullptr if lane change not viable. + SimpleWaypointPtr change_over_point = nullptr; - // Constructing output vector. - std::vector> collision_candidates; - std::for_each(collision_candidate_ids.begin(), collision_candidate_ids.end(), - [&overlapping_actor_info, &collision_candidates, this] (const ActorId& a_id) { - collision_candidates.push_back({a_id, - overlapping_actor_info.at(a_id).first, - this->GetVelocity(a_id)}); - }); + // Retrieve waypoint buffer for current vehicle. + const Buffer &waypoint_buffer = buffer_map.at(actor_id); - // Sampling waypoint window for teleportation in hybrid physics mode. - std::vector position_window; - if (hybrid_physics_mode) { + // Check buffer is not empty. + if (!waypoint_buffer.empty()) { + // Get the left and right waypoints for the current closest waypoint. + const SimpleWaypointPtr ¤t_waypoint = waypoint_buffer.front(); + const SimpleWaypointPtr left_waypoint = current_waypoint->GetLeftWaypoint(); + const SimpleWaypointPtr right_waypoint = current_waypoint->GetRightWaypoint(); - cg::Vector3D heading = vehicle->GetTransform().GetForwardVector(); - bool window_begin = false; - SimpleWaypointPtr begining_wp; - bool window_complete = false; + // Retrieve vehicles with overlapping waypoint buffers with current vehicle. + const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id); - for (uint32_t j = 0u; j < waypoint_buffer.size() && !window_complete; ++j) { + // Find immediate in-lane obstacle and check if any are too close to initiate lane change. + bool obstacle_too_close = false; + float minimum_squared_distance = std::numeric_limits::infinity(); + ActorId obstacle_actor_id = 0u; + for (auto i = blocking_vehicles.begin(); + i != blocking_vehicles.end() && !obstacle_too_close && !force; + ++i) { + const ActorId &other_actor_id = *i; + // Find vehicle in buffer map and check if it's buffer is not empty. + if (buffer_map.find(other_actor_id) != buffer_map.end() && !buffer_map.at(other_actor_id).empty()) { + const Buffer &other_buffer = buffer_map.at(other_actor_id); + const SimpleWaypointPtr &other_current_waypoint = other_buffer.front(); + const cg::Location other_location = other_current_waypoint->GetLocation(); - SimpleWaypointPtr &swp = waypoint_buffer.at(j); - cg::Vector3D relative_position = swp->GetLocation() - vehicle_location; + const cg::Vector3D reference_heading = current_waypoint->GetForwardVector(); + cg::Vector3D reference_to_other = other_location - current_waypoint->GetLocation(); + const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector(); - // Sample waypoints in front of the vehicle; - if (!window_begin && cg::Math::Dot(relative_position, heading) > 0.0f) { - window_begin = true; - begining_wp = swp; - } - - if (window_begin && !window_complete) { - if (swp->DistanceSquared(begining_wp) > std::pow(POSITION_WINDOW_SIZE, 2)) { - // Stop when maximum size is reached. - window_complete = true; - } else { - position_window.push_back(swp); - } - } - } - } - - // Editing output frames. - LocalizationToPlannerData &planner_message = current_planner_frame->at(i); - planner_message.actor = vehicle; - planner_message.deviation = dot_product; - planner_message.distance = distance; - planner_message.approaching_true_junction = approaching_junction; - planner_message.velocity = vehicle_velocity_vector; - planner_message.position_window = std::move(position_window); - planner_message.physics_enabled = IsPhysicsEnabled(actor_id); - - LocalizationToCollisionData &collision_message = current_collision_frame->at(i); - collision_message.actor = vehicle; - collision_message.buffer = waypoint_buffer; - collision_message.overlapping_actors = std::move(collision_candidates); - collision_message.closest_waypoint = updated_front_waypoint; - collision_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index); - collision_message.safe_point_after_junction = final_safe_points[actor_id]; - collision_message.velocity = vehicle_velocity_vector; - - LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i); - traffic_light_message.actor = vehicle; - traffic_light_message.closest_waypoint = updated_front_waypoint; - traffic_light_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index); - - // Updating idle time when necessary. - UpdateIdleTime(vehicle); - } - - if (IsVehicleStuck(maximum_idle_time.first)) { - TryDestroyVehicle(maximum_idle_time.first); - } - - // Updating maximum idle time to null for the next iteration. - maximum_idle_time = std::make_pair(nullptr, current_timestamp.elapsed_seconds); - } - - bool LocalizationStage::RunStep() { - - if (parameters.GetSynchronousMode()) { - std::unique_lock lock(step_execution_mutex); - // Set flag to true, notify DataReceiver initiate pipeline. - run_step.store(true); - step_execution_trigger.notify_one(); - } - - return true; - } - - void LocalizationStage::DataReceiver() { - - // Wait for external trigger to initiate the pipeline in synchronous mode. - if (parameters.GetSynchronousMode()) { - std::unique_lock lock(step_execution_mutex); - while (!run_step.load()) { - step_execution_trigger.wait_for(lock, 1ms, [this]() {return run_step.load();}); - } - // Set flag to false, unblock RunStep() call and release mutex lock. - run_step.store(false); - } - - hybrid_physics_mode = parameters.GetHybridPhysicsMode(); - hybrid_physics_radius = parameters.GetHybridPhysicsRadius(); - - bool is_deleted_actors_present = false; - std::set world_actor_id; - std::vector actor_list_to_be_deleted; - - // Filter function to collect the data. - auto Filter = [&](auto &actors, auto &wildcard_pattern) { - std::vector filtered; - for (auto &&actor : actors) { - if (carla::StringUtil::Match(carla::client::detail::ActorVariant(actor).GetTypeId(), wildcard_pattern)) { - filtered.push_back(actor); - } - } - return filtered; - }; - - // TODO: Combine this actor scanning logic with ScanUnregisteredActors() - // Get all the actors. - auto world_actors_list = episode_proxy_ls.Lock()->GetAllTheActorsInTheEpisode(); - - // Filter with vehicle wildcard. - auto vehicles = Filter(world_actors_list, "vehicle.*"); - - // Building a set of vehicle ids in the world. - for (const auto &actor : vehicles) { - world_actor_id.insert(actor.GetId()); - - // Identify hero vehicles if system is in hybrid physics mode. - if (hybrid_physics_mode) { - Actor actor_ptr = actor.Get(episode_proxy_ls); - ActorId hero_actor_id = actor_ptr->GetId(); - if (hero_actors.find(hero_actor_id) == hero_actors.end()) { - for (auto&& attribute: actor_ptr->GetAttributes()) { - if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") { - hero_actors.insert({hero_actor_id, actor_ptr}); + WaypointPtr current_raw_waypoint = current_waypoint->GetWaypoint(); + WaypointPtr other_current_raw_waypoint = other_current_waypoint->GetWaypoint(); + // Check both vehicles are not in junction, + // Check if the other vehicle is in front of the current vehicle, + // Check if the two vehicles have acceptable angular deviation between their headings. + if (!current_waypoint->CheckJunction() + && !other_current_waypoint->CheckJunction() + && other_current_raw_waypoint->GetRoadId() == current_raw_waypoint->GetRoadId() + && other_current_raw_waypoint->GetLaneId() == current_raw_waypoint->GetLaneId() + && cg::Math::Dot(reference_heading, reference_to_other) > 0.0f + && cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) { + float squared_distance = cg::Math::DistanceSquared(vehicle_location, other_location); + // Abort if the obstacle is too close. + if (squared_distance > SQUARE(MINIMUM_LANE_CHANGE_DISTANCE)) { + // Remember if the new vehicle is closer. + if (squared_distance < minimum_squared_distance && squared_distance < SQUARE(MAXIMUM_LANE_OBSTACLE_DISTANCE)) { + minimum_squared_distance = squared_distance; + obstacle_actor_id = other_actor_id; } + } else { + obstacle_too_close = true; } } } } - // Invalidate hero actor pointer if it is not alive anymore. - ActorIdSet hero_actors_to_delete; - if (hybrid_physics_mode && hero_actors.size() != 0u) { - for (auto &hero_actor_info: hero_actors) { - if(world_actor_id.find(hero_actor_info.first) == world_actor_id.end()) { - hero_actors_to_delete.insert(hero_actor_info.first); + // If a valid immediate obstacle found. + if (!obstacle_too_close && obstacle_actor_id != 0u && !force) { + const Buffer &other_buffer = buffer_map.at(obstacle_actor_id); + const SimpleWaypointPtr &other_current_waypoint = other_buffer.front(); + const auto other_neighbouring_lanes = {other_current_waypoint->GetLeftWaypoint(), + other_current_waypoint->GetRightWaypoint()}; + + // Flags reflecting whether adjacent lanes are free near the obstacle. + bool distant_left_lane_free = false; + bool distant_right_lane_free = false; + + // Check if the neighbouring lanes near the obstructing vehicle are free of other vehicles. + bool left_right = true; + for (auto &candidate_lane_wp : other_neighbouring_lanes) { + if (candidate_lane_wp != nullptr && + track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0) { + + if (left_right) + distant_left_lane_free = true; + else + distant_right_lane_free = true; } + left_right = !left_right; } - } - for (auto &deletion_id: hero_actors_to_delete) { - hero_actors.erase(deletion_id); - } - // Search for invalid/destroyed vehicles. - for (auto &actor : actor_list) { - ActorId deletion_id = actor->GetId(); - if (world_actor_id.find(deletion_id) == world_actor_id.end()) { - actor_list_to_be_deleted.emplace_back(actor); - track_traffic.DeleteActor(deletion_id); - last_lane_change_location.erase(deletion_id); - kinematic_state_map.erase(deletion_id); + // Based on what lanes are free near the obstacle, + // find the change over point with no vehicles passing through them. + if (distant_right_lane_free && right_waypoint != nullptr + && track_traffic.GetPassingVehicles(right_waypoint->GetId()).size() == 0) { + change_over_point = right_waypoint; + } else if (distant_left_lane_free && left_waypoint != nullptr + && track_traffic.GetPassingVehicles(left_waypoint->GetId()).size() == 0) { + change_over_point = left_waypoint; + } + } else if (force) { + if (direction && right_waypoint != nullptr) { + change_over_point = right_waypoint; + } else if (!direction && left_waypoint != nullptr) { + change_over_point = left_waypoint; } } - // Clearing the registered actor list. - if(!actor_list_to_be_deleted.empty()) { - registered_actors.Remove(actor_list_to_be_deleted); - actor_list.clear(); - actor_list = registered_actors.GetList(); - is_deleted_actors_present = true; - } - - // Building a list of registered actors and connecting - // the vehicle ids to their position indices on data arrays. - - if (is_deleted_actors_present || (registered_actors_state != registered_actors.GetState())) { - actor_list.clear(); - actor_list_to_be_deleted.clear(); - actor_list = registered_actors.GetList(); - uint64_t index = 0u; - vehicle_id_to_index.clear(); - for (auto &actor : actor_list) { - vehicle_id_to_index.insert({actor->GetId(), index}); - ++index; - } - registered_actors_state = registered_actors.GetState(); - } - - // Allocating new containers for the changed number of registered vehicles. - if (number_of_vehicles != actor_list.size()) { - number_of_vehicles = static_cast(actor_list.size()); - // Allocating output frames to be shared with the motion planner stage. - planner_frame_a = std::make_shared(number_of_vehicles); - planner_frame_b = std::make_shared(number_of_vehicles); - // Allocating output frames to be shared with the collision stage. - collision_frame_a = std::make_shared(number_of_vehicles); - collision_frame_b = std::make_shared(number_of_vehicles); - // Allocating output frames to be shared with the traffic light stage - traffic_light_frame_a = std::make_shared(number_of_vehicles); - traffic_light_frame_b = std::make_shared(number_of_vehicles); - } - } - - void LocalizationStage::DataSender() { - - planner_messenger->Push(planner_frame_selector ? planner_frame_a : planner_frame_b); - planner_frame_selector = !planner_frame_selector; - - const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b; - collision_messenger->Push(current_collision_frame); - collision_frame_selector = !collision_frame_selector; - - traffic_light_messenger->Push(traffic_light_frame_selector ? traffic_light_frame_a : traffic_light_frame_b); - traffic_light_frame_selector = !traffic_light_frame_selector; - } - - void LocalizationStage::DrawBuffer(Buffer &buffer) { - uint64_t buffer_size = buffer.size(); - uint64_t step_size = buffer_size/10u; - for (uint64_t i = 0u; i + step_size < buffer_size; i += step_size) { - debug_helper.DrawLine(buffer.at(i)->GetLocation() + cg::Location(0.0, 0.0, 2.0), - buffer.at(i + step_size)->GetLocation() + cg::Location(0.0, 0.0, 2.0), - 0.2f, {0u, 255u, 0u}, 0.05f); - } - } - - void LocalizationStage::PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint) { - - const uint64_t waypoint_id = waypoint->GetId(); - buffer.push_back(waypoint); - track_traffic.UpdatePassingVehicle(waypoint_id, actor_id); - } - - void LocalizationStage::PopWaypoint(Buffer& buffer, ActorId actor_id, bool front_or_back) { - - SimpleWaypointPtr removed_waypoint = front_or_back? buffer.front(): buffer.back(); - const uint64_t removed_waypoint_id = removed_waypoint->GetId(); - if (front_or_back) { - buffer.pop_front(); - } else { - buffer.pop_back(); - } - track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id); - } - - void LocalizationStage::ScanUnregisteredVehicles() { - ++unregistered_scan_duration; - // Periodically check for actors not spawned by TrafficManager. - if (unregistered_scan_duration == UNREGISTERED_ACTORS_SCAN_INTERVAL) { - unregistered_scan_duration = 0; - - auto Filter = [&](auto &actors, auto &wildcard_pattern) { - std::vector filtered; - for (auto &&actor : actors) { - if (carla::StringUtil::Match - ( carla::client::detail::ActorVariant(actor).GetTypeId() - , wildcard_pattern)) { - filtered.push_back(actor); - } - } - return filtered; - }; - - /// Get all actors of the world - auto world_actors_list = episode_proxy_ls.Lock()->GetAllTheActorsInTheEpisode(); - - /// Filter based on wildcard_pattern - const auto world_actors = Filter(world_actors_list, "vehicle.*"); - const auto world_walker = Filter(world_actors_list, "walker.*"); - - // Scanning for vehicles. - for (auto actor: world_actors) { - const auto unregistered_id = actor.GetId(); - if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() && - unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { - unregistered_actors.insert({unregistered_id, actor.Get(episode_proxy_ls)}); - } - } - // Scanning for pedestrians. - for (auto walker: world_walker) { - const auto unregistered_id = walker.GetId(); - if (unregistered_actors.find(unregistered_id) == unregistered_actors.end()) { - unregistered_actors.insert({unregistered_id, walker.Get(episode_proxy_ls)}); + if (change_over_point != nullptr) { + const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, 3.0f, 20.0f); + const auto starting_point = change_over_point; + while (change_over_point->DistanceSquared(starting_point) < SQUARE(change_over_distance) && + !change_over_point->CheckJunction()) { + change_over_point = change_over_point->GetNextWaypoint()[0]; } } } - // Regularly update unregistered actors. - const auto current_snapshot = episode_proxy_ls.Lock()->GetWorldSnapshot(); - for (auto it = unregistered_actors.cbegin(); it != unregistered_actors.cend();) { - if (registered_actors.Contains(it->first) || !current_snapshot.Contains(it->first)) { - track_traffic.DeleteActor(it->first); - it = unregistered_actors.erase(it); - } else { - // Updating data structures. - cg::Location location = it->second->GetLocation(); - const auto type = it->second->GetTypeId(); + return change_over_point; +} - std::vector nearest_waypoints; - if (type[0] == 'v') { - auto vehicle_ptr = boost::static_pointer_cast(it->second); - cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent; - float bx = extent.x; - float by = extent.y; - std::vector corners = {location + cg::Location(bx, by, 0.0f), - location + cg::Location(-bx, by, 0.0f), - location + cg::Location(bx, -by, 0.0f), - location + cg::Location(-bx, -by, 0.0f)}; - for (cg::Location &vertex: corners) { - SimpleWaypointPtr nearest_waypoint = local_map.GetWaypointInVicinity(vertex); - if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetPedWaypoint(vertex);}; - if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetWaypoint(location);}; - nearest_waypoints.push_back(nearest_waypoint); - } - } else if (type[0] == 'w') { - SimpleWaypointPtr nearest_waypoint = local_map.GetPedWaypoint(location); - if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetWaypoint(location);}; - nearest_waypoints.push_back(nearest_waypoint); - } - - track_traffic.UpdateUnregisteredGridPosition(it->first, nearest_waypoints); - - ++it; - } +void LocalizationStage::DrawBuffer(Buffer &buffer) { + uint64_t buffer_size = buffer.size(); + uint64_t step_size = buffer_size/20u; + cc::DebugHelper::Color color {0u, 0u, 0u}; + cg::Location two_meters_up = cg::Location(0.0f, 0.0f, 2.0f); + for (uint64_t i = 0u; i + step_size < buffer_size; i += step_size) { + if (!buffer.at(i)->CheckJunction() && !buffer.at(i + step_size)->CheckJunction()) { + color.g = 255u; } + debug_helper.DrawLine(buffer.at(i)->GetLocation() + two_meters_up, + buffer.at(i + step_size)->GetLocation() + two_meters_up, + 0.2f, color, 0.05f); + color = {0u, 0u, 0u}; } - - SimpleWaypointPtr LocalizationStage::AssignLaneChange(Actor vehicle, const cg::Location &vehicle_location, - bool force, bool direction) - { - - const ActorId actor_id = vehicle->GetId(); - const float vehicle_velocity = GetVelocity(actor_id).Length(); - - // Waypoint representing the new starting point for the waypoint buffer - // due to lane change. Remains nullptr if lane change not viable. - SimpleWaypointPtr change_over_point = nullptr; - - // Retrieve waypoint buffer for current vehicle. - const Buffer& waypoint_buffer = buffer_list->at(actor_id); - - // Check buffer is not empty. - if (!waypoint_buffer.empty()) - { - // Get the left and right waypoints for the current closest waypoint. - const SimpleWaypointPtr& current_waypoint = waypoint_buffer.front(); - const SimpleWaypointPtr left_waypoint = current_waypoint->GetLeftWaypoint(); - const SimpleWaypointPtr right_waypoint = current_waypoint->GetRightWaypoint(); - - // Retrieve vehicles with overlapping waypoint buffers with current vehicle. - const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id); - - // Find immediate in-lane obstacle and check if any are too close to initiate lane change. - bool obstacle_too_close = false; - float minimum_squared_distance = std::numeric_limits::infinity(); - ActorId obstacle_actor_id = 0u; - for (auto i = blocking_vehicles.begin(); - i != blocking_vehicles.end() && !obstacle_too_close && !force; - ++i) - { - const ActorId &other_actor_id = *i; - // Find vehicle in registered list and check if it's buffer is not empty. - if (vehicle_id_to_index.find(other_actor_id) != vehicle_id_to_index.end() - && buffer_list->find(other_actor_id) != buffer_list->end() - && !buffer_list->at(other_actor_id).empty()) - { - const Buffer& other_buffer = buffer_list->at(other_actor_id); - const SimpleWaypointPtr& other_current_waypoint = other_buffer.front(); - const cg::Location other_location = other_current_waypoint->GetLocation(); - - const cg::Vector3D reference_heading = current_waypoint->GetForwardVector(); - cg::Vector3D reference_to_other = other_current_waypoint->GetLocation() - - current_waypoint->GetLocation(); - const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector(); - - // Check both vehicles are not in junction, - // Check if the other vehicle is in front of the current vehicle, - // Check if the two vehicles have acceptable angular deviation between their headings. - if (!current_waypoint->CheckJunction() - && !other_current_waypoint->CheckJunction() - && other_current_waypoint->GetWaypoint()->GetRoadId() == current_waypoint->GetWaypoint()->GetRoadId() - && other_current_waypoint->GetWaypoint()->GetLaneId() == current_waypoint->GetWaypoint()->GetLaneId() - && cg::Math::Dot(reference_heading, reference_to_other) > 0.0f - && cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) - { - float squared_distance = cg::Math::DistanceSquared(vehicle_location, other_location); - // Abort if the obstacle is too close. - if (squared_distance > std::pow(MINIMUM_LANE_CHANGE_DISTANCE, 2)) - { - // Remember if the new vehicle is closer. - if (squared_distance < minimum_squared_distance - && squared_distance < std::pow(MAXIMUM_LANE_OBSTACLE_DISTANCE, 2.0f)) - { - minimum_squared_distance = squared_distance; - obstacle_actor_id = other_actor_id; - } - } else { - obstacle_too_close = true; - } - } - } - } - - // If a valid immediate obstacle found. - if (!obstacle_too_close && obstacle_actor_id != 0u && !force) - { - const Buffer& other_buffer = buffer_list->at(obstacle_actor_id); - const SimpleWaypointPtr& other_current_waypoint = other_buffer.front(); - const auto other_neighbouring_lanes = {other_current_waypoint->GetLeftWaypoint(), - other_current_waypoint->GetRightWaypoint()}; - - // Flags reflecting whether adjacent lanes are free near the obstacle. - bool distant_left_lane_free = false; - bool distant_right_lane_free = false; - - // Check if the neighbouring lanes near the obstructing vehicle are free of other vehicles. - bool left_right = true; - for (auto& candidate_lane_wp: other_neighbouring_lanes) { - if (candidate_lane_wp != nullptr && - track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0) { - - if (left_right) distant_left_lane_free = true; - else distant_right_lane_free = true; - - } - left_right = !left_right; - } - - // Based on what lanes are free near the obstacle, - // find the change over point with no vehicles passing through them. - if (distant_right_lane_free && right_waypoint != nullptr - && track_traffic.GetPassingVehicles(right_waypoint->GetId()).size() == 0) - { - change_over_point = right_waypoint; - } else if (distant_left_lane_free && left_waypoint != nullptr - && track_traffic.GetPassingVehicles(left_waypoint->GetId()).size() == 0) - { - change_over_point = left_waypoint; - } - } else if (force) { - if (direction && right_waypoint != nullptr) { - change_over_point = right_waypoint; - } else if (!direction && left_waypoint != nullptr) { - change_over_point = left_waypoint; - } - } - - if (change_over_point != nullptr) - { - const float change_over_distance = cg::Math::Clamp(1.5f*vehicle_velocity, 3.0f, 20.0f); - const auto starting_point = change_over_point; - while (change_over_point->DistanceSquared(starting_point) < std::pow(change_over_distance, 2) && - !change_over_point->CheckJunction()) { - change_over_point = change_over_point->GetNextWaypoint()[0]; - } - - // Reset this variable if needed - if (approached[actor_id]){ - approached[actor_id] = false; - } - } - } - - return change_over_point; - } - - SimpleWaypointPtr LocalizationStage::GetSafeLocationAfterJunction(const Vehicle &vehicle, Buffer &waypoint_buffer){ - - ActorId actor_id = vehicle->GetId(); - // Get the length of the car - float length = vehicle->GetBoundingBox().extent.x; - // First Waypoint before the junction - const SimpleWaypointPtr initial_point; - uint64_t initial_index = 0; - // First Waypoint after the junction - SimpleWaypointPtr safe_point = nullptr; - uint64_t safe_index = 0; - // Vehicle position after the junction - SimpleWaypointPtr final_point = nullptr; - // Safe space after the junction - const float safe_distance = 1.5f*length; - - for (uint64_t j = 0u; j < waypoint_buffer.size(); ++j){ - if (waypoint_buffer.at(j)->CheckJunction()){ - initial_index = j; - break; - } - } - - // Stop if something failed - if (initial_index == 0 && !waypoint_buffer.front()->CheckJunction()){ - return final_point; - } - - // 2) Search for the end of the intersection (if it is in the buffer) - for (uint64_t i = initial_index; i < waypoint_buffer.size(); ++i){ - - if (!waypoint_buffer.at(i)->CheckJunction()){ - safe_point = waypoint_buffer.at(i); - safe_index = i; - break; - } - } - - // If it hasn't been found, extend the buffer - if(safe_point == nullptr){ - while (waypoint_buffer.back()->CheckJunction()) { - - std::vector next_waypoints = waypoint_buffer.back()->GetNextWaypoint(); - uint64_t selection_index = 0u; - if (next_waypoints.size() > 1) { - selection_index = static_cast(rand()) % next_waypoints.size(); - } - - PushWaypoint(waypoint_buffer, actor_id, next_waypoints.at(selection_index)); - } - // Save the last one - safe_point = waypoint_buffer.back(); - } - - // Stop if something failed - if (safe_index == 0){ - return final_point; - } - - // 3) Search for final_point (again, if it is in the buffer) - - for(uint64_t k = safe_index; k < waypoint_buffer.size(); ++k){ - - if(safe_point->Distance(waypoint_buffer.at(k)->GetLocation()) > safe_distance){ - final_point = waypoint_buffer.at(k); - break; - } - } - - // If it hasn't been found, extend the buffer - if(final_point == nullptr){ - while (safe_point->Distance(waypoint_buffer.back()->GetLocation()) < safe_distance) { - - // Record the last point as a safe one and save it - std::vector next_waypoints = waypoint_buffer.back()->GetNextWaypoint(); - uint64_t selection_index = 0u; - // Pseudo-randomized path selection if found more than one choice. - if (next_waypoints.size() > 1) { - selection_index = static_cast(rand()) % next_waypoints.size(); - } - - PushWaypoint(waypoint_buffer, actor_id, next_waypoints.at(selection_index)); - } - final_point = waypoint_buffer.back(); - } - - return final_point; - } - - void LocalizationStage::UpdateIdleTime(const Actor& actor) { - if (idle_time.find(actor->GetId()) == idle_time.end()) { - return; - } - - const auto vehicle = boost::static_pointer_cast(actor); - if (GetVelocity(actor->GetId()).Length() > STOPPED_VELOCITY_THRESHOLD - || (vehicle->IsAtTrafficLight() && vehicle->GetTrafficLightState() != TLS::Green)) { - idle_time[actor->GetId()] = current_timestamp.elapsed_seconds; - } - - // Checking maximum idle time. - if (maximum_idle_time.first == nullptr || maximum_idle_time.second > idle_time[actor->GetId()]) { - maximum_idle_time = std::make_pair(actor, idle_time[actor->GetId()]); - } - } - - bool LocalizationStage::IsVehicleStuck(const Actor& actor) { - if (actor == nullptr) { - return false; - } - - if (idle_time.find(actor->GetId()) != idle_time.end()) { - auto delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor->GetId()); - if (delta_idle_time >= BLOCKED_TIME_THRESHOLD) { - return true; - } - } - return false; - } - - void LocalizationStage::CleanActor(const ActorId actor_id) { - track_traffic.DeleteActor(actor_id); - for (const auto& waypoint : buffer_list->at(actor_id)) { - track_traffic.RemovePassingVehicle(waypoint->GetId(), actor_id); - } - - idle_time.erase(actor_id); - buffer_list->erase(actor_id); - } - - bool LocalizationStage::TryDestroyVehicle(const Actor& actor) { - if (!actor->IsAlive()) { - return false; - } - - const ActorId actor_id = actor->GetId(); - - auto delta_last_actor_destruction = current_timestamp.elapsed_seconds - elapsed_last_actor_destruction; - if (delta_last_actor_destruction >= DELTA_TIME_BETWEEN_DESTRUCTIONS) { - registered_actors.Destroy(actor); - - // Clean actor data structures. - CleanActor(actor_id); - - elapsed_last_actor_destruction = current_timestamp.elapsed_seconds; - - return true; - } - return false; - } - - void LocalizationStage::UpdateSwarmVelocities() { - - // Location of hero vehicle if present. - std::vector hero_locations; - if (hybrid_physics_mode && hero_actors.size() != 0u) { - for (auto &hero_actor_info: hero_actors) { - hero_locations.push_back(hero_actor_info.second->GetLocation()); - } - } - - // Using (1/20)s time delta for computing velocity. - float dt = HYBRID_MODE_DT; - // Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode. - if (!parameters.GetSynchronousMode()) { - TimePoint current_instance = chr::system_clock::now(); - chr::duration elapsed_time = current_instance - previous_update_instance; - if (elapsed_time.count() > dt) { - previous_update_instance = current_instance; - } else if (hybrid_physics_mode) { - return; - } - } - - // Update velocity for all registered vehicles. - for (const Actor &actor: actor_list) { - - ActorId actor_id = actor->GetId(); - cg::Location vehicle_location = actor->GetLocation(); - - // Adding entry if not present. - if (kinematic_state_map.find(actor_id) == kinematic_state_map.end()) { - kinematic_state_map.insert({actor_id, KinematicState{true, vehicle_location, cg::Vector3D()}}); - } - - // Check if current actor is in range of hero actor and enable physics in hybrid mode. - bool in_range_of_hero_actor = false; - if (hybrid_physics_mode && hero_actors.size() != 0u){ - for (auto &hero_location: hero_locations) { - if (cg::Math::DistanceSquared(vehicle_location, hero_location) < std::pow(hybrid_physics_radius, 2)) { - in_range_of_hero_actor = true; - break; - } - } - } - bool enable_physics = hybrid_physics_mode? in_range_of_hero_actor: true; - kinematic_state_map.at(actor_id).physics_enabled = enable_physics; - actor->SetSimulatePhysics(enable_physics); - - // When we say velocity, we usually mean velocity for a vehicle along it's heading. - // Velocity component due to rotation can be removed by taking dot product with heading vector. - cg::Vector3D heading = actor->GetTransform().GetForwardVector(); - if (enable_physics) { - - kinematic_state_map.at(actor_id).velocity = cg::Math::Dot(actor->GetVelocity(), heading) * heading; - - } else { - - cg::Vector3D displacement = (vehicle_location - kinematic_state_map.at(actor_id).location); - cg::Vector3D displacement_along_heading = cg::Math::Dot(displacement, heading) * heading; - cg::Vector3D velocity = displacement_along_heading/dt; - kinematic_state_map.at(actor_id).velocity = velocity; - } - - // Updating location after velocity is computed. - kinematic_state_map.at(actor_id).location = vehicle_location; - } - } - - cg::Vector3D LocalizationStage::GetVelocity(ActorId actor_id) { - cg::Vector3D vel; - if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) { - vel = kinematic_state_map.at(actor_id).velocity; - } else if (unregistered_actors.find(actor_id) != unregistered_actors.end()) { - vel = unregistered_actors.at(actor_id)->GetVelocity(); - } - return vel; - } - - bool LocalizationStage::IsPhysicsEnabled(ActorId actor_id) { - bool enabled = true; - if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) { - enabled = kinematic_state_map.at(actor_id).physics_enabled; - } - return enabled; - } +} } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index 6bf5ff414..e3ada46ea 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -1,206 +1,74 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . #pragma once -#include -#include -#include -#include -#include #include -#include -#include -#include -#include "carla/StringUtil.h" +#include "carla/client/DebugHelper.h" -#include "carla/client/Actor.h" -#include "carla/client/Vehicle.h" -#include "carla/geom/Location.h" -#include "carla/geom/Math.h" -#include "carla/geom/Transform.h" -#include "carla/geom/Vector3D.h" -#include "carla/Memory.h" -#include "carla/rpc/ActorId.h" - -#include "carla/trafficmanager/AtomicActorSet.h" +#include "carla/trafficmanager/DataStructures.h" #include "carla/trafficmanager/InMemoryMap.h" #include "carla/trafficmanager/LocalizationUtils.h" -#include "carla/trafficmanager/MessengerAndDataTypes.h" #include "carla/trafficmanager/Parameters.h" -#include "carla/trafficmanager/PipelineStage.h" -#include "carla/trafficmanager/SimpleWaypoint.h" -#include "carla/trafficmanager/PerformanceDiagnostics.h" - -#include "carla/client/detail/ActorVariant.h" -#include "carla/client/detail/EpisodeProxy.h" -#include "carla/client/detail/Simulator.h" +#include "carla/trafficmanager/RandomGenerator.h" +#include "carla/trafficmanager/TrackTraffic.h" +#include "carla/trafficmanager/SimulationState.h" +#include "carla/trafficmanager/Stage.h" namespace carla { namespace traffic_manager { - namespace cc = carla::client; - namespace cg = carla::geom; - namespace chr = std::chrono; - using namespace chr; - using Actor = carla::SharedPtr; - using Vehicle = carla::SharedPtr; - using ActorId = carla::ActorId; - using ActorIdSet = std::unordered_set; - using TLS = carla::rpc::TrafficLightState; +namespace cc = carla::client; - /// Structure to hold kinematic state of actors. - struct KinematicState { - bool physics_enabled; - cg::Location location; - cg::Vector3D velocity; - }; +using LocalMapPtr = std::shared_ptr; +using LaneChangeLocationMap = std::unordered_map; - /// This class is responsible for maintaining a horizon of waypoints ahead - /// of the vehicle for it to follow. - /// The class is also responsible for managing lane change decisions and - /// modify the waypoint trajectory appropriately. - class LocalizationStage : public PipelineStage { +/// This class has functionality to maintain a horizon of waypoints ahead +/// of the vehicle for it to follow. +/// The class is also responsible for managing lane change decisions and +/// modify the waypoint trajectory appropriately. +class LocalizationStage : Stage { +private: + const std::vector &vehicle_id_list; + BufferMap &buffer_map; + const SimulationState &simulation_state; + TrackTraffic &track_traffic; + const LocalMapPtr &local_map; + Parameters ¶meters; + LocalizationFrame &output_array; + cc::DebugHelper &debug_helper; + LaneChangeLocationMap last_lane_change_location; + ActorIdSet vehicles_at_junction; + using SimpleWaypointPair = std::pair; + std::unordered_map vehicles_at_junction_entrance; + RandomGenerator<> pgen; - private: + SimpleWaypointPtr AssignLaneChange(const ActorId actor_id, + const cg::Location vehicle_location, + const float vehicle_speed, + bool force, bool direction); - /// Section keys to switch between the output data frames. - bool planner_frame_selector; - bool collision_frame_selector; - bool traffic_light_frame_selector; - /// Output data frames to be shared with the motion planner stage. - std::shared_ptr planner_frame_a; - std::shared_ptr planner_frame_b; - /// Output data frames to be shared with the collision stage. - std::shared_ptr collision_frame_a; - std::shared_ptr collision_frame_b; - /// Output data frames to be shared with the traffic light stage - std::shared_ptr traffic_light_frame_a; - std::shared_ptr traffic_light_frame_b; - /// Pointer to messenger to motion planner stage. - std::shared_ptr planner_messenger; - /// Pointer to messenger to collision stage. - std::shared_ptr collision_messenger; - /// Pointer to messenger to traffic light stage. - std::shared_ptr traffic_light_messenger; - /// Reference to set of all actors registered with the traffic manager. - AtomicActorSet ®istered_actors; - /// List of actors registered with the traffic manager in - /// current update cycle. - std::vector actor_list; - /// State counter to track changes in registered actors. - int registered_actors_state; - /// Reference to local map-cache object. - InMemoryMap &local_map; - /// Runtime parameterization object. - Parameters ¶meters; - /// Reference to Carla's debug helper object. - cc::DebugHelper &debug_helper; - /// Reference to carla client connection object. - carla::client::detail::EpisodeProxy episode_proxy_ls; - /// Structures to hold waypoint buffers for all vehicles. - /// These are shared with the collisions stage. - std::shared_ptr buffer_list; - /// Map connecting actor ids to indices of data arrays. - std::unordered_map vehicle_id_to_index; - /// Number of vehicles currently registered with the traffic manager. - uint64_t number_of_vehicles; - /// Used to only calculate the extended buffer once at junctions - std::map approached; - /// Point used to know if the junction has free space after its end, mapped to their respective actor id - std::map final_safe_points; - /// Object for tracking paths of the traffic vehicles. - TrackTraffic track_traffic; - /// Map of all vehicles' idle time. - std::unordered_map idle_time; - /// Structure to hold the actor with the maximum idle time at each iteration. - std::pair maximum_idle_time; - /// Variable to hold current timestamp from the world snapshot. - cc::Timestamp current_timestamp; - /// Simulated seconds since the beginning of the current episode when the last actor was destroyed. - double elapsed_last_actor_destruction = 0.0; - /// Counter to track unregistered actors' scan interval. - uint64_t unregistered_scan_duration = 0; - /// A structure used to keep track of actors spawned outside of traffic - /// manager. - std::unordered_map unregistered_actors; - /// Code snippet execution time profiler. - SnippetProfiler snippet_profiler; - /// Map to keep track of last lane change location. - std::unordered_map last_lane_change_location; - /// Records of all vehicles with hero attribute. - std::unordered_map hero_actors; - /// Switch indicating hybrid physics mode. - bool hybrid_physics_mode {false}; - /// Switch indicating hybrid physics mode. - float hybrid_physics_radius {70.0f}; - /// Structure to hold previous state of physics-less vehicle. - std::unordered_map kinematic_state_map; - /// Time instance used to calculate dt in asynchronous mode. - TimePoint previous_update_instance; - /// Step runner flag. - std::atomic run_step {false}; - /// Mutex for progressing synchronous execution. - std::mutex step_execution_mutex; - /// Condition variables for progressing synchronous execution. - std::condition_variable step_execution_trigger; + void DrawBuffer(Buffer &buffer); - /// A simple method used to draw waypoint buffer ahead of a vehicle. - void DrawBuffer(Buffer &buffer); + void ExtendAndFindSafeSpace(const ActorId actor_id, + const bool is_at_junction_entrance, + Buffer &waypoint_buffer); - /// Method to determine lane change and obtain target lane waypoint. - SimpleWaypointPtr AssignLaneChange(Actor vehicle, const cg::Location &vehicle_location, bool force, bool direction); +public: + LocalizationStage(const std::vector &vehicle_id_list, + BufferMap &buffer_map, + const SimulationState &simulation_state, + TrackTraffic &track_traffic, + const LocalMapPtr &local_map, + Parameters ¶meters, + LocalizationFrame &output_array, + cc::DebugHelper &debug_helper); - // When near an intersection, extends the buffer throughout all the - // intersection to see if there is space after it - SimpleWaypointPtr GetSafeLocationAfterJunction(const Vehicle &vehicle, Buffer &waypoint_buffer); + void Update(const unsigned long index) override; - /// Methods to modify waypoint buffer and track traffic. - void PushWaypoint(Buffer& buffer, ActorId actor_id, SimpleWaypointPtr& waypoint); - void PopWaypoint(Buffer& buffer, ActorId actor_id, bool front_or_back = true); + void RemoveActor(const ActorId actor_id) override; - /// Method to scan for unregistered actors and update their grid positioning. - void ScanUnregisteredVehicles(); - - /// Methods for idle vehicle elimination. - void UpdateIdleTime(const Actor& actor); - bool IsVehicleStuck(const Actor& actor); - void CleanActor(const ActorId actor_id); - bool TryDestroyVehicle(const Actor& actor); - - /// Methods for actor state management. - void UpdateSwarmVelocities(); - cg::Vector3D GetVelocity(ActorId actor_id); - bool IsPhysicsEnabled(ActorId actor_id); - - public: - - LocalizationStage( - std::string stage_name, - std::shared_ptr planner_messenger, - std::shared_ptr collision_messenger, - std::shared_ptr traffic_light_messenger, - AtomicActorSet ®istered_actors, - InMemoryMap &local_map, - Parameters ¶meters, - carla::client::DebugHelper &debug_helper, - carla::client::detail::EpisodeProxy &episodeProxy); - - ~LocalizationStage(); - - void DataReceiver() override; - - void Action() override; - - void DataSender() override; - - /// Method to trigger initiation of pipeline in synchronous mode. - bool RunStep(); - }; + void Reset() override; +}; } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp index 47f79127b..cba56552a 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp @@ -9,258 +9,86 @@ namespace carla { namespace traffic_manager { -namespace LocalizationConstants { - const static uint64_t BUFFER_STEP_THROUGH = 10u; - const static float SAMPLING_RESOLUTION = 0.1f; -} // namespace LocalizationConstants +float DeviationCrossProduct(const cg::Location &reference_location, + const cg::Vector3D &heading_vector, + const cg::Location &target_location) { + cg::Vector3D next_vector = target_location - reference_location; + float vector_magnitude_epsilon = 2.0f * std::numeric_limits::epsilon(); + next_vector = next_vector.MakeSafeUnitVector(vector_magnitude_epsilon); + const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x; + return cross_z; +} - using namespace LocalizationConstants; +float DeviationDotProduct(const cg::Location &reference_location, + const cg::Vector3D &heading_vector, + const cg::Location &target_location) { + cg::Vector3D next_vector = target_location - reference_location; + float vector_magnitude_epsilon = 2.0f * std::numeric_limits::epsilon(); + next_vector.z = 0.0f; + next_vector = next_vector.MakeSafeUnitVector(vector_magnitude_epsilon); + cg::Vector3D heading_vector_flat(heading_vector.x, heading_vector.y, 0); + heading_vector_flat = heading_vector_flat.MakeSafeUnitVector(vector_magnitude_epsilon); + const float dot_product = cg::Math::Dot(next_vector, heading_vector_flat); + return dot_product; +} - float DeviationCrossProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location) { +void PushWaypoint(ActorId actor_id, TrackTraffic &track_traffic, + Buffer &buffer, SimpleWaypointPtr &waypoint) { - cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); - cg::Location next_vector = target_location - vehicle_location; - next_vector = next_vector.MakeSafeUnitVector(2 * std::numeric_limits::epsilon()); + const uint64_t waypoint_id = waypoint->GetId(); + buffer.push_back(waypoint); + track_traffic.UpdatePassingVehicle(waypoint_id, actor_id); +} - const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x; +void PopWaypoint(ActorId actor_id, TrackTraffic &track_traffic, + Buffer &buffer, bool front_or_back) { - return cross_z; + SimpleWaypointPtr removed_waypoint = front_or_back ? buffer.front() : buffer.back(); + const uint64_t removed_waypoint_id = removed_waypoint->GetId(); + if (front_or_back) { + buffer.pop_front(); + } else { + buffer.pop_back(); } + track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id); +} - float DeviationDotProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location, bool rear_offset) { +TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance) { - cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector(); - cg::Location next_vector; + SimpleWaypointPtr target_waypoint = waypoint_buffer.front(); + const SimpleWaypointPtr &buffer_front = waypoint_buffer.front(); + uint64_t startPosn = static_cast(std::fabs(target_point_distance * INV_MAP_RESOLUTION)); + uint64_t index = 0; + /// Condition to determine forward or backward scanning of WayPoint Buffer. - if (!rear_offset) { - next_vector = target_location - vehicle_location; - } else { - const auto vehicle_ptr = boost::static_pointer_cast(actor); - const float vehicle_half_length = vehicle_ptr->GetBoundingBox().extent.x; - next_vector = target_location - (cg::Location(-1* vehicle_half_length * heading_vector) - + vehicle_location); - } - next_vector = next_vector.MakeSafeUnitVector(2 * std::numeric_limits::epsilon()); - - const float dot_product = cg::Math::Dot(next_vector, heading_vector); - - return dot_product; - } - - TrackTraffic::TrackTraffic() {} - - void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id, - const std::vector waypoints) { - - // Add actor entry, if not present. - if (actor_to_grids.find(actor_id) == actor_to_grids.end()) { - actor_to_grids.insert({actor_id, {}}); + if (startPosn < waypoint_buffer.size()) { + bool mScanForward = false; + const float target_point_dist_power = target_point_distance * target_point_distance; + if (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power) { + mScanForward = true; } - std::unordered_set& current_grids = actor_to_grids.at(actor_id); - - // Clear current actor from all grids containing itself. - for (auto& grid_id: current_grids) { - if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { - ActorIdSet& actor_ids = grid_to_actors.at(grid_id); - if (actor_ids.find(actor_id) != actor_ids.end()) { - actor_ids.erase(actor_id); - } - } - } - - // Clear all grids current actor is tracking. - current_grids.clear(); - - // Step through buffer and update grid list for actor and actor list for grids. - for (auto &waypoint: waypoints) { - - GeoGridId ggid = waypoint->GetGeodesicGridId(); - current_grids.insert(ggid); - - // Add grid entry, if not present. - if (grid_to_actors.find(ggid) == grid_to_actors.end()) { - grid_to_actors.insert({ggid, {}}); - } - - ActorIdSet& actor_ids = grid_to_actors.at(ggid); - if (actor_ids.find(actor_id) == actor_ids.end()) { - actor_ids.insert(actor_id); - } - } - } - - - void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer& buffer) { - - if (!buffer.empty()) { - - // Add actor entry, if not present. - if (actor_to_grids.find(actor_id) == actor_to_grids.end()) { - actor_to_grids.insert({actor_id, {}}); - } - - std::unordered_set& current_grids = actor_to_grids.at(actor_id); - - // Clear current actor from all grids containing itself. - for (auto& grid_id: current_grids) { - if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { - ActorIdSet& actor_ids = grid_to_actors.at(grid_id); - if (actor_ids.find(actor_id) != actor_ids.end()) { - actor_ids.erase(actor_id); - } - } - } - - // Clear all grids the current actor is tracking. - current_grids.clear(); - - // Step through buffer and update grid list for actor and actor list for grids. - uint64_t buffer_size = buffer.size(); - uint64_t step_size = static_cast(std::floor(buffer_size/BUFFER_STEP_THROUGH)); - for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) { - GeoGridId ggid = buffer.at(std::min(i* step_size, buffer_size-1u))->GetGeodesicGridId(); - current_grids.insert(ggid); - - // Add grid entry if not present. - if (grid_to_actors.find(ggid) == grid_to_actors.end()) { - grid_to_actors.insert({ggid, {}}); - } - - ActorIdSet& actor_ids = grid_to_actors.at(ggid); - if (actor_ids.find(actor_id) == actor_ids.end()) { - actor_ids.insert(actor_id); - } - } - } - } - - ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) { - - ActorIdSet actor_id_set; - - if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { - std::unordered_set& grid_ids = actor_to_grids.at(actor_id); - for (auto& grid_id: grid_ids) { - if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { - ActorIdSet& actor_ids = grid_to_actors.at(grid_id); - actor_id_set.insert(actor_ids.begin(), actor_ids.end()); - } - } - } - - return actor_id_set; - } - - void TrackTraffic::DeleteActor(ActorId actor_id) { - - if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { - std::unordered_set& grid_ids = actor_to_grids.at(actor_id); - for (auto& grid_id: grid_ids) { - if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { - ActorIdSet& actor_ids = grid_to_actors.at(grid_id); - if (actor_ids.find(actor_id) != actor_ids.end()) { - actor_ids.erase(actor_id); - } - } - } - actor_to_grids.erase(actor_id); - } - } - - std::unordered_set TrackTraffic::GetGridIds(ActorId actor_id) { - - std::unordered_set grid_ids; - - if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { - grid_ids = actor_to_grids.at(actor_id); - } - - return grid_ids; - } - - std::unordered_map TrackTraffic::GetGridActors() { - return grid_to_actors; - } - - void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) { - - if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) { - - ActorIdSet& actor_id_set = waypoint_overlap_tracker.at(waypoint_id); - if (actor_id_set.find(actor_id) == actor_id_set.end()) { - actor_id_set.insert(actor_id); + if (mScanForward) { + for (uint64_t i = startPosn; + (i < waypoint_buffer.size()) && (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power); + ++i) { + target_waypoint = waypoint_buffer.at(i); + index = i; } } else { - - waypoint_overlap_tracker.insert({waypoint_id, {actor_id}}); + for (uint64_t i = startPosn; + (buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power); + --i) { + target_waypoint = waypoint_buffer.at(i); + index = i; + } } + } else { + target_waypoint = waypoint_buffer.back(); + index = waypoint_buffer.size() - 1; } - - void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) { - - if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) { - - auto& actor_id_set = waypoint_overlap_tracker.at(waypoint_id); - if (actor_id_set.find(actor_id) != actor_id_set.end()) { - actor_id_set.erase(actor_id); - } - - if (actor_id_set.size() == 0) { - waypoint_overlap_tracker.erase(waypoint_id); - } - } - } - - ActorIdSet TrackTraffic::GetPassingVehicles(uint64_t waypoint_id) { - - if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) { - return waypoint_overlap_tracker.at(waypoint_id); - } else { - return ActorIdSet(); - } - - } - -std::pair TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) { - - SimpleWaypointPtr target_waypoint = waypoint_buffer.front(); - const SimpleWaypointPtr& buffer_front = waypoint_buffer.front(); - uint64_t startPosn = static_cast(std::fabs(target_point_distance/SAMPLING_RESOLUTION)); - uint64_t index = 0; - /// Condition to determine forward or backward scanning of WayPoint Buffer. - - if (startPosn < waypoint_buffer.size()) { - bool mScanForward = false; - double target_point_dist_power = std::pow(target_point_distance, 2); - if(buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power) { - mScanForward = true; - } - - if(mScanForward) { - for (uint64_t i = startPosn; - (i < waypoint_buffer.size()) - && (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power); - ++i) { - target_waypoint = waypoint_buffer.at(i); - index = i; - } - } - else { - for (uint64_t i = startPosn; - (buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power); - --i) { - target_waypoint = waypoint_buffer.at(i); - index = i; - } - } - } - else { - target_waypoint = waypoint_buffer.back(); - index = waypoint_buffer.size() - 1; - } return std::make_pair(target_waypoint, index); - } +} } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h index a005b3e2b..ce100641d 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h @@ -14,7 +14,9 @@ #include "carla/road/RoadTypes.h" #include "carla/rpc/ActorId.h" +#include "carla/trafficmanager/Constants.h" #include "carla/trafficmanager/SimpleWaypoint.h" +#include "carla/trafficmanager/TrackTraffic.h" namespace carla { namespace traffic_manager { @@ -27,50 +29,32 @@ namespace traffic_manager { using SimpleWaypointPtr = std::shared_ptr; using Buffer = std::deque; using GeoGridId = carla::road::JuncId; - - class TrackTraffic{ - - private: - /// Structure to keep track of overlapping waypoints between vehicles. - using WaypointOverlap = std::unordered_map; - WaypointOverlap waypoint_overlap_tracker; - /// Stored vehicle id set record. - ActorIdSet actor_id_set_record; - /// Geodesic grids occupied by actors's paths. - std::unordered_map> actor_to_grids; - /// Actors currently passing through grids. - std::unordered_map grid_to_actors; - - public: - TrackTraffic(); - - /// Methods to update, remove and retrieve vehicles passing through a waypoint. - void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id); - void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id); - ActorIdSet GetPassingVehicles(uint64_t waypoint_id); - - void UpdateGridPosition(const ActorId actor_id, const Buffer& buffer); - void UpdateUnregisteredGridPosition(const ActorId actor_id, - const std::vector waypoints); - /// Method to return the wayPoints from the waypoint Buffer by using target point distance - std::pair GetTargetWaypoint(const Buffer& waypoint_buffer, const float& target_point_distance); - - ActorIdSet GetOverlappingVehicles(ActorId actor_id); - /// Method to delete actor data from tracking. - void DeleteActor(ActorId actor_id); - - std::unordered_set GetGridIds(ActorId actor_id); - - std::unordered_map GetGridActors(); - }; + using constants::Map::MAP_RESOLUTION; + using constants::Map::INV_MAP_RESOLUTION; /// Returns the cross product (z component value) between the vehicle's /// heading vector and the vector along the direction to the next /// target waypoint on the horizon. - float DeviationCrossProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location); + float DeviationCrossProduct(const cg::Location &reference_location, + const cg::Vector3D &heading_vector, + const cg::Location &target_location); /// Returns the dot product between the vehicle's heading vector and /// the vector along the direction to the next target waypoint on the horizon. - float DeviationDotProduct(Actor actor, const cg::Location &vehicle_location, const cg::Location &target_location, bool rear_offset=false); + float DeviationDotProduct(const cg::Location &reference_location, + const cg::Vector3D &heading_vector, + const cg::Location &target_location); + + // Function to add a waypoint to a path buffer and update waypoint tracking. + void PushWaypoint(ActorId actor_id, TrackTraffic& track_traffic, + Buffer& buffer, SimpleWaypointPtr& waypoint); + + // Function to remove a waypoint from a path buffer and update waypoint tracking. + void PopWaypoint(ActorId actor_id, TrackTraffic& track_traffic, + Buffer& buffer, bool front_or_back=true); + + /// Method to return the wayPoints from the waypoint Buffer by using target point distance + using TargetWPInfo = std::pair; + TargetWPInfo GetTargetWaypoint(const Buffer& waypoint_buffer, const float& target_point_distance); } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/Messenger.h b/LibCarla/source/carla/trafficmanager/Messenger.h deleted file mode 100644 index 02049c3f5..000000000 --- a/LibCarla/source/carla/trafficmanager/Messenger.h +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include -#include -#include -#include -#include - -namespace carla { -namespace traffic_manager { - - using namespace std::chrono_literals; - - template - class Messenger { - - private: - - /// Flag used to wake up and join any waiting function calls on this object. - std::atomic stop_messenger; - /// Member used to hold data sent by the sender. - std::deque d_queue; - /// Mutex used to manage contention between the sender and receiver. - std::mutex data_modification_mutex; - /// Variable to conditionally block sender in case waiting for the receiver. - std::condition_variable send_condition; - /// Variable to conditionally block receiver in case waiting for the sender. - std::condition_variable receive_condition; - - public: - - Messenger() { - stop_messenger.store(false); - } - ~Messenger() {} - - void Push(Data data) { - - std::unique_lock lock(data_modification_mutex); - while (!d_queue.empty() && !stop_messenger.load()) { - send_condition.wait_for(lock, 1ms, [=] { - return (d_queue.empty() && stop_messenger.load()); - }); - } - if(!stop_messenger.load()){ - d_queue.push_front(data); - receive_condition.notify_one(); - } - } - - Data Peek() { - - std::unique_lock lock(data_modification_mutex); - while (d_queue.empty() && !stop_messenger.load()) { - receive_condition.wait_for(lock, 1ms, [=] { - return (!d_queue.empty() && stop_messenger.load()); - }); - } - - if(!stop_messenger.load()) { - Data data = d_queue.back(); - return data; - } - return Data(); - } - - void Pop() { - - std::unique_lock lock(data_modification_mutex); - if (!(d_queue.empty() && stop_messenger.load())) { - d_queue.pop_back(); - send_condition.notify_one(); - } - } - - void Start() { - stop_messenger.store(false); - } - - void Stop() { - stop_messenger.store(true); - d_queue.clear(); - send_condition.notify_one(); - receive_condition.notify_one(); - } - - }; - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h b/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h deleted file mode 100644 index 55111a488..000000000 --- a/LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h +++ /dev/null @@ -1,113 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include -#include -#include -#include - -#include "carla/client/Actor.h" -#include "carla/geom/Vector3D.h" -#include "carla/geom/Transform.h" -#include "carla/Memory.h" -#include "carla/rpc/ActorId.h" - -#include "carla/trafficmanager/Messenger.h" -#include "carla/trafficmanager/SimpleWaypoint.h" - -namespace carla { -namespace traffic_manager { - - namespace cc = carla::client; - namespace cg = carla::geom; - - /// Convenience typing. - - /// Alias for waypoint buffer used in the localization stage. - using Buffer = std::deque>; - /// Alias used for the list of buffers in the localization stage. - using BufferList = std::unordered_map; - - using Actor = carla::SharedPtr; - using ActorId = carla::ActorId; - - /// Data types. - - /// Type of data sent by the localization stage to the motion planner stage. - struct LocalizationToPlannerData { - Actor actor; - float deviation; - float distance; - bool approaching_true_junction; - cg::Vector3D velocity; - bool physics_enabled; - std::vector> position_window; - }; - - /// Type of data sent by the motion planner stage to the batch control stage. - struct PlannerToControlData { - Actor actor; - float throttle; - float brake; - float steer; - bool physics_enabled; - cg::Transform transform; - }; - - /// Type of data sent by the localization stage to the collision stage. - struct LocalizationToCollisionData { - Actor actor; - Buffer buffer; - std::vector> overlapping_actors; - std::shared_ptr safe_point_after_junction; - std::shared_ptr closest_waypoint; - std::shared_ptr junction_look_ahead_waypoint; - cg::Vector3D velocity; - }; - - /// Type of data sent by the collision stage to the motion planner stage. - struct CollisionToPlannerData { - bool hazard; - float distance_to_other_vehicle; - cg::Vector3D other_vehicle_velocity; - }; - - /// Type of data sent by the localization stage to the traffic light stage. - struct LocalizationToTrafficLightData { - Actor actor; - std::shared_ptr closest_waypoint; - std::shared_ptr junction_look_ahead_waypoint; - }; - - /// Type of data sent by the traffic light stage to the motion planner stage. - struct TrafficLightToPlannerData { - bool traffic_light_hazard; - }; - - /// Data frame types. - - /// Array types of data flowing between stages. - - using LocalizationToPlannerFrame = std::vector; - using PlannerToControlFrame = std::vector; - using LocalizationToCollisionFrame = std::vector; - using LocalizationToTrafficLightFrame = std::vector; - using CollisionToPlannerFrame = std::vector; - using TrafficLightToPlannerFrame = std::vector; - - /// Messenger types - - using LocalizationToPlannerMessenger = Messenger>; - using PlannerToControlMessenger = Messenger>; - using LocalizationToCollisionMessenger = Messenger>; - using LocalizationToTrafficLightMessenger = Messenger>; - using CollisionToPlannerMessenger = Messenger>; - using TrafficLightToPlannerMessenger = Messenger>; - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp new file mode 100644 index 000000000..bcb99d433 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -0,0 +1,290 @@ + +#include "carla/trafficmanager/Constants.h" +#include "carla/trafficmanager/PIDController.h" + +#include "carla/trafficmanager/MotionPlanStage.h" + +namespace carla { +namespace traffic_manager { + +using namespace constants::MotionPlan; +using namespace constants::WaypointSelection; +using namespace constants::SpeedThreshold; + +using constants::HybridMode::HYBRID_MODE_DT; + +MotionPlanStage::MotionPlanStage( + const std::vector &vehicle_id_list, + const SimulationState &simulation_state, + const Parameters ¶meters, + const BufferMap &buffer_map, + const TrackTraffic &track_traffic, + const std::vector &urban_longitudinal_parameters, + const std::vector &highway_longitudinal_parameters, + const std::vector &urban_lateral_parameters, + const std::vector &highway_lateral_parameters, + const LocalizationFrame &localization_frame, + const CollisionFrame&collision_frame, + const TLFrame &tl_frame, + ControlFrame &output_array) + : vehicle_id_list(vehicle_id_list), + simulation_state(simulation_state), + parameters(parameters), + buffer_map(buffer_map), + track_traffic(track_traffic), + urban_longitudinal_parameters(urban_longitudinal_parameters), + highway_longitudinal_parameters(highway_longitudinal_parameters), + urban_lateral_parameters(urban_lateral_parameters), + highway_lateral_parameters(highway_lateral_parameters), + localization_frame(localization_frame), + collision_frame(collision_frame), + tl_frame(tl_frame), + output_array(output_array) {} + +void MotionPlanStage::Update(const unsigned long index) { + const ActorId actor_id = vehicle_id_list.at(index); + const cg::Location ego_location = simulation_state.GetLocation(actor_id); + const cg::Vector3D ego_velocity = simulation_state.GetVelocity(actor_id); + const float ego_speed = ego_velocity.Length(); + const cg::Vector3D ego_heading = simulation_state.GetHeading(actor_id); + const bool ego_physics_enabled = simulation_state.IsPhysicsEnabled(actor_id); + const Buffer &waypoint_buffer = buffer_map.at(actor_id); + const LocalizationData &localization = localization_frame.at(index); + const CollisionHazardData &collision_hazard = collision_frame.at(index); + const bool &tl_hazard = tl_frame.at(index); + + const float target_point_distance = std::max(ego_speed * TARGET_WAYPOINT_TIME_HORIZON, + TARGET_WAYPOINT_HORIZON_LENGTH); + const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first; + const cg::Location target_location = target_waypoint->GetLocation(); + float dot_product = DeviationDotProduct(ego_location, ego_heading, target_location); + float cross_product = DeviationCrossProduct(ego_location, ego_heading, target_location); + dot_product = 1.0f - dot_product; + if (cross_product < 0.0f) { + dot_product *= -1.0f; + } + const float current_deviation = dot_product; + + // If previous state for vehicle not found, initialize state entry. + if (pid_state_map.find(actor_id) == pid_state_map.end()) { + const auto initial_state = StateEntry{chr::system_clock::now(), 0.0f, 0.0f, 0.0f, 0.0f}; + pid_state_map.insert({actor_id, initial_state}); + } + + // Retrieving the previous state. + traffic_manager::StateEntry previous_state; + previous_state = pid_state_map.at(actor_id); + + // Select PID parameters. + std::vector longitudinal_parameters; + std::vector lateral_parameters; + if (ego_speed > HIGHWAY_SPEED) { + longitudinal_parameters = highway_longitudinal_parameters; + lateral_parameters = highway_lateral_parameters; + } else { + longitudinal_parameters = urban_longitudinal_parameters; + lateral_parameters = urban_lateral_parameters; + } + + // Target velocity for vehicle. + const float ego_speed_limit = simulation_state.GetSpeedLimit(actor_id); + float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, ego_speed_limit) / 3.6f; + + // Collision handling and target velocity correction. + std::pair collision_response = CollisionHandling(collision_hazard, tl_hazard, ego_velocity, + ego_heading, max_target_velocity); + bool collision_emergency_stop = collision_response.first; + float dynamic_target_velocity = collision_response.second; + + + // Don't enter junction if there isn't enough free space after the junction. + bool safe_after_junction = SafeAfterJunction(localization, tl_hazard, collision_emergency_stop); + + // In case of collision or traffic light hazard. + bool emergency_stop = tl_hazard || collision_emergency_stop || !safe_after_junction; + + ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f}; + cg::Transform teleportation_transform; + + // If physics is enabled for the vehicle, use PID controller. + const auto current_time = chr::system_clock::now(); + StateEntry current_state; + if (ego_physics_enabled) { + + // State update for vehicle. + current_state = PID::StateUpdate(previous_state, ego_speed, dynamic_target_velocity, + current_deviation, current_time); + + // Controller actuation. + actuation_signal = PID::RunStep(current_state, previous_state, + longitudinal_parameters, lateral_parameters); + + if (emergency_stop) { + + current_state.deviation_integral = 0.0f; + current_state.velocity_integral = 0.0f; + actuation_signal.throttle = 0.0f; + actuation_signal.brake = 1.0f; + } + } + // For physics-less vehicles, determine position and orientation for teleportation. + else { + // Flushing controller state for vehicle. + current_state = {chr::system_clock::now(), + 0.0f, 0.0f, + 0.0f, 0.0f}; + + // Add entry to teleportation duration clock table if not present. + if (teleportation_instance.find(actor_id) == teleportation_instance.end()) { + teleportation_instance.insert({actor_id, chr::system_clock::now()}); + } + + // Measuring time elapsed since last teleportation for the vehicle. + chr::duration elapsed_time = current_time - teleportation_instance.at(actor_id); + + // Find a location ahead of the vehicle for teleportation to achieve intended velocity. + if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time.count() > HYBRID_MODE_DT)) { + + // Target displacement magnitude to achieve target velocity. + const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT; + const SimpleWaypointPtr teleport_target_waypoint = GetTargetWaypoint(waypoint_buffer, target_displacement).first; + + // Construct target transform to accurately achieve desired velocity. + float missing_displacement = 0.0f; + const float base_displacement = teleport_target_waypoint->Distance(ego_location); + if (base_displacement < target_displacement) { + missing_displacement = target_displacement - base_displacement; + } + cg::Transform target_base_transform = teleport_target_waypoint->GetTransform(); + cg::Location target_base_location = target_base_transform.location; + cg::Vector3D target_heading = target_base_transform.GetForwardVector(); + cg::Location teleportation_location = target_base_location + cg::Location(target_heading * missing_displacement); + teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation); + } + // In case of an emergency stop, stay in the same location. + // Also, teleport only once every dt in asynchronous mode. + else { + teleportation_transform = cg::Transform(ego_location, simulation_state.GetRotation(actor_id)); + } + } + + // Updating PID state. + StateEntry &state = pid_state_map.at(actor_id); + state = current_state; + + // Constructing the actuation signal. + if (ego_physics_enabled) { + carla::rpc::VehicleControl vehicle_control; + vehicle_control.throttle = actuation_signal.throttle; + vehicle_control.brake = actuation_signal.brake; + vehicle_control.steer = actuation_signal.steer; + + output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control); + } else { + output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform); + } +} + +bool MotionPlanStage::SafeAfterJunction(const LocalizationData &localization, + const bool tl_hazard, + const bool collision_emergency_stop) { + + SimpleWaypointPtr junction_end_point = localization.junction_end_point; + SimpleWaypointPtr safe_point = localization.safe_point; + + bool safe_after_junction = true; + + if (!tl_hazard && !collision_emergency_stop + && localization.is_at_junction_entrance + && junction_end_point != nullptr && safe_point != nullptr + && junction_end_point->DistanceSquared(safe_point) > SQUARE(MIN_SAFE_INTERVAL_LENGTH)) { + + ActorIdSet initial_set = track_traffic.GetPassingVehicles(junction_end_point->GetId()); + float safe_interval_length_squared = junction_end_point->DistanceSquared(safe_point); + cg::Location mid_point = (junction_end_point->GetLocation() + safe_point->GetLocation())/2.0f; + + // Scan through the safe interval and find if any vehicles are present in it + // by finding their occupied waypoints. + for (SimpleWaypointPtr current_waypoint = junction_end_point; + current_waypoint->DistanceSquared(junction_end_point) < safe_interval_length_squared && safe_after_junction; + current_waypoint = current_waypoint->GetNextWaypoint().front()) { + + ActorIdSet current_set = track_traffic.GetPassingVehicles(current_waypoint->GetId()); + ActorIdSet difference; + std::set_difference(current_set.begin(), current_set.end(), + initial_set.begin(), initial_set.end(), + std::inserter(difference, difference.begin())); + if (difference.size() > 0) { + for (const ActorId &blocking_id: difference) { + cg::Location blocking_actor_location = simulation_state.GetLocation(blocking_id); + if (cg::Math::DistanceSquared(blocking_actor_location, mid_point) < SQUARE(MAX_JUNCTION_BLOCK_DISTANCE) + && simulation_state.GetVelocity(blocking_id).SquaredLength() < SQUARE(AFTER_JUNCTION_MIN_SPEED)) { + safe_after_junction = false; + } + } + } + } + } + + return safe_after_junction; +} + +std::pair MotionPlanStage::CollisionHandling(const CollisionHazardData &collision_hazard, + const bool tl_hazard, + const cg::Vector3D ego_velocity, + const cg::Vector3D ego_heading, + const float max_target_velocity) { + bool collision_emergency_stop = false; + float dynamic_target_velocity = max_target_velocity; + + if (collision_hazard.hazard && !tl_hazard) { + const ActorId other_actor_id = collision_hazard.hazard_actor_id; + const cg::Vector3D other_velocity = simulation_state.GetVelocity(other_actor_id); + const float ego_relative_speed = (ego_velocity - other_velocity).Length(); + const float available_distance_margin = collision_hazard.available_distance_margin; + + const float other_speed_along_heading = cg::Math::Dot(other_velocity, ego_heading); + + // Consider collision avoidance decisions only if there is positive relative velocity + // of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle). + if (ego_relative_speed > EPSILON_RELATIVE_SPEED) { + // If other vehicle is approaching lead vehicle and lead vehicle is further + // than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m. + float follow_lead_distance = ego_relative_speed * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE; + if (available_distance_margin > follow_lead_distance) { + // Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE + // by maintaining a relative speed of RELATIVE_APPROACH_SPEED + dynamic_target_velocity = other_speed_along_heading + RELATIVE_APPROACH_SPEED; + } + // If vehicle is approaching a lead vehicle and the lead vehicle is further + // than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE. + else if (available_distance_margin > CRITICAL_BRAKING_MARGIN) { + // Then follow the lead vehicle by acquiring it's speed along current heading. + dynamic_target_velocity = std::max(other_speed_along_heading, RELATIVE_APPROACH_SPEED); + } else { + // If lead vehicle closer than CRITICAL_BRAKING_MARGIN, initiate emergency stop. + collision_emergency_stop = true; + } + } + if (available_distance_margin < CRITICAL_BRAKING_MARGIN) { + collision_emergency_stop = true; + } + } + + dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity); + + return {collision_emergency_stop, dynamic_target_velocity}; +} + +void MotionPlanStage::RemoveActor(const ActorId actor_id) { + pid_state_map.erase(actor_id); + teleportation_instance.erase(actor_id); +} + +void MotionPlanStage::Reset() { + pid_state_map.clear(); + teleportation_instance.clear(); +} + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.h b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h new file mode 100644 index 000000000..ace2de47e --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h @@ -0,0 +1,72 @@ + +/// This file has functionality for motion planning based on information +/// from localization, collision avoidance and traffic light response. + +#pragma once + +#include "carla/trafficmanager/DataStructures.h" +#include "carla/trafficmanager/LocalizationUtils.h" +#include "carla/trafficmanager/Parameters.h" +#include "carla/trafficmanager/SimulationState.h" +#include "carla/trafficmanager/Stage.h" +#include "carla/trafficmanager/TrackTraffic.h" + +namespace carla { +namespace traffic_manager { + +class MotionPlanStage: Stage { +private: + const std::vector &vehicle_id_list; + const SimulationState &simulation_state; + const Parameters ¶meters; + const BufferMap &buffer_map; + const TrackTraffic &track_traffic; + // PID paramenters for various road conditions. + const std::vector urban_longitudinal_parameters; + const std::vector highway_longitudinal_parameters; + const std::vector urban_lateral_parameters; + const std::vector highway_lateral_parameters; + const LocalizationFrame &localization_frame; + const CollisionFrame &collision_frame; + const TLFrame &tl_frame; + // Structure holding the controller state for registered vehicles. + std::unordered_map pid_state_map; + // Structure to keep track of duration between teleportation + // in hybrid physics mode. + std::unordered_map teleportation_instance; + ControlFrame &output_array; + + std::pair CollisionHandling(const CollisionHazardData &collision_hazard, + const bool tl_hazard, + const cg::Vector3D ego_velocity, + const cg::Vector3D ego_heading, + const float max_target_velocity); + +bool SafeAfterJunction(const LocalizationData &localization, + const bool tl_hazard, + const bool collision_emergency_stop); + +public: + MotionPlanStage(const std::vector &vehicle_id_list, + const SimulationState &simulation_state, + const Parameters ¶meters, + const BufferMap &buffer_map, + const TrackTraffic &track_traffic, + const std::vector &urban_longitudinal_parameters, + const std::vector &highway_longitudinal_parameters, + const std::vector &urban_lateral_parameters, + const std::vector &highway_lateral_parameters, + const LocalizationFrame &localization_frame, + const CollisionFrame &collision_frame, + const TLFrame &tl_frame, + ControlFrame &output_array); + + void Update(const unsigned long index); + + void RemoveActor(const ActorId actor_id); + + void Reset(); +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp deleted file mode 100644 index ae5d60af4..000000000 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "carla/trafficmanager/MotionPlannerStage.h" - -namespace carla { -namespace traffic_manager { - -namespace PlannerConstants { - - static const float HIGHWAY_SPEED = 50.0f / 3.6f; - static const float RELATIVE_APPROACH_SPEED = 10.0f / 3.6f; - static const float MIN_FOLLOW_LEAD_DISTANCE = 5.0f; - static const float MAX_FOLLOW_LEAD_DISTANCE = 10.0f; - static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f; - static const float FOLLOW_DISTANCE_RATE = (MAX_FOLLOW_LEAD_DISTANCE-MIN_FOLLOW_LEAD_DISTANCE)/ARBITRARY_MAX_SPEED; - static const float CRITICAL_BRAKING_MARGIN = 0.25f; - static const float HYBRID_MODE_DT = 0.05f; - static const float EPSILON_VELOCITY = 0.001f; -} // namespace PlannerConstants - - using namespace PlannerConstants; - - MotionPlannerStage::MotionPlannerStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr collision_messenger, - std::shared_ptr traffic_light_messenger, - std::shared_ptr control_messenger, - Parameters ¶meters, - std::vector urban_longitudinal_parameters, - std::vector highway_longitudinal_parameters, - std::vector urban_lateral_parameters, - std::vector highway_lateral_parameters, - cc::DebugHelper &debug_helper) - : PipelineStage(stage_name), - localization_messenger(localization_messenger), - collision_messenger(collision_messenger), - traffic_light_messenger(traffic_light_messenger), - control_messenger(control_messenger), - parameters(parameters), - urban_longitudinal_parameters(urban_longitudinal_parameters), - highway_longitudinal_parameters(highway_longitudinal_parameters), - urban_lateral_parameters(urban_lateral_parameters), - highway_lateral_parameters(highway_lateral_parameters), - debug_helper(debug_helper){ - - // Initializing the output frame selector. - frame_selector = true; - - // Initializing number of vehicles to zero in the beginning. - number_of_vehicles = 0u; - } - - MotionPlannerStage::~MotionPlannerStage() {} - - void MotionPlannerStage::Action() { - - // Selecting an output frame. - const auto current_control_frame = frame_selector ? control_frame_a : control_frame_b; - - // Looping over all vehicles. - for (uint64_t i = 0u; - i < number_of_vehicles && - localization_frame != nullptr && - collision_frame != nullptr && - traffic_light_frame != nullptr; - ++i) { - - const LocalizationToPlannerData &localization_data = localization_frame->at(i); - const CollisionToPlannerData& collision_data = collision_frame->at(i); - - if (!localization_data.actor->IsAlive()) { - continue; - } - - const Actor actor = localization_data.actor; - const float current_deviation = localization_data.deviation; - const float current_distance = localization_data.distance; - const cg::Vector3D current_velocity_vector = localization_data.velocity; - const float current_velocity = current_velocity_vector.Length(); - - const ActorId actor_id = actor->GetId(); - const cg::Vector3D ego_heading = actor->GetTransform().GetForwardVector(); - const auto vehicle = boost::static_pointer_cast(actor); - - const auto current_time = chr::system_clock::now(); - - // If previous state for vehicle not found, initialize state entry. - if (pid_state_map.find(actor_id) == pid_state_map.end()) { - const auto initial_state = StateEntry{0.0f, 0.0f, 0.0f, chr::system_clock::now(), 0.0f, 0.0f, 0.0f}; - pid_state_map.insert({actor_id, initial_state}); - } - - // Retrieving the previous state. - traffic_manager::StateEntry previous_state; - previous_state = pid_state_map.at(actor_id); - - // Change PID parameters if on highway. - if (current_velocity > HIGHWAY_SPEED) { - longitudinal_parameters = highway_longitudinal_parameters; - lateral_parameters = highway_lateral_parameters; - } else { - longitudinal_parameters = urban_longitudinal_parameters; - lateral_parameters = urban_lateral_parameters; - } - - // Target velocity for vehicle. - float max_target_velocity = parameters.GetVehicleTargetVelocity(actor) / 3.6f; - float dynamic_target_velocity = max_target_velocity; - //////////////////////// Collision related data handling /////////////////////////// - bool collision_emergency_stop = false; - if (collision_data.hazard) { - cg::Vector3D other_vehicle_velocity = collision_data.other_vehicle_velocity; - float ego_relative_velocity = (current_velocity_vector - other_vehicle_velocity).Length(); - float other_velocity_along_heading = cg::Math::Dot(other_vehicle_velocity, ego_heading); - - // Consider collision avoidance decisions only if there is positive relative velocity - // of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle). - if (ego_relative_velocity > EPSILON_VELOCITY) { - // If other vehicle is approaching lead vehicle and lead vehicle is further - // than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m. - float follow_lead_distance = ego_relative_velocity * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE; - if (collision_data.distance_to_other_vehicle > follow_lead_distance) { - // Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE - // by maintaining a relative speed of RELATIVE_APPROACH_SPEED - dynamic_target_velocity = other_velocity_along_heading + RELATIVE_APPROACH_SPEED; - } - // If vehicle is approaching a lead vehicle and the lead vehicle is further - // than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE. - else if (collision_data.distance_to_other_vehicle > CRITICAL_BRAKING_MARGIN) { - // Then follow the lead vehicle by acquiring it's speed along current heading. - dynamic_target_velocity = std::max(other_velocity_along_heading, RELATIVE_APPROACH_SPEED); - } else { - // If lead vehicle closer than CRITICAL_BRAKING_MARGIN, initiate emergency stop. - collision_emergency_stop = true; - } - } - if (collision_data.distance_to_other_vehicle < CRITICAL_BRAKING_MARGIN) { - collision_emergency_stop = true; - } - } - /////////////////////////////////////////////////////////////////////////////////// - - // Clip dynamic target velocity to maximum allowed speed for the vehicle. - dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity); - - bool emergency_stop = false; - // In case of collision or traffic light hazard. - if (traffic_light_frame->at(i).traffic_light_hazard - || collision_emergency_stop) { - emergency_stop = true; - } - - // Message items to be sent to batch control stage. - ActuationSignal actuation_signal {0.0f, 0.0f, 0.0f}; - bool physics_enabled = true; - cg::Transform teleportation_transform; - - // If physics is enabled for the vehicle, use PID controller. - StateEntry current_state; - if (localization_data.physics_enabled) { - - // State update for vehicle. - current_state = controller.StateUpdate(previous_state, current_velocity, - dynamic_target_velocity, current_deviation, - current_distance, current_time); - - // Controller actuation. - actuation_signal = controller.RunStep(current_state, previous_state, - longitudinal_parameters, lateral_parameters); - - if (emergency_stop) { - - current_state.deviation_integral = 0.0f; - current_state.velocity_integral = 0.0f; - actuation_signal.throttle = 0.0f; - actuation_signal.brake = 1.0f; - } - - } - // For physics-less vehicles, determine position and orientation for teleportation. - else if (hybrid_physics_mode) { - - physics_enabled = false; - - // Flushing controller state for vehicle. - current_state = {0.0f, 0.0f, 0.0f, - chr::system_clock::now(), - 0.0f, 0.0f, 0.0f}; - - // Add entry to teleportation duration clock table if not present. - if (teleportation_instance.find(actor_id) == teleportation_instance.end()) { - teleportation_instance.insert({actor_id, chr::system_clock::now()}); - } - - // Measuring time elapsed since last teleportation for the vehicle. - chr::duration elapsed_time = current_time - teleportation_instance.at(actor_id); - - // Find a location ahead of the vehicle for teleportation to achieve intended velocity. - if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time.count() > HYBRID_MODE_DT)) { - - // Target displacement magnitude to achieve target velocity. - const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT; - const float target_displacement_square = std::pow(target_displacement, 2.0f); - - SimpleWaypointPtr target_interval_begin = nullptr; - SimpleWaypointPtr target_interval_end = nullptr; - bool teleportation_interval_found = false; - cg::Location vehicle_location = actor->GetLocation(); - - // Find the interval containing position to achieve target displacement. - for (uint32_t j = 0u; - j+1 < localization_data.position_window.size() && !teleportation_interval_found; - ++j) { - - target_interval_begin = localization_data.position_window.at(j); - target_interval_end = localization_data.position_window.at(j+1); - - cg::Vector3D relative_position = target_interval_begin->GetLocation() - vehicle_location; - if (cg::Math::Dot(relative_position, ego_heading) > 0.0f - && ((target_interval_begin->DistanceSquared(vehicle_location) > target_displacement_square) - || (target_interval_begin->DistanceSquared(vehicle_location) < target_displacement_square - && target_interval_end->DistanceSquared(vehicle_location) > target_displacement_square))) { - teleportation_interval_found = true; - } - } - - if (target_interval_begin != nullptr && target_interval_end != nullptr) { - - // Construct target transform to accurately achieve desired velocity. - float missing_displacement = 0.0f; - const float base_displacement = target_interval_begin->Distance(vehicle_location); - if (base_displacement < target_displacement) { - missing_displacement = target_displacement - base_displacement; - } - cg::Transform target_base_transform = target_interval_begin->GetTransform(); - cg::Location target_base_location = target_base_transform.location; - cg::Vector3D target_heading = target_base_transform.GetForwardVector(); - cg::Location teleportation_location = target_base_location - + cg::Location(target_heading * missing_displacement); - teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation); - - } else { - - teleportation_transform = actor->GetTransform(); - - } - - } - // In case of an emergency stop, stay in the same location. - // Also, teleport only once every dt in asynchronous mode. - else { - - teleportation_transform = actor->GetTransform(); - - } - - } - - // Updating PID state. - StateEntry &state = pid_state_map.at(actor_id); - state = current_state; - - // Constructing the actuation signal. - PlannerToControlData &message = current_control_frame->at(i); - message.actor = actor; - message.throttle = actuation_signal.throttle; - message.brake = actuation_signal.brake; - message.steer = actuation_signal.steer; - message.physics_enabled = physics_enabled; - message.transform = teleportation_transform; - } - } - - void MotionPlannerStage::DataReceiver() { - - localization_frame = localization_messenger->Peek(); - collision_frame = collision_messenger->Peek(); - traffic_light_frame = traffic_light_messenger->Peek(); - - // Allocating new containers for the changed number of registered vehicles. - if (localization_frame != nullptr && - number_of_vehicles != (*localization_frame.get()).size()) { - - number_of_vehicles = static_cast((*localization_frame.get()).size()); - // Allocate output frames. - control_frame_a = std::make_shared(number_of_vehicles); - control_frame_b = std::make_shared(number_of_vehicles); - } - - hybrid_physics_mode = parameters.GetHybridPhysicsMode(); - } - - void MotionPlannerStage::DataSender() { - - localization_messenger->Pop(); - collision_messenger->Pop(); - traffic_light_messenger->Pop(); - - control_messenger->Push(frame_selector ? control_frame_a : control_frame_b); - frame_selector = !frame_selector; - } - - void MotionPlannerStage::DrawPIDValues(const boost::shared_ptr vehicle, - const float throttle, const float brake) { - auto vehicle_location = vehicle->GetLocation(); - debug_helper.DrawString(vehicle_location + cg::Location(0.0f,0.0f,2.0f), - std::to_string(throttle), false, {0u, 255u, 0u}, 0.005f); - debug_helper.DrawString(vehicle_location + cg::Location(0.0f,0.0f,4.0f), - std::to_string(brake), false, {255u, 0u, 0u}, 0.005f); - } - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h deleted file mode 100644 index da165c4e1..000000000 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include -#include -#include -#include - -#include "carla/client/Vehicle.h" -#include "carla/geom/Math.h" -#include "carla/rpc/Actor.h" - -#include "carla/trafficmanager/MessengerAndDataTypes.h" -#include "carla/trafficmanager/Parameters.h" -#include "carla/trafficmanager/PIDController.h" -#include "carla/trafficmanager/PipelineStage.h" -#include "carla/trafficmanager/SimpleWaypoint.h" - -namespace carla { -namespace traffic_manager { - - namespace chr = std::chrono; - namespace cc = carla::client; - - using Actor = carla::SharedPtr; - using ActorId = carla::rpc::ActorId; - using SimpleWaypointPtr = std::shared_ptr; - - /// The class is responsible for aggregating information from various stages - /// like the localization stage, traffic light stage, collision detection - /// stage and actuation signals from the PID controller and makes decisions - /// on how to move the vehicle to follow it's trajectory safely. - class MotionPlannerStage : public PipelineStage { - - private: - - /// Selection key to switch between the output frames. - bool frame_selector; - /// Pointers to data frames to be shared with the batch control stage - std::shared_ptr control_frame_a; - std::shared_ptr control_frame_b; - /// Pointers to data frames received from various stages. - std::shared_ptr localization_frame; - std::shared_ptr collision_frame; - std::shared_ptr traffic_light_frame; - /// Pointers to messenger objects connecting to various stages. - std::shared_ptr localization_messenger; - std::shared_ptr collision_messenger; - std::shared_ptr traffic_light_messenger; - std::shared_ptr control_messenger; - /// Map to store states for integral and differential components - /// of the PID controller for every vehicle - std::unordered_map pid_state_map; - /// Run time parameterization object. - Parameters ¶meters; - /// Configuration parameters for the PID controller. - std::vector urban_longitudinal_parameters; - std::vector highway_longitudinal_parameters; - std::vector urban_lateral_parameters; - std::vector highway_lateral_parameters; - std::vector longitudinal_parameters; - std::vector lateral_parameters; - /// Controller object. - PIDController controller; - /// Number of vehicles registered with the traffic manager. - uint64_t number_of_vehicles; - /// Reference to Carla's debug helper object. - cc::DebugHelper &debug_helper; - /// Switch indicating hybrid physics mode. - bool hybrid_physics_mode {false}; - /// Teleportation duration clock; - std::unordered_map teleportation_instance; - - public: - - MotionPlannerStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr collision_messenger, - std::shared_ptr traffic_light_messenger, - std::shared_ptr control_messenger, - Parameters ¶meters, - std::vector longitudinal_parameters, - std::vector highway_longitudinal_parameters, - std::vector lateral_parameters, - std::vector highway_lateral_parameters, - cc::DebugHelper &debug_helper); - - ~MotionPlannerStage(); - - void DataReceiver() override; - - void Action() override; - - void DataSender() override; - - void DrawPIDValues(const boost::shared_ptr vehicle, const float throttle, const float brake); - - }; - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/PIDController.cpp b/LibCarla/source/carla/trafficmanager/PIDController.cpp deleted file mode 100644 index da239da90..000000000 --- a/LibCarla/source/carla/trafficmanager/PIDController.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "carla/trafficmanager/PIDController.h" - -#include - -#define CLAMP(__v, __hi, __lo) (__v > __hi? __hi : (__v < __lo? __lo: __v)) - -namespace carla { -namespace traffic_manager { - -namespace PIDControllerConstants { - - const float MAX_THROTTLE = 0.7f; - const float MAX_BRAKE = 1.0f; - const float VELOCITY_INTEGRAL_MAX = 5.0f; - const float VELOCITY_INTEGRAL_MIN = -5.0f; - // PID will be stable only over 20 FPS. - const float dt = 1/20.0f; - -} // namespace PIDControllerConstants - - using namespace PIDControllerConstants; - - PIDController::PIDController() {} - - // Initializing present state. - StateEntry PIDController::StateUpdate( - StateEntry previous_state, - float current_velocity, - float target_velocity, - float angular_deviation, - float distance, - TimeInstance current_time) { - - traffic_manager::StateEntry current_state = { - angular_deviation, distance, - (current_velocity - target_velocity) / target_velocity, - current_time, - 0.0f, - 0.0f, - 0.0f - }; - - // Calculating integrals. - current_state.deviation_integral = angular_deviation * dt + previous_state.deviation_integral; - current_state.distance_integral = distance * dt + previous_state.distance_integral; - current_state.velocity_integral = dt * current_state.velocity + previous_state.velocity_integral; - - // Clamp velocity integral to avoid accumulating over-compensation - // with time for vehicles that take a long time to reach target velocity. - current_state.velocity_integral = CLAMP(current_state.velocity_integral, - VELOCITY_INTEGRAL_MAX, VELOCITY_INTEGRAL_MIN); - - return current_state; - } - - ActuationSignal PIDController::RunStep( - StateEntry present_state, - StateEntry previous_state, - const std::vector &longitudinal_parameters, - const std::vector &lateral_parameters) const { - - // Longitudinal PID calculation. - const float expr_v = - longitudinal_parameters[0] * present_state.velocity + - longitudinal_parameters[1] * present_state.velocity_integral + - longitudinal_parameters[2] * (present_state.velocity - - previous_state.velocity) / dt; - - float throttle; - float brake; - - if (expr_v < 0.0f) { - throttle = std::min(std::abs(expr_v), MAX_THROTTLE); - brake = 0.0f; - } else { - throttle = 0.0f; - brake = std::min(expr_v, MAX_BRAKE); - } - - // Lateral PID calculation. - float steer = - lateral_parameters[0] * present_state.deviation + - lateral_parameters[1] * present_state.deviation_integral + - lateral_parameters[2] * (present_state.deviation - - previous_state.deviation) / dt; - - steer = std::max(-0.8f, std::min(steer, 0.8f)); - - return ActuationSignal{throttle, brake, steer}; - } - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/PIDController.h b/LibCarla/source/carla/trafficmanager/PIDController.h index a4991f3fb..20079dea7 100644 --- a/LibCarla/source/carla/trafficmanager/PIDController.h +++ b/LibCarla/source/carla/trafficmanager/PIDController.h @@ -6,63 +6,82 @@ #pragma once -#include -#include -#include +#include + +#include "carla/trafficmanager/Constants.h" +#include "carla/trafficmanager/DataStructures.h" namespace carla { namespace traffic_manager { - namespace chr = std::chrono; - using TimeInstance = chr::time_point; +namespace chr = std::chrono; - /// Structure to hold the actuation signals. - struct ActuationSignal { - float throttle; - float brake; - float steer; - }; +using namespace constants::PID; - /// Structure to hold the controller state. - struct StateEntry { - float deviation; - float distance; - float velocity; - TimeInstance time_instance; - float deviation_integral; - float distance_integral; - float velocity_integral; - }; +using TimeInstance = chr::time_point; - /// This class calculates PID actuation signals to control the vehicle - /// such that the vehicle maintains a target velocity and aligns its - /// orientation along its trajectory. +namespace PID { - class PIDController { +/// This function calculates the present state of the vehicle including +/// the accumulated integral component of the PID system. +StateEntry StateUpdate(StateEntry previous_state, + float current_velocity, + float target_velocity, + float angular_deviation, + TimeInstance current_time) { + StateEntry current_state = { + current_time, + angular_deviation, + (current_velocity - target_velocity) / target_velocity, + 0.0f, + 0.0f}; - public: + // Calculating integrals. + current_state.deviation_integral = angular_deviation * DT + previous_state.deviation_integral; + current_state.velocity_integral = DT * current_state.velocity + previous_state.velocity_integral; - PIDController(); + // Clamp velocity integral to avoid accumulating over-compensation + // with time for vehicles that take a long time to reach target velocity. + current_state.velocity_integral = std::min(VELOCITY_INTEGRAL_MAX, std::max(current_state.velocity_integral, VELOCITY_INTEGRAL_MIN)); - /// This method calculates the present state of the vehicle including - /// the accumulated integral component of the PID system. - StateEntry StateUpdate( - StateEntry previous_state, - float current_velocity, - float target_velocity, - float angular_deviation, - float distance, - TimeInstance current_time); + return current_state; +} - /// This method calculates the actuation signals based on the resent state - /// change of the vehicle to minimize PID error. - ActuationSignal RunStep( - StateEntry present_state, - StateEntry previous_state, - const std::vector &longitudinal_parameters, - const std::vector &lateral_parameters) const; +/// This function calculates the actuation signals based on the resent state +/// change of the vehicle to minimize PID error. +ActuationSignal RunStep(StateEntry present_state, + StateEntry previous_state, + const std::vector &longitudinal_parameters, + const std::vector &lateral_parameters) { - }; + // Longitudinal PID calculation. + const float expr_v = + longitudinal_parameters[0] * present_state.velocity + + longitudinal_parameters[1] * present_state.velocity_integral + + longitudinal_parameters[2] * (present_state.velocity - previous_state.velocity) * INV_DT; + float throttle; + float brake; + + if (expr_v < 0.0f) { + throttle = std::min(std::abs(expr_v), MAX_THROTTLE); + brake = 0.0f; + } else { + throttle = 0.0f; + brake = std::min(expr_v, MAX_BRAKE); + } + + // Lateral PID calculation. + float steer = + lateral_parameters[0] * present_state.deviation + + lateral_parameters[1] * present_state.deviation_integral + + lateral_parameters[2] * (present_state.deviation - previous_state.deviation) * INV_DT; + + steer = std::max(-0.8f, std::min(steer, 0.8f)); + + return ActuationSignal{throttle, brake, steer}; +} + +} // namespace PID } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/Parameters.cpp b/LibCarla/source/carla/trafficmanager/Parameters.cpp index 9646592cb..397a22983 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.cpp +++ b/LibCarla/source/carla/trafficmanager/Parameters.cpp @@ -4,8 +4,6 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -#include - #include "carla/trafficmanager/Parameters.h" namespace carla { @@ -19,6 +17,8 @@ Parameters::Parameters() { Parameters::~Parameters() {} +//////////////////////////////////// SETTERS ////////////////////////////////// + void Parameters::SetHybridPhysicsMode(const bool mode_switch) { hybrid_physics_mode.store(mode_switch); @@ -43,7 +43,7 @@ void Parameters::SetCollisionDetection(const ActorPtr &reference_actor, const Ac if (ignore_collision.Contains(reference_id)) { std::shared_ptr actor_set = ignore_collision.GetValue(reference_id); if (actor_set->Contains(other_id)) { - actor_set->Remove({other_actor}); + actor_set->Remove({other_id}); } } } else { @@ -87,118 +87,19 @@ void Parameters::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance_to_leading_vehicle.AddEntry(entry); } -bool Parameters::GetSynchronousMode() { - return synchronous_mode.load(); +void Parameters::SetSynchronousMode(const bool mode_switch) { + synchronous_mode.store(mode_switch); } void Parameters::SetSynchronousModeTimeOutInMiliSecond(const double time) { synchronous_time_out = std::chrono::duration(time); } -double Parameters::GetSynchronousModeTimeOutInMiliSecond() { - return synchronous_time_out.count(); -} - -void Parameters::SetSynchronousMode(const bool mode_switch) { - synchronous_mode.store(mode_switch); -} - -float Parameters::GetVehicleTargetVelocity(const ActorPtr &actor) { - - const ActorId actor_id = actor->GetId(); - const auto vehicle = boost::static_pointer_cast(actor); - const float speed_limit = vehicle->GetSpeedLimit(); - float percentage_difference = global_percentage_difference_from_limit; - - if (percentage_difference_from_speed_limit.Contains(actor_id)) { - percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id); - } - - return speed_limit * (1.0f - percentage_difference / 100.0f); -} - -bool Parameters::GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor) { - - const ActorId reference_actor_id = reference_actor->GetId(); - const ActorId other_actor_id = other_actor->GetId(); - bool avoid_collision = true; - - if (ignore_collision.Contains(reference_actor_id) && - ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) { - avoid_collision = false; - } - - return avoid_collision; -} - -ChangeLaneInfo Parameters::GetForceLaneChange(const ActorPtr &actor) { - - const ActorId actor_id = actor->GetId(); - ChangeLaneInfo change_lane_info; - - if (force_lane_change.Contains(actor_id)) { - change_lane_info = force_lane_change.GetValue(actor_id); - } - - force_lane_change.RemoveEntry(actor_id); - - return change_lane_info; -} - -float Parameters::GetKeepRightPercentage(const ActorPtr &actor) { - - const ActorId actor_id = actor->GetId(); - float percentage = -1.0f; - - if (perc_keep_right.Contains(actor_id)) { - percentage = perc_keep_right.GetValue(actor_id); - } - - perc_keep_right.RemoveEntry(actor_id); - - return percentage; -} - -bool Parameters::GetAutoLaneChange(const ActorPtr &actor) { - - const ActorId actor_id = actor->GetId(); - bool auto_lane_change_policy = true; - - if (auto_lane_change.Contains(actor_id)) { - auto_lane_change_policy = auto_lane_change.GetValue(actor_id); - } - - return auto_lane_change_policy; -} - -float Parameters::GetDistanceToLeadingVehicle(const ActorPtr &actor) { - - const ActorId actor_id = actor->GetId(); - float specific_distance_margin = 0.0f; - if (distance_to_leading_vehicle.Contains(actor_id)) { - specific_distance_margin = distance_to_leading_vehicle.GetValue(actor_id); - } else { - specific_distance_margin = distance_margin; - } - - return specific_distance_margin; -} - void Parameters::SetGlobalDistanceToLeadingVehicle(const float dist) { distance_margin.store(dist); } -float Parameters::GetHybridPhysicsRadius() { - - return hybrid_physics_radius.load(); -} - -void Parameters::SetHybridPhysicsRadius(const float radius) { - float new_radius = std::max(radius, 0.0f); - hybrid_physics_radius.store(new_radius); -} - void Parameters::SetPercentageRunningLight(const ActorPtr &actor, const float perc) { float new_perc = cg::Math::Clamp(perc, 0.0f, 100.0f); @@ -227,9 +128,100 @@ void Parameters::SetPercentageIgnoreWalkers(const ActorPtr &actor, const float p perc_ignore_walkers.AddEntry(entry); } -float Parameters::GetPercentageRunningLight(const ActorPtr &actor) { +void Parameters::SetHybridPhysicsRadius(const float radius) { + float new_radius = std::max(radius, 0.0f); + hybrid_physics_radius.store(new_radius); +} + +//////////////////////////////////// GETTERS ////////////////////////////////// + +float Parameters::GetHybridPhysicsRadius() const { + + return hybrid_physics_radius.load(); +} + +bool Parameters::GetSynchronousMode() const { + return synchronous_mode.load(); +} + +double Parameters::GetSynchronousModeTimeOutInMiliSecond() const { + return synchronous_time_out.count(); +} + +float Parameters::GetVehicleTargetVelocity(const ActorId &actor_id, const float speed_limit) const { + + float percentage_difference = global_percentage_difference_from_limit; + + if (percentage_difference_from_speed_limit.Contains(actor_id)) { + percentage_difference = percentage_difference_from_speed_limit.GetValue(actor_id); + } + + return speed_limit * (1.0f - percentage_difference / 100.0f); +} + +bool Parameters::GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const { + + bool avoid_collision = true; + + if (ignore_collision.Contains(reference_actor_id) && + ignore_collision.GetValue(reference_actor_id)->Contains(other_actor_id)) { + avoid_collision = false; + } + + return avoid_collision; +} + +ChangeLaneInfo Parameters::GetForceLaneChange(const ActorId &actor_id) { + + ChangeLaneInfo change_lane_info {false, false}; + + if (force_lane_change.Contains(actor_id)) { + change_lane_info = force_lane_change.GetValue(actor_id); + } + + force_lane_change.RemoveEntry(actor_id); + + return change_lane_info; +} + +float Parameters::GetKeepRightPercentage(const ActorId &actor_id) { + + float percentage = -1.0f; + + if (perc_keep_right.Contains(actor_id)) { + percentage = perc_keep_right.GetValue(actor_id); + } + + perc_keep_right.RemoveEntry(actor_id); + + return percentage; +} + +bool Parameters::GetAutoLaneChange(const ActorId &actor_id) const { + + bool auto_lane_change_policy = true; + + if (auto_lane_change.Contains(actor_id)) { + auto_lane_change_policy = auto_lane_change.GetValue(actor_id); + } + + return auto_lane_change_policy; +} + +float Parameters::GetDistanceToLeadingVehicle(const ActorId &actor_id) const { + + float specific_distance_margin = 0.0f; + if (distance_to_leading_vehicle.Contains(actor_id)) { + specific_distance_margin = distance_to_leading_vehicle.GetValue(actor_id); + } else { + specific_distance_margin = distance_margin; + } + + return specific_distance_margin; +} + +float Parameters::GetPercentageRunningLight(const ActorId &actor_id) const { - const ActorId actor_id = actor->GetId(); float percentage = 0.0f; if (perc_run_traffic_light.Contains(actor_id)) { @@ -239,9 +231,8 @@ float Parameters::GetPercentageRunningLight(const ActorPtr &actor) { return percentage; } -float Parameters::GetPercentageRunningSign(const ActorPtr &actor) { +float Parameters::GetPercentageRunningSign(const ActorId &actor_id) const { - const ActorId actor_id = actor->GetId(); float percentage = 0.0f; if (perc_run_traffic_sign.Contains(actor_id)) { @@ -251,9 +242,8 @@ float Parameters::GetPercentageRunningSign(const ActorPtr &actor) { return percentage; } -float Parameters::GetPercentageIgnoreWalkers(const ActorPtr &actor) { +float Parameters::GetPercentageIgnoreWalkers(const ActorId &actor_id) const { - const ActorId actor_id = actor->GetId(); float percentage = 0.0f; if (perc_ignore_walkers.Contains(actor_id)) { @@ -263,9 +253,8 @@ float Parameters::GetPercentageIgnoreWalkers(const ActorPtr &actor) { return percentage; } -float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) { +float Parameters::GetPercentageIgnoreVehicles(const ActorId &actor_id) const { - const ActorId actor_id = actor->GetId(); float percentage = 0.0f; if (perc_ignore_vehicles.Contains(actor_id)) { @@ -275,7 +264,7 @@ float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) { return percentage; } -bool Parameters::GetHybridPhysicsMode() { +bool Parameters::GetHybridPhysicsMode() const { return hybrid_physics_mode.load(); } diff --git a/LibCarla/source/carla/trafficmanager/Parameters.h b/LibCarla/source/carla/trafficmanager/Parameters.h index 3237fd410..7843bcba4 100644 --- a/LibCarla/source/carla/trafficmanager/Parameters.h +++ b/LibCarla/source/carla/trafficmanager/Parameters.h @@ -8,6 +8,7 @@ #include #include +#include #include "carla/client/Actor.h" #include "carla/client/Vehicle.h" @@ -20,156 +21,158 @@ namespace carla { namespace traffic_manager { - namespace cc = carla::client; - namespace cg = carla::geom; - using ActorPtr = carla::SharedPtr; - using ActorId = carla::ActorId; +namespace cc = carla::client; +namespace cg = carla::geom; +using ActorPtr = carla::SharedPtr; +using ActorId = carla::ActorId; - struct ChangeLaneInfo { - bool change_lane = false; - bool direction = false; - }; +struct ChangeLaneInfo { + bool change_lane = false; + bool direction = false; +}; - class Parameters { +class Parameters { - private: +private: + /// Target velocity map for individual vehicles. + AtomicMap percentage_difference_from_speed_limit; + /// Global target velocity limit % difference. + float global_percentage_difference_from_limit = 0; + /// Map containing a set of actors to be ignored during collision detection. + AtomicMap> ignore_collision; + /// Map containing distance to leading vehicle command. + AtomicMap distance_to_leading_vehicle; + /// Map containing force lane change commands. + AtomicMap force_lane_change; + /// Map containing auto lane change commands. + AtomicMap auto_lane_change; + /// Map containing % of running a traffic light. + AtomicMap perc_run_traffic_light; + /// Map containing % of running a traffic sign. + AtomicMap perc_run_traffic_sign; + /// Map containing % of ignoring walkers. + AtomicMap perc_ignore_walkers; + /// Map containing % of ignoring vehicles. + AtomicMap perc_ignore_vehicles; + /// Map containing % of keep right rule. + AtomicMap perc_keep_right; + /// Synchronous mode switch. + std::atomic synchronous_mode{false}; + /// Distance margin + std::atomic distance_margin{2.0}; + /// Hybrid physics mode switch. + std::atomic hybrid_physics_mode{false}; + /// Hybrid physics radius. + std::atomic hybrid_physics_radius {70.0}; - /// Target velocity map for individual vehicles. - AtomicMap percentage_difference_from_speed_limit; - /// Global target velocity limit % difference. - float global_percentage_difference_from_limit = 0; - /// Map containing a set of actors to be ignored during collision detection. - AtomicMap> ignore_collision; - /// Map containing distance to leading vehicle command. - AtomicMap distance_to_leading_vehicle; - /// Map containing force lane change commands. - AtomicMap force_lane_change; - /// Map containing auto lane change commands. - AtomicMap auto_lane_change; - /// Map containing % of running a traffic light. - AtomicMap perc_run_traffic_light; - /// Map containing % of running a traffic sign. - AtomicMap perc_run_traffic_sign; - /// Map containing % of ignoring walkers. - AtomicMap perc_ignore_walkers; - /// Map containing % of ignoring vehicles. - AtomicMap perc_ignore_vehicles; - /// Map containing % of keep right rule. - AtomicMap perc_keep_right; - /// Synchronous mode switch. - std::atomic synchronous_mode {false}; - /// Distance margin - std::atomic distance_margin {2.0}; - /// Hybrid physics mode switch. - std::atomic hybrid_physics_mode {false}; - /// Hybrid physics radius. - std::atomic hybrid_physics_radius {70.0}; +public: + Parameters(); + ~Parameters(); - public: - Parameters(); - ~Parameters(); + ////////////////////////////////// SETTERS ///////////////////////////////////// - /// Set a vehicle's % decrease in velocity with respect to the speed limit. - /// If less than 0, it's a % increase. - void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); + /// Set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); - /// Set a global % decrease in velocity with respect to the speed limit. - /// If less than 0, it's a % increase. - void SetGlobalPercentageSpeedDifference(float const percentage); + /// Set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetGlobalPercentageSpeedDifference(float const percentage); - /// Method to set collision detection rules between vehicles. - void SetCollisionDetection( - const ActorPtr &reference_actor, - const ActorPtr &other_actor, - const bool detect_collision); + /// Method to set collision detection rules between vehicles. + void SetCollisionDetection( + const ActorPtr &reference_actor, + const ActorPtr &other_actor, + const bool detect_collision); - /// Method to force lane change on a vehicle. - /// Direction flag can be set to true for left and false for right. - void SetForceLaneChange(const ActorPtr &actor, const bool direction); + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + void SetForceLaneChange(const ActorPtr &actor, const bool direction); - /// Enable/disable automatic lane change on a vehicle. - void SetAutoLaneChange(const ActorPtr &actor, const bool enable); + /// Enable/disable automatic lane change on a vehicle. + void SetAutoLaneChange(const ActorPtr &actor, const bool enable); - /// Method to specify how much distance a vehicle should maintain to - /// the leading vehicle. - void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); - /// Method to query target velocity for a vehicle. - float GetVehicleTargetVelocity(const ActorPtr &actor); + /// Method to set % to run any traffic sign. + void SetPercentageRunningSign(const ActorPtr &actor, const float perc); - /// Method to query collision avoidance rule between a pair of vehicles. - bool GetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor); + /// Method to set % to run any traffic light. + void SetPercentageRunningLight(const ActorPtr &actor, const float perc); - /// Method to query lane change command for a vehicle. - ChangeLaneInfo GetForceLaneChange(const ActorPtr &actor); + /// Method to set % to ignore any vehicle. + void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc); - /// Method to query percentage probability of keep right rule for a vehicle. - float GetKeepRightPercentage(const ActorPtr &actor); + /// Method to set % to ignore any vehicle. + void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc); - /// Method to query auto lane change rule for a vehicle. - bool GetAutoLaneChange(const ActorPtr &actor); + /// Method to set probabilistic preference to keep on the right lane. + void SetKeepRightPercentage(const ActorPtr &actor, const float percentage); - /// Method to query distance to leading vehicle for a given vehicle. - float GetDistanceToLeadingVehicle(const ActorPtr &actor); + /// Method to set the distance to leading vehicle for all registered vehicles. + void SetGlobalDistanceToLeadingVehicle(const float dist); - /// Method to get % to run any traffic light. - float GetPercentageRunningSign(const ActorPtr &actor); + /// Set Synchronous mode time out. + void SetSynchronousModeTimeOutInMiliSecond(const double time); - /// Method to get % to run any traffic light. - float GetPercentageRunningLight(const ActorPtr &actor); + /// Method to set hybrid physics mode. + void SetHybridPhysicsMode(const bool mode_switch); - /// Method to get % to ignore any vehicle. - float GetPercentageIgnoreVehicles(const ActorPtr &actor); + /// Method to set synchronous mode. + void SetSynchronousMode(const bool mode_switch = true); - /// Method to get % to ignore any walker. - float GetPercentageIgnoreWalkers(const ActorPtr &actor); + /// Method to set hybrid physics radius. + void SetHybridPhysicsRadius(const float radius); - /// Method to set distance to leading vehicle for a given vehicle. - void SetGlobalDistanceToLeadingVehicle(const float dist); + ///////////////////////////////// GETTERS ///////////////////////////////////// - /// Method to set % to run any traffic sign. - void SetPercentageRunningSign(const ActorPtr &actor, const float perc); + /// Method to retrieve hybrid physics radius. + float GetHybridPhysicsRadius() const; - /// Method to set % to run any traffic light. - void SetPercentageRunningLight(const ActorPtr &actor, const float perc); + /// Method to query target velocity for a vehicle. + float GetVehicleTargetVelocity(const ActorId &actor_id, const float speed_limit) const; - /// Method to set % to ignore any vehicle. - void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc); + /// Method to query collision avoidance rule between a pair of vehicles. + bool GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const; - /// Method to set % to ignore any vehicle. - void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc); + /// Method to query lane change command for a vehicle. + ChangeLaneInfo GetForceLaneChange(const ActorId &actor_id); - /// Method to set probabilistic preference to keep on the right lane. - void SetKeepRightPercentage(const ActorPtr &actor,const float percentage); + /// Method to query percentage probability of keep right rule for a vehicle. + float GetKeepRightPercentage(const ActorId &actor_id); - /// Method to get synchronous mode. - bool GetSynchronousMode(); + /// Method to query auto lane change rule for a vehicle. + bool GetAutoLaneChange(const ActorId &actor_id) const; - /// Method to set synchronous mode. - void SetSynchronousMode(const bool mode_switch = true); + /// Method to query distance to leading vehicle for a given vehicle. + float GetDistanceToLeadingVehicle(const ActorId &actor_id) const; - /// Get synchronous mode time out - double GetSynchronousModeTimeOutInMiliSecond(); + /// Method to get % to run any traffic light. + float GetPercentageRunningSign(const ActorId &actor_id) const; - /// Set Synchronous mode time out. - void SetSynchronousModeTimeOutInMiliSecond(const double time); + /// Method to get % to run any traffic light. + float GetPercentageRunningLight(const ActorId &actor_id) const; - /// Method to set hybrid physics mode. - void SetHybridPhysicsMode(const bool mode_switch); + /// Method to get % to ignore any vehicle. + float GetPercentageIgnoreVehicles(const ActorId &actor_id) const; - /// Method to retrieve hybrid physics mode. - bool GetHybridPhysicsMode(); + /// Method to get % to ignore any walker. + float GetPercentageIgnoreWalkers(const ActorId &actor_id) const; - /// Method to set hybrid physics radius. - void SetHybridPhysicsRadius(const float radius); + /// Method to get synchronous mode. + bool GetSynchronousMode() const; - /// Method to retrieve hybrid physics radius. - float GetHybridPhysicsRadius(); + /// Get synchronous mode time out + double GetSynchronousModeTimeOutInMiliSecond() const; - /// Synchronous mode time out variable. - std::chrono::duration synchronous_time_out; + /// Method to retrieve hybrid physics mode. + bool GetHybridPhysicsMode() const; - }; + /// Synchronous mode time out variable. + std::chrono::duration synchronous_time_out; +}; } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp deleted file mode 100644 index 2f81a8c93..000000000 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "carla/trafficmanager/PerformanceDiagnostics.h" - -namespace carla { -namespace traffic_manager { - - PerformanceDiagnostics::PerformanceDiagnostics(std::string stage_name) - : stage_name(stage_name) { - - throughput_clock = chr::system_clock::now(); - throughput_counter = 0u; - inter_update_clock = chr::system_clock::now(); - inter_update_duration = chr::duration(0); - } - - void PerformanceDiagnostics::RegisterUpdate(bool begin_or_end) { - - const auto current_time = chr::system_clock::now(); - - if (begin_or_end) { - const chr::duration throughput_count_duration = current_time - throughput_clock; - if (throughput_count_duration.count() > 1.0f) { - - // std::cout << "Stage: " + stage_name + ", throughput: " << throughput_counter - // << ", avg. cycle duration: " << 1000* inter_update_duration.count() - // << " ms" << std::endl; - - throughput_clock = current_time; - throughput_counter = 0u; - } else { - - ++throughput_counter; - } - - inter_update_clock = current_time; - } else { - const chr::duration last_update_duration = current_time - inter_update_clock; - inter_update_duration = (inter_update_duration + last_update_duration) / 2.0f; - } - } - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h b/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h deleted file mode 100644 index f782378e5..000000000 --- a/LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include -#include -#include - -#include "carla/Logging.h" - -namespace carla { -namespace traffic_manager { - - namespace chr = std::chrono; - using namespace chr; - using TimePoint = chr::time_point; - - class PerformanceDiagnostics { - - private: - /// Stage name. - std::string stage_name; - /// Throughput clock. - TimePoint throughput_clock; - /// Throughput counter. - uint64_t throughput_counter; - /// Inter-update clock. - TimePoint inter_update_clock; - /// Inter-update duration. - chr::duration inter_update_duration; - - public: - PerformanceDiagnostics(std::string name); - - void RegisterUpdate(bool begin_or_end); - }; - - class SnippetProfiler { - - private: - - std::unordered_map print_clocks; - std::unordered_map snippet_clocks; - std::unordered_map> snippet_durations; - std::unordered_map number_of_calls; - - public: - - SnippetProfiler(){}; - - void MeasureExecutionTime(std::string snippet_name, bool begin_or_end) { - - if (print_clocks.find(snippet_name) == print_clocks.end()) { - print_clocks.insert({snippet_name, chr::system_clock::now()}); - } - if (snippet_clocks.find(snippet_name) == snippet_clocks.end()) { - snippet_clocks.insert({snippet_name, TimePoint()}); - } - if (snippet_durations.find(snippet_name) == snippet_durations.end()) { - snippet_durations.insert({snippet_name, chr::duration()}); - } - if (number_of_calls.find(snippet_name) == number_of_calls.end()) { - number_of_calls.insert({snippet_name, 0u}); - } - - TimePoint current_time = chr::system_clock::now(); - TimePoint& print_clock = print_clocks.at(snippet_name); - TimePoint& snippet_clock = snippet_clocks.at(snippet_name); - chr::duration& snippet_duration = snippet_durations.at(snippet_name); - unsigned long& call_count = number_of_calls.at(snippet_name); - - if (begin_or_end) { - snippet_clock = current_time; - } else { - chr::duration measured_duration = current_time - snippet_clock; - snippet_duration += measured_duration; - ++call_count; - } - - chr::duration print_duration = current_time - print_clock; - if (print_duration.count() > 1.0f) { - call_count = call_count == 0u ? 1 : call_count; - std::cout << "Snippet name : " << snippet_name << ", " - << "avg. duration : " << 1000 * snippet_duration.count() / call_count << " ms, " - << "total duration : " << snippet_duration.count() << " s, " - << "total calls : " << call_count << ", " - << std::endl; - - snippet_duration = 0s; - call_count = 0u; - - print_clock = current_time; - } - } - }; - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp b/LibCarla/source/carla/trafficmanager/PipelineStage.cpp deleted file mode 100644 index b50039a1f..000000000 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#include "carla/trafficmanager/PipelineStage.h" - -namespace carla { -namespace traffic_manager { - -PipelineStage::PipelineStage( - const std::string &stage_name) - : stage_name(stage_name), - performance_diagnostics(PerformanceDiagnostics(stage_name)) { - run_stage.store(false); -} - -PipelineStage::~PipelineStage() { - Stop(); -} - -void PipelineStage::Start() { - run_stage.store(true); - if(worker_thread) { - Stop(); - } - worker_thread = std::make_unique(&PipelineStage::Update, this); -} - -void PipelineStage::Stop() { - run_stage.store(false); - if(worker_thread) { - if(worker_thread->joinable()){ - worker_thread->join(); - } - worker_thread.release(); - } -} - -void PipelineStage::Update() { - while (run_stage.load()){ - // Receive data. - DataReceiver(); - - if(run_stage.load()){ - performance_diagnostics.RegisterUpdate(true); - Action(); - performance_diagnostics.RegisterUpdate(false); - } - - if(run_stage.load()) { - DataSender(); - } - - } -} - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/PipelineStage.h b/LibCarla/source/carla/trafficmanager/PipelineStage.h deleted file mode 100644 index 80b2f69e2..000000000 --- a/LibCarla/source/carla/trafficmanager/PipelineStage.h +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "carla/Logging.h" -#include "carla/rpc/ActorId.h" - -#include "carla/trafficmanager/Messenger.h" -#include "carla/trafficmanager/PerformanceDiagnostics.h" - -namespace carla { -namespace traffic_manager { - - namespace chr = std::chrono; - using namespace std::chrono_literals; - - /// This class provides base functionality and template for - /// various stages of the pipeline. - class PipelineStage { - - private: - std::unique_ptr worker_thread; - - protected: - /// Flag to start/stop stage. - std::atomic run_stage; - /// Stage name string. - std::string stage_name; - - private: - /// Object to track stage performance. - PerformanceDiagnostics performance_diagnostics; - - void Update(); - - protected: - - /// Implement this method with the logic to receive data from - /// the previous stage(s) and distribute to Action() threads. - virtual void DataReceiver() = 0; - - /// Implement this method with logic to gather results from the - /// action threads and send to next stage(s). - virtual void DataSender() = 0; - - /// Implement this method with logic to process data inside the stage - virtual void Action() = 0; - - public: - - PipelineStage(const std::string &stage_name); - - virtual ~PipelineStage(); - - void Start(); - - void Stop(); - - }; - -} // namespace traffic_manager -} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/RandomGenerator.h b/LibCarla/source/carla/trafficmanager/RandomGenerator.h new file mode 100644 index 000000000..a01d93891 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/RandomGenerator.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +namespace carla { +namespace traffic_manager { + +template::value> +> class RandomGenerator { +public: + RandomGenerator(): mt{std::random_device{}()}, dist(0.0, 100.0) {} + T next() { return dist(mt); } +private: + std::mt19937 mt; + std::uniform_real_distribution dist; +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp index e94dfd21f..ad307cbcc 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp @@ -4,6 +4,8 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . +#include "carla/geom/Math.h" + #include "carla/trafficmanager/SimpleWaypoint.h" namespace carla { diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h index a71d112a5..a1ce9d528 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.h @@ -6,12 +6,10 @@ #pragma once -#include #include #include "carla/client/Waypoint.h" #include "carla/geom/Location.h" -#include "carla/geom/Math.h" #include "carla/geom/Transform.h" #include "carla/geom/Vector3D.h" #include "carla/Memory.h" @@ -93,7 +91,7 @@ namespace traffic_manager { void SetGeodesicGridId(GeoGridId _geodesic_grid_id); GeoGridId GetGeodesicGridId(); - /// Metod to retreive junction id of the waypoint. + /// Method to retreive junction id of the waypoint. GeoGridId GetJunctionId() const; /// Calculates the distance from the object's waypoint to the passed diff --git a/LibCarla/source/carla/trafficmanager/SimulationState.cpp b/LibCarla/source/carla/trafficmanager/SimulationState.cpp new file mode 100644 index 000000000..7879d51bc --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/SimulationState.cpp @@ -0,0 +1,83 @@ + +#include "carla/trafficmanager/SimulationState.h" + +namespace carla { +namespace traffic_manager { + +SimulationState::SimulationState() {} + +void SimulationState::AddActor(ActorId actor_id, + KinematicState kinematic_state, + StaticAttributes attributes, + TrafficLightState tl_state) { + actor_set.insert(actor_id); + kinematic_state_map.insert({actor_id, kinematic_state}); + static_attribute_map.insert({actor_id, attributes}); + tl_state_map.insert({actor_id, tl_state}); +} + +bool SimulationState::ContainsActor(ActorId actor_id) const { + return actor_set.find(actor_id) != actor_set.end(); +} + +void SimulationState::RemoveActor(ActorId actor_id) { + actor_set.erase(actor_id); + kinematic_state_map.erase(actor_id); + static_attribute_map.erase(actor_id); + tl_state_map.erase(actor_id); +} + +void SimulationState::Reset() { + actor_set.clear(); + kinematic_state_map.clear(); + static_attribute_map.clear(); + tl_state_map.clear(); +} + +void SimulationState::UpdateKinematicState(ActorId actor_id, KinematicState state) { + kinematic_state_map.at(actor_id) = state; +} + +void SimulationState::UpdateTrafficLightState(ActorId actor_id, TrafficLightState state) { + tl_state_map.at(actor_id) = state; +} + +cg::Location SimulationState::GetLocation(ActorId actor_id) const { + return kinematic_state_map.at(actor_id).location; +} + +cg::Rotation SimulationState::GetRotation(ActorId actor_id) const { + return kinematic_state_map.at(actor_id).rotation; +} + +cg::Vector3D SimulationState::GetHeading(ActorId actor_id) const { + return kinematic_state_map.at(actor_id).rotation.GetForwardVector(); +} + +cg::Vector3D SimulationState::GetVelocity(ActorId actor_id) const { + return kinematic_state_map.at(actor_id).velocity; +} + +float SimulationState::GetSpeedLimit(ActorId actor_id) const { + return kinematic_state_map.at(actor_id).speed_limit; +} + +bool SimulationState::IsPhysicsEnabled(ActorId actor_id) const { + return kinematic_state_map.at(actor_id).physics_enabled; +} + +TrafficLightState SimulationState::GetTLS(ActorId actor_id) const { + return tl_state_map.at(actor_id); +} + +ActorType SimulationState::GetType(ActorId actor_id) const { + return static_attribute_map.at(actor_id).actor_type; +} + +cg::Vector3D SimulationState::GetDimensions(ActorId actor_id) const { + const StaticAttributes &attributes = static_attribute_map.at(actor_id); + return cg::Vector3D(attributes.half_length, attributes.half_width, attributes.half_height); +} + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/SimulationState.h b/LibCarla/source/carla/trafficmanager/SimulationState.h new file mode 100644 index 000000000..7b025eaa5 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/SimulationState.h @@ -0,0 +1,96 @@ + +#pragma once + +#include + +#include "carla/trafficmanager/DataStructures.h" + +namespace carla { +namespace traffic_manager { + +enum ActorType { + Vehicle, + Pedestrian, + Any +}; + +struct KinematicState { + cg::Location location; + cg::Rotation rotation; + cg::Vector3D velocity; + float speed_limit; + bool physics_enabled; +}; +using KinematicStateMap = std::unordered_map; + +struct TrafficLightState { + TLS tl_state; + bool at_traffic_light; +}; +using TrafficLightStateMap = std::unordered_map; + +struct StaticAttributes { + ActorType actor_type; + float half_length; + float half_width; + float half_height; +}; +using StaticAttributeMap = std::unordered_map; + +/// This class holds the state of all the vehicles in the simlation. +class SimulationState { + +private: + // Structure to hold ids of all actors in the simulation. + std::unordered_set actor_set; + // Structure containing dynamic motion related state of actors. + KinematicStateMap kinematic_state_map; + // Structure containing static attributes of actors. + StaticAttributeMap static_attribute_map; + // Structure containing dynamic traffic light related state of actors. + TrafficLightStateMap tl_state_map; + +public : + SimulationState(); + + // Method to add an actor to the simulation state. + void AddActor(ActorId actor_id, + KinematicState kinematic_state, + StaticAttributes attributes, + TrafficLightState tl_state); + + // Method to verify if an actor is present currently present in the simulation state. + bool ContainsActor(ActorId actor_id) const; + + // Method to remove an actor from simulation state. + void RemoveActor(ActorId actor_id); + + // Method to flush all states and actors. + void Reset(); + + void UpdateKinematicState(ActorId actor_id, KinematicState state); + + void UpdateTrafficLightState(ActorId actor_id, TrafficLightState state); + + cg::Location GetLocation(const ActorId actor_id) const; + + cg::Rotation GetRotation(const ActorId actor_id) const; + + cg::Vector3D GetHeading(const ActorId actor_id) const; + + cg::Vector3D GetVelocity(const ActorId actor_id) const; + + float GetSpeedLimit(const ActorId actor_id) const; + + bool IsPhysicsEnabled(const ActorId actor_id) const; + + TrafficLightState GetTLS(const ActorId actor_id) const; + + ActorType GetType(const ActorId actor_id) const; + + cg::Vector3D GetDimensions(const ActorId actor_id) const; + +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/SnippetProfiler.h b/LibCarla/source/carla/trafficmanager/SnippetProfiler.h new file mode 100644 index 000000000..cc33f3a82 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/SnippetProfiler.h @@ -0,0 +1,80 @@ +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include +#include +#include + +namespace carla { +namespace traffic_manager { + +namespace chr = std::chrono; +using namespace chr; +using TimePoint = chr::time_point; + +// This class can be used to measure execution time, total call duration and number of calls +// of any code snippet by assigning it a name. +class SnippetProfiler { + +private: + std::unordered_map print_clocks; + std::unordered_map snippet_clocks; + std::unordered_map> snippet_durations; + std::unordered_map number_of_calls; + +public: + SnippetProfiler(){}; + + void MeasureExecutionTime(std::string snippet_name, bool begin_or_end) { + TimePoint current_time = chr::system_clock::now(); + + if (print_clocks.find(snippet_name) == print_clocks.end()) { + print_clocks.insert({snippet_name, current_time}); + } + if (snippet_clocks.find(snippet_name) == snippet_clocks.end()) { + snippet_clocks.insert({snippet_name, current_time}); + } + if (snippet_durations.find(snippet_name) == snippet_durations.end()) { + snippet_durations.insert({snippet_name, chr::duration()}); + } + if (number_of_calls.find(snippet_name) == number_of_calls.end()) { + number_of_calls.insert({snippet_name, 0u}); + } + + TimePoint &print_clock = print_clocks.at(snippet_name); + TimePoint &snippet_clock = snippet_clocks.at(snippet_name); + chr::duration &snippet_duration = snippet_durations.at(snippet_name); + unsigned long &call_count = number_of_calls.at(snippet_name); + + if (begin_or_end) { + snippet_clock = current_time; + } else { + chr::duration measured_duration = current_time - snippet_clock; + snippet_duration += measured_duration; + ++call_count; + } + + chr::duration print_duration = current_time - print_clock; + if (print_duration.count() > 1.0f) { + call_count = call_count == 0u ? 1 : call_count; + std::cout << "Snippet name : " << snippet_name << ", " + << "avg. duration : " << 1000 * snippet_duration.count() / call_count << " ms, " + << "total duration : " << snippet_duration.count() << " s, " + << "total calls : " << call_count << ", " + << std::endl; + + snippet_duration = 0s; + call_count = 0u; + + print_clock = current_time; + } + } +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/Stage.h b/LibCarla/source/carla/trafficmanager/Stage.h new file mode 100644 index 000000000..2e7189f9a --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/Stage.h @@ -0,0 +1,29 @@ + +#pragma once + +#include "carla/rpc/ActorId.h" + +namespace carla { +namespace traffic_manager { + +using ActorId = carla::rpc::ActorId; + +/// Stage type interface. +class Stage { + +public: + Stage() {}; + Stage(const Stage&) {}; + virtual ~Stage() {}; + + // This method should be called every update cycle + // and represents the core operation of the stage. + virtual void Update(const unsigned long index) = 0; + // This method should remove an actor from the internal state of the stage type. + virtual void RemoveActor(const ActorId actor_id) = 0; + // This method should flush all internal state of the state type. + virtual void Reset() = 0; +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrackTraffic.cpp b/LibCarla/source/carla/trafficmanager/TrackTraffic.cpp new file mode 100644 index 000000000..93f53ab5d --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrackTraffic.cpp @@ -0,0 +1,172 @@ + +#include "carla/trafficmanager/Constants.h" + +#include "carla/trafficmanager/TrackTraffic.h" + +namespace carla { +namespace traffic_manager { + +using constants::TrackTraffic::BUFFER_STEP_THROUGH; +using constants::TrackTraffic::INV_BUFFER_STEP_THROUGH; + +TrackTraffic::TrackTraffic() {} + +void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id, + const std::vector waypoints) { + + DeleteActor(actor_id); + + std::unordered_set current_grids; + // Step through waypoints and update grid list for actor and actor list for grids. + for (auto &waypoint : waypoints) { + UpdatePassingVehicle(waypoint->GetId(), actor_id); + + GeoGridId ggid = waypoint->GetGeodesicGridId(); + current_grids.insert(ggid); + + if (grid_to_actors.find(ggid) != grid_to_actors.end()) { + ActorIdSet &actor_ids = grid_to_actors.at(ggid); + if (actor_ids.find(actor_id) == actor_ids.end()) { + actor_ids.insert(actor_id); + } + } else { + grid_to_actors.insert({ggid, {actor_id}}); + } + } + + actor_to_grids.insert({actor_id, current_grids}); +} + +void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buffer) { + if (!buffer.empty()) { + + // Clear current actor from all grids containing itself. + if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { + std::unordered_set ¤t_grids = actor_to_grids.at(actor_id); + for (auto &grid_id : current_grids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + ActorIdSet &actor_ids = grid_to_actors.at(grid_id); + actor_ids.erase(actor_id); + } + } + + actor_to_grids.erase(actor_id); + } + + // Step through buffer and update grid list for actor and actor list for grids. + std::unordered_set current_grids; + uint64_t buffer_size = buffer.size(); + uint64_t step_size = static_cast(static_cast(buffer_size) * INV_BUFFER_STEP_THROUGH); + for (uint64_t i = 0u; i <= BUFFER_STEP_THROUGH; ++i) { + GeoGridId ggid = buffer.at(std::min(i * step_size, buffer_size - 1u))->GetGeodesicGridId(); + current_grids.insert(ggid); + + // Add grid entry if not present. + if (grid_to_actors.find(ggid) == grid_to_actors.end()) { + grid_to_actors.insert({ggid, {}}); + } + + ActorIdSet &actor_ids = grid_to_actors.at(ggid); + if (actor_ids.find(actor_id) == actor_ids.end()) { + actor_ids.insert(actor_id); + } + } + + actor_to_grids.insert({actor_id, current_grids}); + } +} + +ActorIdSet TrackTraffic::GetOverlappingVehicles(ActorId actor_id) const { + ActorIdSet actor_id_set; + + if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { + const std::unordered_set &grid_ids = actor_to_grids.at(actor_id); + for (auto &grid_id : grid_ids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + const ActorIdSet &actor_ids = grid_to_actors.at(grid_id); + actor_id_set.insert(actor_ids.begin(), actor_ids.end()); + } + } + } + + return actor_id_set; +} + +void TrackTraffic::DeleteActor(ActorId actor_id) { + if (actor_to_grids.find(actor_id) != actor_to_grids.end()) { + std::unordered_set &grid_ids = actor_to_grids.at(actor_id); + for (auto &grid_id : grid_ids) { + if (grid_to_actors.find(grid_id) != grid_to_actors.end()) { + ActorIdSet &actor_ids = grid_to_actors.at(grid_id); + actor_ids.erase(actor_id); + } + } + actor_to_grids.erase(actor_id); + } + + if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) { + WaypointIdSet waypoint_id_set = waypoint_occupied.at(actor_id); + for (const uint64_t &waypoint_id : waypoint_id_set) { + RemovePassingVehicle(waypoint_id, actor_id); + } + } +} + +void TrackTraffic::UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id) { + if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) { + ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id); + if (actor_id_set.find(actor_id) == actor_id_set.end()) { + actor_id_set.insert(actor_id); + } + } else { + waypoint_overlap_tracker.insert({waypoint_id, {actor_id}}); + } + + if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) { + WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id); + if (waypoint_id_set.find(waypoint_id) == waypoint_id_set.end()) { + waypoint_id_set.insert(waypoint_id); + } + } else { + waypoint_occupied.insert({actor_id, {waypoint_id}}); + } +} + +void TrackTraffic::RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id) { + if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) { + ActorIdSet &actor_id_set = waypoint_overlap_tracker.at(waypoint_id); + actor_id_set.erase(actor_id); + + if (actor_id_set.size() == 0) { + waypoint_overlap_tracker.erase(waypoint_id); + } + } + + if (waypoint_occupied.find(actor_id) != waypoint_occupied.end()) { + WaypointIdSet &waypoint_id_set = waypoint_occupied.at(actor_id); + waypoint_id_set.erase(waypoint_id); + + if (waypoint_id_set.size() == 0) { + waypoint_occupied.erase(actor_id); + } + } +} + +ActorIdSet TrackTraffic::GetPassingVehicles(uint64_t waypoint_id) const { + + if (waypoint_overlap_tracker.find(waypoint_id) != waypoint_overlap_tracker.end()) { + return waypoint_overlap_tracker.at(waypoint_id); + } else { + return ActorIdSet(); + } +} + +void TrackTraffic::Clear() { + waypoint_overlap_tracker.clear(); + waypoint_occupied.clear(); + actor_to_grids.clear(); + grid_to_actors.clear(); +} + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrackTraffic.h b/LibCarla/source/carla/trafficmanager/TrackTraffic.h new file mode 100644 index 000000000..8564f4937 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/TrackTraffic.h @@ -0,0 +1,56 @@ + +#pragma once + +#include "carla/road/RoadTypes.h" +#include "carla/rpc/ActorId.h" + +#include "carla/trafficmanager/SimpleWaypoint.h" + +namespace carla { +namespace traffic_manager { + +using ActorId = carla::ActorId; +using ActorIdSet = std::unordered_set; +using SimpleWaypointPtr = std::shared_ptr; +using Buffer = std::deque; +using GeoGridId = carla::road::JuncId; + +// This class is used to track the waypoint occupancy of all the actors. +class TrackTraffic { + +private: + /// Structure to keep track of overlapping waypoints between vehicles. + using WaypointOverlap = std::unordered_map; + WaypointOverlap waypoint_overlap_tracker; + + /// Structure to keep track of waypoints occupied by vehicles; + using WaypointIdSet = std::unordered_set; + using WaypointOccupancyMap = std::unordered_map; + WaypointOccupancyMap waypoint_occupied; + + /// Geodesic grids occupied by actors's paths. + std::unordered_map> actor_to_grids; + /// Actors currently passing through grids. + std::unordered_map grid_to_actors; + +public: + TrackTraffic(); + + /// Methods to update, remove and retrieve vehicles passing through a waypoint. + void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id); + void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id); + ActorIdSet GetPassingVehicles(uint64_t waypoint_id) const; + + void UpdateGridPosition(const ActorId actor_id, const Buffer &buffer); + void UpdateUnregisteredGridPosition(const ActorId actor_id, + const std::vector waypoints); + + ActorIdSet GetOverlappingVehicles(ActorId actor_id) const; + /// Method to delete actor data from tracking. + void DeleteActor(ActorId actor_id); + + void Clear(); +}; + +} // namespace traffic_manager +} // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index dfc6c9115..d778f9a03 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -1,191 +1,135 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . -#include +#include "carla/trafficmanager/Constants.h" +#include "carla/trafficmanager/LocalizationUtils.h" #include "carla/trafficmanager/TrafficLightStage.h" namespace carla { namespace traffic_manager { - static const uint64_t NO_SIGNAL_PASSTHROUGH_INTERVAL = 5u; +using constants::TrafficLight::NO_SIGNAL_PASSTHROUGH_INTERVAL; +using constants::TrafficLight::DOUBLE_NO_SIGNAL_PASSTHROUGH_INTERVAL; +using constants::WaypointSelection::JUNCTION_LOOK_AHEAD; - TrafficLightStage::TrafficLightStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr planner_messenger, - Parameters ¶meters, - cc::DebugHelper &debug_helper) - : PipelineStage(stage_name), - localization_messenger(localization_messenger), - planner_messenger(planner_messenger), - parameters(parameters), - debug_helper(debug_helper){ +TrafficLightStage::TrafficLightStage( + const std::vector &vehicle_id_list, + const SimulationState &simulation_state, + const BufferMap &buffer_map, + const Parameters ¶meters, + TLFrame &output_array) + : vehicle_id_list(vehicle_id_list), + simulation_state(simulation_state), + buffer_map(buffer_map), + parameters(parameters), + output_array(output_array) {} - // Initializing output frame selector. - frame_selector = true; - // Initializing number of vehicles to zero in the beginning. - number_of_vehicles = 0u; - /// @todo: replace with RandomEngine - // Initializing srand. - srand(static_cast(time(NULL))); +void TrafficLightStage::Update(const unsigned long index) { + bool traffic_light_hazard = false; + const ActorId ego_actor_id = vehicle_id_list.at(index); + const Buffer &waypoint_buffer = buffer_map.at(ego_actor_id); + const SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first; + + const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId(); + const TimeInstance current_time = chr::system_clock::now(); + + const TrafficLightState tl_state = simulation_state.GetTLS(ego_actor_id); + const TLS traffic_light_state = tl_state.tl_state; + const bool is_at_traffic_light = tl_state.at_traffic_light; + + // We determine to stop if the current position of the vehicle is not a + // junction and there is a red or yellow light. + if (is_at_traffic_light && + traffic_light_state != TLS::Green && + parameters.GetPercentageRunningLight(ego_actor_id) <= pgen.next()) { + + traffic_light_hazard = true; + } + // Handle entry negotiation at non-signalised junction. + else if (look_ahead_point->CheckJunction() + && !is_at_traffic_light + && traffic_light_state != TLS::Green + && parameters.GetPercentageRunningSign(ego_actor_id) <= pgen.next()) { + + traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_time); } - TrafficLightStage::~TrafficLightStage() {} + output_array.at(index) = traffic_light_hazard; +} - void TrafficLightStage::Action() { +bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id, + const TimeInstance current_time) { - // Selecting the output frame based on the selection key. - const auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b; + bool traffic_light_hazard = false; - // Looping over registered actors. - for (uint64_t i = 0u; i < number_of_vehicles && localization_frame != nullptr; ++i) { - - bool traffic_light_hazard = false; - const LocalizationToTrafficLightData &data = localization_frame->at(i); - if (!data.actor->IsAlive()) { - continue; - } - - const Actor ego_actor = data.actor; - const ActorId ego_actor_id = ego_actor->GetId(); - const SimpleWaypointPtr closest_waypoint = data.closest_waypoint; - const SimpleWaypointPtr look_ahead_point = data.junction_look_ahead_waypoint; - - const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId(); - const TimeInstance current_time = chr::system_clock::now(); - - const auto ego_vehicle = boost::static_pointer_cast(ego_actor); - TLS traffic_light_state = ego_vehicle->GetTrafficLightState(); - - // We determine to stop if the current position of the vehicle is not a - // junction and there is a red or yellow light. - if (ego_vehicle->IsAtTrafficLight() && - traffic_light_state != TLS::Green && - parameters.GetPercentageRunningLight(boost::shared_ptr(ego_actor)) <= (rand() % 101)) { - - traffic_light_hazard = true; - } - - // Handle entry negotiation at non-signalised junction. - else if (look_ahead_point->CheckJunction() && - !ego_vehicle->IsAtTrafficLight() && - traffic_light_state != TLS::Green && - parameters.GetPercentageRunningSign(boost::shared_ptr(ego_actor)) <= (rand() % 101)) { - - std::lock_guard lock(no_signal_negotiation_mutex); - - if (vehicle_last_junction.find(ego_actor_id) == vehicle_last_junction.end()) { - // Initializing new map entry for the actor with - // an arbitrary and different junction id. - vehicle_last_junction.insert({ego_actor_id, junction_id + 1}); - } - - if (vehicle_last_junction.at(ego_actor_id) != junction_id) { - vehicle_last_junction.at(ego_actor_id) = junction_id; - - // Check if the vehicle has an outdated ticket or needs a new one. - bool need_to_issue_new_ticket = false; - if (vehicle_last_ticket.find(ego_actor_id) == vehicle_last_ticket.end()) { - - need_to_issue_new_ticket = true; - } else { - - const TimeInstance &previous_ticket = vehicle_last_ticket.at(ego_actor_id); - const chr::duration diff = current_time - previous_ticket; - if (diff.count() > NO_SIGNAL_PASSTHROUGH_INTERVAL) { - need_to_issue_new_ticket = true; - } - } - - // If new ticket is needed for the vehicle, then query the junction - // ticket map - // and update the map value to the new ticket value. - if (need_to_issue_new_ticket) { - if (junction_last_ticket.find(junction_id) != junction_last_ticket.end()) { - - TimeInstance &last_ticket = junction_last_ticket.at(junction_id); - const chr::duration diff = current_time - last_ticket; - if (diff.count() > 0.0) { - last_ticket = current_time + chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL); - } else { - last_ticket += chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL); - } - } else { - junction_last_ticket.insert({junction_id, current_time + - chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL)}); - } - if (vehicle_last_ticket.find(ego_actor_id) != vehicle_last_ticket.end()) { - vehicle_last_ticket.at(ego_actor_id) = junction_last_ticket.at(junction_id); - } else { - vehicle_last_ticket.insert({ego_actor_id, junction_last_ticket.at(junction_id)}); - } - } - } - - // If current time is behind ticket time, then do not enter junction. - const TimeInstance ¤t_ticket = vehicle_last_ticket.at(ego_actor_id); - const chr::duration diff = current_ticket - current_time; - if (diff.count() > 0.0) { - traffic_light_hazard = true; - } - } - - TrafficLightToPlannerData &message = current_planner_frame->at(i); - message.traffic_light_hazard = traffic_light_hazard; - } + if (vehicle_last_junction.find(ego_actor_id) == vehicle_last_junction.end()) { + // Initializing new map entry for the actor with + // an arbitrary and different junction id. + vehicle_last_junction.insert({ego_actor_id, junction_id + 1}); } - void TrafficLightStage::DataReceiver() { + if (vehicle_last_junction.at(ego_actor_id) != junction_id) { + vehicle_last_junction.at(ego_actor_id) = junction_id; - localization_frame = localization_messenger->Peek(); + // Check if the vehicle has an outdated ticket or needs a new one. + bool need_to_issue_new_ticket = false; + if (vehicle_last_ticket.find(ego_actor_id) == vehicle_last_ticket.end()) { - // Allocating new containers for the changed number of registered vehicles. - if (localization_frame != nullptr && number_of_vehicles != (*localization_frame.get()).size()) { - - number_of_vehicles = static_cast((*localization_frame.get()).size()); - - // Allocating output frames. - planner_frame_a = std::make_shared(number_of_vehicles); - planner_frame_b = std::make_shared(number_of_vehicles); - } - } - - void TrafficLightStage::DataSender() { - - localization_messenger->Pop(); - - planner_messenger->Push(frame_selector ? planner_frame_a : planner_frame_b); - frame_selector = !frame_selector; - } - - void TrafficLightStage::DrawLight(TLS traffic_light_state, const Actor &ego_actor) const { - - const cg::Location ego_location = ego_actor->GetLocation(); - if (traffic_light_state == TLS::Green) { - debug_helper.DrawString( - cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f), - "Green", - false, - {0u, 255u, 0u}, 0.1f, true); - } else if (traffic_light_state == TLS::Yellow) { - debug_helper.DrawString( - cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f), - "Yellow", - false, - {255u, 255u, 0u}, 0.1f, true); + need_to_issue_new_ticket = true; } else { - debug_helper.DrawString( - cg::Location(ego_location.x, ego_location.y, ego_location.z+1.0f), - "Red", - false, - {255u, 0u, 0u}, 0.1f, true); + + const TimeInstance &previous_ticket = vehicle_last_ticket.at(ego_actor_id); + const chr::duration diff = current_time - previous_ticket; + if (diff.count() > DOUBLE_NO_SIGNAL_PASSTHROUGH_INTERVAL) { + need_to_issue_new_ticket = true; + } + } + + // If new ticket is needed for the vehicle, then query the junction + // ticket map + // and update the map value to the new ticket value. + if (need_to_issue_new_ticket) { + if (junction_last_ticket.find(junction_id) != junction_last_ticket.end()) { + + TimeInstance &last_ticket = junction_last_ticket.at(junction_id); + const chr::duration diff = current_time - last_ticket; + if (diff.count() > 0.0) { + last_ticket = current_time + chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL); + } else { + last_ticket += chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL); + } + } else { + junction_last_ticket.insert({junction_id, current_time + + chr::seconds(NO_SIGNAL_PASSTHROUGH_INTERVAL)}); + } + if (vehicle_last_ticket.find(ego_actor_id) != vehicle_last_ticket.end()) { + vehicle_last_ticket.at(ego_actor_id) = junction_last_ticket.at(junction_id); + } else { + vehicle_last_ticket.insert({ego_actor_id, junction_last_ticket.at(junction_id)}); + } } } + // If current time is behind ticket time, then do not enter junction. + const TimeInstance ¤t_ticket = vehicle_last_ticket.at(ego_actor_id); + const chr::duration diff = current_ticket - current_time; + if (diff.count() > 0.0) { + traffic_light_hazard = true; + } + + return traffic_light_hazard; +} + +void TrafficLightStage::RemoveActor(const ActorId actor_id) { + vehicle_last_ticket.erase(actor_id); + vehicle_last_junction.erase(actor_id); +} + +void TrafficLightStage::Reset() { + vehicle_last_ticket.clear(); + vehicle_last_junction.clear(); + junction_last_ticket.clear(); +} + } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h index d2668df09..6f74a9e80 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h @@ -1,94 +1,48 @@ -// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma -// de Barcelona (UAB). -// -// This work is licensed under the terms of the MIT license. -// For a copy, see . #pragma once -#include -#include -#include -#include - -#include "carla/client/ActorList.h" -#include "carla/client/TrafficLight.h" -#include "carla/client/Vehicle.h" -#include "carla/client/World.h" -#include "carla/Memory.h" -#include "carla/rpc/TrafficLightState.h" - -#include "carla/trafficmanager/MessengerAndDataTypes.h" +#include "carla/trafficmanager/DataStructures.h" #include "carla/trafficmanager/Parameters.h" -#include "carla/trafficmanager/PipelineStage.h" - +#include "carla/trafficmanager/RandomGenerator.h" +#include "carla/trafficmanager/SimulationState.h" +#include "carla/trafficmanager/Stage.h" namespace carla { namespace traffic_manager { - namespace chr = std::chrono; - namespace cc = carla::client; - namespace cg = carla::geom; +/// This class has functionality for responding to traffic lights +/// and managing entry into non-signalized junctions. +class TrafficLightStage: Stage { +private: + const std::vector &vehicle_id_list; + const SimulationState &simulation_state; + const BufferMap &buffer_map; + const Parameters ¶meters; + /// Map containing the time ticket issued for vehicles. + std::unordered_map vehicle_last_ticket; + /// Map containing the previous time ticket issued for junctions. + std::unordered_map junction_last_ticket; + /// Map containing the previous junction visited by a vehicle. + std::unordered_map vehicle_last_junction; + TLFrame &output_array; + RandomGenerator<> pgen; - using ActorId = carla::ActorId; - using Actor = carla::SharedPtr; - using JunctionID = carla::road::JuncId; - using SimpleWaypointPtr = std::shared_ptr; - using TrafficLight = carla::SharedPtr; - using TLS = carla::rpc::TrafficLightState; - using TimeInstance = chr::time_point; + bool HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id, + const TimeInstance current_time); - /// This class provides the information about the Traffic Lights at the - /// junctions. - class TrafficLightStage : public PipelineStage { +public: + TrafficLightStage(const std::vector &vehicle_id_list, + const SimulationState &Simulation_state, + const BufferMap &buffer_map, + const Parameters ¶meters, + TLFrame &output_array); - private: + void Update(const unsigned long index) override; - /// Selection key to switch between output frames. - bool frame_selector; - /// Pointer data frame received from localization stage. - std::shared_ptr localization_frame; - /// Pointers to data frames to be shared with motion planner stage. - std::shared_ptr planner_frame_a; - std::shared_ptr planner_frame_b; - /// Pointers to messenger objects. - std::shared_ptr localization_messenger; - std::shared_ptr planner_messenger; - /// Runtime parameterization object. - Parameters ¶meters; - /// Reference to Carla's debug helper object. - cc::DebugHelper &debug_helper; - /// Map containing the time ticket issued for vehicles. - std::unordered_map vehicle_last_ticket; - /// Map containing the previous time ticket issued for junctions. - std::unordered_map junction_last_ticket; - /// Map containing the previous junction visited by a vehicle. - std::unordered_map vehicle_last_junction; - /// No signal negotiation mutex. - std::mutex no_signal_negotiation_mutex; - /// Number of vehicles registered with the traffic manager. - uint64_t number_of_vehicles; + void RemoveActor(const ActorId actor_id) override; - - void DrawLight(TLS traffic_light_state, const Actor &ego_actor) const; - - public: - - TrafficLightStage( - std::string stage_name, - std::shared_ptr localization_messenger, - std::shared_ptr planner_messenger, - Parameters ¶meters, - cc::DebugHelper &debug_helper); - ~TrafficLightStage(); - - void DataReceiver() override; - - void Action() override; - - void DataSender() override; - - }; + void Reset() override; +}; } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp index 890875270..8be165df9 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp @@ -8,6 +8,8 @@ #include "carla/Sockets.h" +#include "carla/client/detail/Simulator.h" + #include "carla/trafficmanager/TrafficManager.h" #include "carla/trafficmanager/TrafficManagerBase.h" #include "carla/trafficmanager/TrafficManagerLocal.h" diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.h b/LibCarla/source/carla/trafficmanager/TrafficManager.h index 45a4b7893..74c91a848 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.h @@ -11,11 +11,14 @@ #include #include "carla/client/Actor.h" +#include "carla/trafficmanager/Constants.h" #include "carla/trafficmanager/TrafficManagerBase.h" namespace carla { namespace traffic_manager { +using constants::Networking::TM_DEFAULT_PORT; + using ActorPtr = carla::SharedPtr; /// This class integrates all the various stages of diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h b/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h index af1ab244e..587b226bb 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerBase.h @@ -9,9 +9,6 @@ #include #include "carla/client/Actor.h" -#define MIN_TRY_COUNT 20 -#define TM_DEFAULT_PORT 8000 - namespace carla { namespace traffic_manager { diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h b/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h index 0eb6cf863..bc161584b 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerClient.h @@ -6,15 +6,17 @@ #pragma once -#include "carla/client/Actor.h" -#include +#include "carla/trafficmanager/Constants.h" +#include "carla/rpc/Actor.h" -#define TM_TIMEOUT 2000 // In ms -#define TM_DEFAULT_PORT 8000 // TM_SERVER_PORT +#include namespace carla { namespace traffic_manager { +using constants::Networking::TM_TIMEOUT; +using constants::Networking::TM_DEFAULT_PORT; + /// Provides communication with the rpc of TrafficManagerServer. class TrafficManagerClient { diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp index e8cf9cd78..2bacf7550 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp @@ -4,149 +4,274 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . +#include "carla/client/detail/Simulator.h" + #include "carla/trafficmanager/TrafficManagerLocal.h" namespace carla { namespace traffic_manager { +using namespace constants::FrameMemory; + TrafficManagerLocal::TrafficManagerLocal( - std::vector longitudinal_PID_parameters, - std::vector longitudinal_highway_PID_parameters, - std::vector lateral_PID_parameters, - std::vector lateral_highway_PID_parameters, - float perc_difference_from_limit, - carla::client::detail::EpisodeProxy& episodeProxy, - uint16_t& RPCportTM) + std::vector longitudinal_PID_parameters, + std::vector longitudinal_highway_PID_parameters, + std::vector lateral_PID_parameters, + std::vector lateral_highway_PID_parameters, + float perc_difference_from_limit, + cc::detail::EpisodeProxy &episode_proxy, + uint16_t &RPCportTM) + : longitudinal_PID_parameters(longitudinal_PID_parameters), longitudinal_highway_PID_parameters(longitudinal_highway_PID_parameters), lateral_PID_parameters(lateral_PID_parameters), lateral_highway_PID_parameters(lateral_highway_PID_parameters), - episodeProxyTM(episodeProxy), - debug_helper(carla::client::DebugHelper{episodeProxyTM}), + + episode_proxy(episode_proxy), + world(cc::World(episode_proxy)), + debug_helper(world.MakeDebugHelper()), + + localization_stage(LocalizationStage(vehicle_id_list, + buffer_map, + simulation_state, + track_traffic, + local_map, + parameters, + localization_frame, + debug_helper)), + + collision_stage(CollisionStage(vehicle_id_list, + simulation_state, + buffer_map, + track_traffic, + parameters, + collision_frame, + debug_helper)), + + traffic_light_stage(TrafficLightStage(vehicle_id_list, + simulation_state, + buffer_map, + parameters, + tl_frame)), + + motion_plan_stage(MotionPlanStage(vehicle_id_list, + simulation_state, + parameters, + buffer_map, + track_traffic, + longitudinal_PID_parameters, + longitudinal_highway_PID_parameters, + lateral_PID_parameters, + lateral_highway_PID_parameters, + localization_frame, + collision_frame, + tl_frame, + control_frame)), + + alsm(ALSM(registered_vehicles, + buffer_map, + track_traffic, + parameters, + world, + local_map, + simulation_state, + localization_stage, + collision_stage, + traffic_light_stage, + motion_plan_stage)), + server(TrafficManagerServer(RPCportTM, static_cast(this))) { parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit); - Start(); + registered_vehicles_state = -1; + SetupLocalMap(); + + Start(); } TrafficManagerLocal::~TrafficManagerLocal() { - episodeProxyTM.Lock()->DestroyTrafficManager(server.port()); + episode_proxy.Lock()->DestroyTrafficManager(server.port()); Release(); } -void TrafficManagerLocal::Start() { - - const carla::SharedPtr world_map = episodeProxyTM.Lock()->GetCurrentMap(); - local_map = std::make_shared(world_map); +void TrafficManagerLocal::SetupLocalMap() { + const carla::SharedPtr world_map = world.GetMap(); + local_map = std::make_shared(world_map); local_map->SetUp(); +} - localization_collision_messenger = std::make_shared(); - localization_traffic_light_messenger = std::make_shared(); - collision_planner_messenger = std::make_shared(); - localization_planner_messenger = std::make_shared(); - traffic_light_planner_messenger = std::make_shared(); - planner_control_messenger = std::make_shared(); +void TrafficManagerLocal::Start() { + run_traffic_manger.store(true); + worker_thread = std::make_unique(&TrafficManagerLocal::Run, this); +} - localization_stage = std::make_unique( - "Localization stage", - localization_planner_messenger, localization_collision_messenger, - localization_traffic_light_messenger, - registered_actors, *local_map.get(), - parameters, debug_helper, - episodeProxyTM); +void TrafficManagerLocal::Run() { - collision_stage = std::make_unique( - "Collision stage", - localization_collision_messenger, collision_planner_messenger, - parameters, debug_helper); + localization_frame.reserve(INITIAL_SIZE); + collision_frame.reserve(INITIAL_SIZE); + tl_frame.reserve(INITIAL_SIZE); + control_frame.reserve(INITIAL_SIZE); + current_reserved_capacity = INITIAL_SIZE; - traffic_light_stage = std::make_unique( - "Traffic light stage", - localization_traffic_light_messenger, traffic_light_planner_messenger, - parameters, debug_helper); + while (run_traffic_manger.load()) { + bool synchronous_mode = parameters.GetSynchronousMode(); + bool hybrid_physics_mode = parameters.GetHybridPhysicsMode(); - planner_stage = std::make_unique( - "Motion planner stage", - localization_planner_messenger, - collision_planner_messenger, - traffic_light_planner_messenger, - planner_control_messenger, - parameters, - longitudinal_PID_parameters, - longitudinal_highway_PID_parameters, - lateral_PID_parameters, - lateral_highway_PID_parameters, - debug_helper); + // Wait for external trigger to initiate cycle in synchronous mode. + if (synchronous_mode) { + std::unique_lock lock(step_execution_mutex); + step_begin_trigger.wait(lock, [this]() {return step_begin.load();}); + step_begin.store(false); + } - control_stage = std::make_unique( - "Batch control stage", - planner_control_messenger, - episodeProxyTM, - parameters); + // Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode. + if (!synchronous_mode && hybrid_physics_mode) { + TimePoint current_instance = chr::system_clock::now(); + chr::duration elapsed_time = current_instance - previous_update_instance; + float time_to_wait = HYBRID_MODE_DT - elapsed_time.count(); + if (time_to_wait > 0.0f) { + std::this_thread::sleep_for(chr::duration(time_to_wait)); + } + previous_update_instance = current_instance; + } - localization_collision_messenger->Start(); - localization_traffic_light_messenger->Start(); - localization_planner_messenger->Start(); - collision_planner_messenger->Start(); - traffic_light_planner_messenger->Start(); - planner_control_messenger->Start(); + // Updating simulation state, actor life cycle and performing necessary cleanup. + alsm.Update(); - localization_stage->Start(); - collision_stage->Start(); - traffic_light_stage->Start(); - planner_stage->Start(); - control_stage->Start(); + // Re-allocating inter-stage communication frames based on changed number of registered vehicles. + int current_registered_vehicles_state = registered_vehicles.GetState(); + unsigned long number_of_vehicles = vehicle_id_list.size(); + if (registered_vehicles_state != current_registered_vehicles_state || number_of_vehicles != registered_vehicles.Size()) { + vehicle_id_list = registered_vehicles.GetIDList(); + number_of_vehicles = vehicle_id_list.size(); + + // Reserve more space if needed. + uint64_t growth_factor = static_cast(static_cast(number_of_vehicles) * INV_GROWTH_STEP_SIZE); + uint64_t new_frame_capacity = INITIAL_SIZE + GROWTH_STEP_SIZE * growth_factor; + if (new_frame_capacity > current_reserved_capacity) { + localization_frame.reserve(new_frame_capacity); + collision_frame.reserve(new_frame_capacity); + tl_frame.reserve(new_frame_capacity); + control_frame.reserve(new_frame_capacity); + } + + registered_vehicles_state = registered_vehicles.GetState(); + } + + // Reset frames for current cycle. + localization_frame.clear(); + localization_frame.resize(number_of_vehicles); + collision_frame.clear(); + collision_frame.resize(number_of_vehicles); + tl_frame.clear(); + tl_frame.resize(number_of_vehicles); + control_frame.clear(); + control_frame.resize(number_of_vehicles); + + // Run core operation stages. + for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) { + localization_stage.Update(index); + } + + for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) { + collision_stage.Update(index); + traffic_light_stage.Update(index); + motion_plan_stage.Update(index); + } + collision_stage.ClearCycleCache(); + + // Building the command array for current cycle. + std::vector batch_command(number_of_vehicles); + for (unsigned long i = 0u; i < number_of_vehicles; ++i) { + batch_command.at(i) = control_frame.at(i); + } + + // Sending the current cycle's batch command to the simulator. + episode_proxy.Lock()->ApplyBatchSync(std::move(batch_command), false); + if (synchronous_mode) { + step_end.store(true); + step_end_trigger.notify_one(); + } + } +} + +bool TrafficManagerLocal::SynchronousTick() { + if (parameters.GetSynchronousMode()) { + step_begin.store(true); + step_begin_trigger.notify_one(); + + std::unique_lock lock(step_execution_mutex); + step_end_trigger.wait(lock, [this]() { return step_end.load(); }); + step_end.store(false); + } + return true; } void TrafficManagerLocal::Stop() { - localization_collision_messenger->Stop(); - localization_traffic_light_messenger->Stop(); - localization_planner_messenger->Stop(); - collision_planner_messenger->Stop(); - traffic_light_planner_messenger->Stop(); - planner_control_messenger->Stop(); - localization_stage->Stop(); - collision_stage->Stop(); - traffic_light_stage->Stop(); - planner_stage->Stop(); - control_stage->Stop(); + run_traffic_manger.store(false); + + if (worker_thread) { + if (worker_thread->joinable()) { + worker_thread->join(); + } + worker_thread.release(); + } + + vehicle_id_list.clear(); + registered_vehicles.Clear(); + registered_vehicles_state = -1; + track_traffic.Clear(); + previous_update_instance = chr::system_clock::now(); + current_reserved_capacity = 0u; + + simulation_state.Reset(); + localization_stage.Reset(); + collision_stage.Reset(); + traffic_light_stage.Reset(); + motion_plan_stage.Reset(); + + buffer_map.clear(); + localization_frame.clear(); + collision_frame.clear(); + tl_frame.clear(); + control_frame.clear(); + + run_traffic_manger.store(true); + step_begin.store(false); + step_end.store(false); } void TrafficManagerLocal::Release() { + Stop(); - localization_collision_messenger.reset(); - localization_traffic_light_messenger.reset(); - localization_planner_messenger.reset(); - collision_planner_messenger.reset(); - traffic_light_planner_messenger.reset(); - planner_control_messenger.reset(); - localization_stage.reset(); - collision_stage.reset(); - traffic_light_stage.reset(); - planner_stage.reset(); - control_stage.reset(); + + local_map.reset(); } void TrafficManagerLocal::Reset() { Release(); - - carla::client::detail::EpisodeProxy episode_proxy = episodeProxyTM.Lock()->GetCurrentEpisode(); - episodeProxyTM = episode_proxy; - + episode_proxy = episode_proxy.Lock()->GetCurrentEpisode(); + world = cc::World(episode_proxy); + SetupLocalMap(); Start(); } -void TrafficManagerLocal::RegisterVehicles(const std::vector& actor_list) { - registered_actors.Insert(actor_list); +void TrafficManagerLocal::RegisterVehicles(const std::vector &vehicle_list) { + registered_vehicles.Insert(vehicle_list); } -void TrafficManagerLocal::UnregisterVehicles(const std::vector& actor_list) { - registered_actors.Remove(actor_list); +void TrafficManagerLocal::UnregisterVehicles(const std::vector &actor_list) { + + std::vector actor_id_list; + for (auto &actor : actor_list) { + actor_id_list.push_back(actor->GetId()); + } + registered_vehicles.Remove(actor_id_list); } void TrafficManagerLocal::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) { @@ -206,7 +331,7 @@ void TrafficManagerLocal::SetHybridPhysicsRadius(const float radius) { } bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) { - for (auto& elem : tl_to_freeze) { + for (auto &elem : tl_to_freeze) { if (!elem->IsFrozen() || elem->GetState() != TLS::Red) { return false; } @@ -216,52 +341,39 @@ bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) { void TrafficManagerLocal::ResetAllTrafficLights() { - auto Filter = [&](auto &actors, auto &wildcard_pattern) { - std::vector filtered; - for (auto &&actor : actors) { - if (carla::StringUtil::Match(carla::client::detail::ActorVariant(actor).GetTypeId(), wildcard_pattern)) { - filtered.push_back(actor); - } - } - return filtered; - }; - - // Get all actors of the world. - auto world_actorsList = episodeProxyTM.Lock()->GetAllTheActorsInTheEpisode(); - // Filter based on wildcard pattern. - const auto world_traffic_lights = Filter(world_actorsList, "*traffic_light*"); + const auto world_traffic_lights = world.GetActors()->Filter("*traffic_light*"); std::vector list_of_all_groups; TLGroup tl_to_freeze; std::vector list_of_ids; - for (auto tl : world_traffic_lights) { - if (!(std::find(list_of_ids.begin(), list_of_ids.end(), tl.GetId()) != list_of_ids.end())) { - const TLGroup tl_group = boost::static_pointer_cast(tl.Get(episodeProxyTM))->GetGroupTrafficLights(); + for (auto iter = world_traffic_lights->begin(); iter != world_traffic_lights->end(); iter++) { + auto tl = *iter; + if (!(std::find(list_of_ids.begin(), list_of_ids.end(), tl->GetId()) != list_of_ids.end())) { + const TLGroup tl_group = boost::static_pointer_cast(tl)->GetGroupTrafficLights(); list_of_all_groups.push_back(tl_group); - for (uint64_t i=0u; iGetId()); - if(i!=0u) { + if (i != 0u) { tl_to_freeze.push_back(tl_group.at(i)); } } } } - for (TLGroup& tl_group : list_of_all_groups) { + for (TLGroup &tl_group : list_of_all_groups) { tl_group.front()->SetState(TLS::Green); std::for_each( - tl_group.begin()+1, tl_group.end(), - [] (auto& tl) {tl->SetState(TLS::Red);}); + tl_group.begin() + 1, tl_group.end(), + [](auto &tl) { tl->SetState(TLS::Red); }); } while (!CheckAllFrozen(tl_to_freeze)) { - for (auto& tln : tl_to_freeze) { + for (auto &tln : tl_to_freeze) { tln->SetState(TLS::Red); tln->Freeze(true); } } - } void TrafficManagerLocal::SetSynchronousMode(bool mode) { @@ -272,19 +384,12 @@ void TrafficManagerLocal::SetSynchronousModeTimeOutInMiliSecond(double time) { parameters.SetSynchronousModeTimeOutInMiliSecond(time); } -bool TrafficManagerLocal::SynchronousTick() { - localization_stage->RunStep(); - control_stage->RunStep(); - return true; -} - -carla::client::detail::EpisodeProxy& TrafficManagerLocal::GetEpisodeProxy() { - return episodeProxyTM; +carla::client::detail::EpisodeProxy &TrafficManagerLocal::GetEpisodeProxy() { + return episode_proxy; } std::vector TrafficManagerLocal::GetRegisteredVehiclesIDs() { - - return registered_actors.GetIDList(); + return registered_vehicles.GetIDList(); } } // namespace traffic_manager diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h index 493f91777..9f376b81a 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h @@ -6,176 +6,215 @@ #pragma once +#include +#include +#include #include -#include "carla/client/detail/Simulator.h" +#include "carla/client/DebugHelper.h" #include "carla/client/detail/EpisodeProxy.h" +#include "carla/client/TrafficLight.h" +#include "carla/client/World.h" +#include "carla/Memory.h" +#include "carla/rpc/Command.h" #include "carla/trafficmanager/AtomicActorSet.h" -#include "carla/trafficmanager/BatchControlStage.h" -#include "carla/trafficmanager/CollisionStage.h" #include "carla/trafficmanager/InMemoryMap.h" -#include "carla/trafficmanager/LocalizationStage.h" -#include "carla/trafficmanager/MotionPlannerStage.h" #include "carla/trafficmanager/Parameters.h" -#include "carla/trafficmanager/TrafficLightStage.h" - +#include "carla/trafficmanager/SimulationState.h" +#include "carla/trafficmanager/TrackTraffic.h" #include "carla/trafficmanager/TrafficManagerBase.h" #include "carla/trafficmanager/TrafficManagerServer.h" +#include "carla/trafficmanager/ALSM.h" +#include "carla/trafficmanager/LocalizationStage.h" +#include "carla/trafficmanager/CollisionStage.h" +#include "carla/trafficmanager/TrafficLightStage.h" +#include "carla/trafficmanager/MotionPlanStage.h" + namespace carla { namespace traffic_manager { - using ActorPtr = carla::SharedPtr; - using TLGroup = std::vector>; +namespace chr = std::chrono; - /// The function of this class is to integrate all the various stages of - /// the traffic manager appropriately using messengers. - class TrafficManagerLocal : public TrafficManagerBase { +using namespace std::chrono_literals; - private: +using TimePoint = chr::time_point; +using TLGroup = std::vector>; +using LocalMapPtr = std::shared_ptr; +using constants::HybridMode::HYBRID_MODE_DT; - /// PID controller parameters. - std::vector longitudinal_PID_parameters; - std::vector longitudinal_highway_PID_parameters; - std::vector lateral_PID_parameters; - std::vector lateral_highway_PID_parameters; +/// The function of this class is to integrate all the various stages of +/// the traffic manager appropriately using messengers. +class TrafficManagerLocal : public TrafficManagerBase { - /// Set of all actors registered with traffic manager. - AtomicActorSet registered_actors; +private: + /// PID controller parameters. + std::vector longitudinal_PID_parameters; + std::vector longitudinal_highway_PID_parameters; + std::vector lateral_PID_parameters; + std::vector lateral_highway_PID_parameters; + /// Carla's client connection object. + carla::client::detail::EpisodeProxy episode_proxy; + /// Carla client and object. + cc::World world; + /// Set of all actors registered with traffic manager. + AtomicActorSet registered_vehicles; + /// State counter to track changes in registered actors. + int registered_vehicles_state; + /// List of vehicles registered with the traffic manager in + /// current update cycle. + std::vector vehicle_id_list; + /// Pointer to local map cache. + LocalMapPtr local_map; + /// Structures to hold waypoint buffers for all vehicles. + BufferMap buffer_map; + /// Carla's debug helper object. + cc::DebugHelper debug_helper; + /// Object for tracking paths of the traffic vehicles. + TrackTraffic track_traffic; + /// Type containing the current state of all actors involved in the simulation. + SimulationState simulation_state; + /// Time instance used to calculate dt in asynchronous mode. + TimePoint previous_update_instance; + /// Parameterization object. + Parameters parameters; + /// Array to hold output data of localization stage. + LocalizationFrame localization_frame; + /// Array to hold output data of collision avoidance. + CollisionFrame collision_frame; + /// Array to hold output data of traffic light response. + TLFrame tl_frame; + /// Array to hold output data of motion planning. + ControlFrame control_frame; + /// Variable to keep track of currently reserved array space for frames. + uint64_t current_reserved_capacity {0u}; + /// Various stages representing core operations of traffic manager. + LocalizationStage localization_stage; + CollisionStage collision_stage; + TrafficLightStage traffic_light_stage; + MotionPlanStage motion_plan_stage; + ALSM alsm; + /// Traffic manager server instance. + TrafficManagerServer server; + /// Switch to turn on / turn off traffic manager. + std::atomic run_traffic_manger{true}; + /// Flags to signal step begin and end. + std::atomic step_begin{false}; + std::atomic step_end{false}; + /// Mutex for progressing synchronous execution. + std::mutex step_execution_mutex; + /// Condition variables for progressing synchronous execution. + std::condition_variable step_begin_trigger; + std::condition_variable step_end_trigger; + /// Single worker thread for sequential execution of sub-components. + std::unique_ptr worker_thread; - /// Pointer to local map cache. - std::shared_ptr local_map; + /// Method to check if all traffic lights are frozen in a group. + bool CheckAllFrozen(TLGroup tl_to_freeze); - /// Carla's client connection object. - carla::client::detail::EpisodeProxy episodeProxyTM; +public: + /// Private constructor for singleton lifecycle management. + TrafficManagerLocal(std::vector longitudinal_PID_parameters, + std::vector longitudinal_highway_PID_parameters, + std::vector lateral_PID_parameters, + std::vector lateral_highway_PID_parameters, + float perc_decrease_from_limit, + cc::detail::EpisodeProxy &episode_proxy, + uint16_t &RPCportTM); - /// Carla's debug helper object. - carla::client::DebugHelper debug_helper; + /// Destructor. + virtual ~TrafficManagerLocal(); - /// Pointers to messenger objects connecting stage pairs. - std::shared_ptr collision_planner_messenger; - std::shared_ptr localization_collision_messenger; - std::shared_ptr localization_traffic_light_messenger; - std::shared_ptr localization_planner_messenger; - std::shared_ptr planner_control_messenger; - std::shared_ptr traffic_light_planner_messenger; + /// Method to setup InMemoryMap. + void SetupLocalMap(); - /// Pointers to the stage objects of traffic manager. - std::unique_ptr collision_stage; - std::unique_ptr control_stage; - std::unique_ptr localization_stage; - std::unique_ptr planner_stage; - std::unique_ptr traffic_light_stage; + /// To start the TrafficManager. + void Start(); - /// Parameterization object. - Parameters parameters; + /// Initiates thread to run the TrafficManager sequentially. + void Run(); - /// Traffic manager server instance. - TrafficManagerServer server; + /// To stop the TrafficManager. + void Stop(); - /// Method to check if all traffic lights are frozen in a group. - bool CheckAllFrozen(TLGroup tl_to_freeze); + /// To release the traffic manager. + void Release(); - public: + /// To reset the traffic manager. + void Reset(); - /// Private constructor for singleton lifecycle management. - TrafficManagerLocal( - std::vector longitudinal_PID_parameters, - std::vector longitudinal_highway_PID_parameters, - std::vector lateral_PID_parameters, - std::vector lateral_highway_PID_parameters, - float perc_decrease_from_limit, - carla::client::detail::EpisodeProxy &episodeProxy, - uint16_t &RPCportTM); + /// This method registers a vehicle with the traffic manager. + void RegisterVehicles(const std::vector &actor_list); - /// Destructor. - virtual ~TrafficManagerLocal(); - - /// To start the TrafficManager. - void Start(); - - /// To stop the TrafficManager. - void Stop(); - - /// To release the traffic manager. - void Release(); - - /// To reset the traffic manager. - void Reset(); - - /// This method registers a vehicle with the traffic manager. - void RegisterVehicles(const std::vector &actor_list); - - /// This method unregisters a vehicle from traffic manager. + /// This method unregisters a vehicle from traffic manager. void UnregisterVehicles(const std::vector &actor_list); - /// Method to set a vehicle's % decrease in velocity with respect to the speed limit. - /// If less than 0, it's a % increase. - void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); + /// Method to set a vehicle's % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage); - /// Methos to set a global % decrease in velocity with respect to the speed limit. - /// If less than 0, it's a % increase. - void SetGlobalPercentageSpeedDifference(float const percentage); + /// Methos to set a global % decrease in velocity with respect to the speed limit. + /// If less than 0, it's a % increase. + void SetGlobalPercentageSpeedDifference(float const percentage); - /// Method to set collision detection rules between vehicles. - void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision); + /// Method to set collision detection rules between vehicles. + void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision); - /// Method to force lane change on a vehicle. - /// Direction flag can be set to true for left and false for right. - void SetForceLaneChange(const ActorPtr &actor, const bool direction); + /// Method to force lane change on a vehicle. + /// Direction flag can be set to true for left and false for right. + void SetForceLaneChange(const ActorPtr &actor, const bool direction); - /// Enable/disable automatic lane change on a vehicle. - void SetAutoLaneChange(const ActorPtr &actor, const bool enable); + /// Enable/disable automatic lane change on a vehicle. + void SetAutoLaneChange(const ActorPtr &actor, const bool enable); - /// Method to specify how much distance a vehicle should maintain to - /// the leading vehicle. - void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); + /// Method to specify how much distance a vehicle should maintain to + /// the leading vehicle. + void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance); - /// Method to specify the % chance of ignoring collisions with any walker. - void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc); + /// Method to specify the % chance of ignoring collisions with any walker. + void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc); - /// Method to specify the % chance of ignoring collisions with any vehicle. - void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc); + /// Method to specify the % chance of ignoring collisions with any vehicle. + void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc); - /// Method to specify the % chance of running any traffic light. - void SetPercentageRunningLight(const ActorPtr &actor, const float perc); + /// Method to specify the % chance of running any traffic light. + void SetPercentageRunningLight(const ActorPtr &actor, const float perc); - /// Method to specify the % chance of running any traffic sign. - void SetPercentageRunningSign(const ActorPtr &actor, const float perc); + /// Method to specify the % chance of running any traffic sign. + void SetPercentageRunningSign(const ActorPtr &actor, const float perc); - /// Method to switch traffic manager into synchronous execution. - void SetSynchronousMode(bool mode); + /// Method to switch traffic manager into synchronous execution. + void SetSynchronousMode(bool mode); - /// Method to set Tick timeout for synchronous execution. - void SetSynchronousModeTimeOutInMiliSecond(double time); + /// Method to set Tick timeout for synchronous execution. + void SetSynchronousModeTimeOutInMiliSecond(double time); - /// Method to provide synchronous tick. - bool SynchronousTick(); + /// Method to provide synchronous tick. + bool SynchronousTick(); - /// Method to reset all traffic light groups to the initial stage. - void ResetAllTrafficLights(); + /// Method to reset all traffic light groups to the initial stage. + void ResetAllTrafficLights(); - /// Get CARLA episode information. - carla::client::detail::EpisodeProxy& GetEpisodeProxy(); + /// Get CARLA episode information. + carla::client::detail::EpisodeProxy &GetEpisodeProxy(); - /// Get list of all registered vehicles. - std::vector GetRegisteredVehiclesIDs(); + /// Get list of all registered vehicles. + std::vector GetRegisteredVehiclesIDs(); - /// Method to specify how much distance a vehicle should maintain to - /// the Global leading vehicle. - void SetGlobalDistanceToLeadingVehicle(const float distance); + /// Method to specify how much distance a vehicle should maintain to + /// the Global leading vehicle. + void SetGlobalDistanceToLeadingVehicle(const float distance); - /// Method to set probabilistic preference to keep on the right lane. - void SetKeepRightPercentage(const ActorPtr &actor, const float percentage); + /// Method to set probabilistic preference to keep on the right lane. + void SetKeepRightPercentage(const ActorPtr &actor, const float percentage); - /// Method to set hybrid physics mode. - void SetHybridPhysicsMode(const bool mode_switch); + /// Method to set hybrid physics mode. + void SetHybridPhysicsMode(const bool mode_switch); - /// Method to set hybrid physics radius. - void SetHybridPhysicsRadius(const float radius); - - }; + /// Method to set hybrid physics radius. + void SetHybridPhysicsRadius(const float radius); +}; } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp index 10516c33b..ac3fad8a1 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp @@ -4,10 +4,7 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -#include -#include "carla/client/TrafficLight.h" -#include "carla/client/ActorList.h" -#include "carla/client/DebugHelper.h" +#include "carla/client/detail/Simulator.h" #include "carla/trafficmanager/TrafficManagerRemote.h" diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h index 86b889cf0..ac3023b91 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h @@ -7,12 +7,10 @@ #pragma once #include -#include #include #include #include "carla/client/Actor.h" -#include "carla/client/detail/Simulator.h" #include "carla/client/detail/EpisodeProxy.h" #include "carla/trafficmanager/TrafficManagerBase.h" #include "carla/trafficmanager/TrafficManagerClient.h" @@ -21,8 +19,6 @@ namespace carla { namespace traffic_manager { using ActorPtr = carla::SharedPtr; -using TLS = carla::rpc::TrafficLightState; -using TLGroup = std::vector>; /// The function of this class is to integrate all the various stages of /// the traffic manager appropriately using messengers. diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h b/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h index 1e0caf852..5f7ed1a00 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerServer.h @@ -12,6 +12,7 @@ #include "carla/client/Actor.h" #include "carla/client/detail/ActorVariant.h" #include "carla/rpc/Server.h" +#include "carla/trafficmanager/Constants.h" #include "carla/trafficmanager/TrafficManagerBase.h" namespace carla { @@ -19,6 +20,8 @@ namespace traffic_manager { using ActorPtr = carla::SharedPtr; +using namespace constants::Networking; + class TrafficManagerServer { public: diff --git a/PythonAPI/carla/source/libcarla/Actor.cpp b/PythonAPI/carla/source/libcarla/Actor.cpp index f9b40e18d..def07f8b2 100644 --- a/PythonAPI/carla/source/libcarla/Actor.cpp +++ b/PythonAPI/carla/source/libcarla/Actor.cpp @@ -17,6 +17,8 @@ #include #include +namespace ctm = carla::traffic_manager; + namespace carla { namespace client { @@ -117,7 +119,7 @@ void export_actor() { .def("get_light_state", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetLightState)) .def("apply_physics_control", &cc::Vehicle::ApplyPhysicsControl, (arg("physics_control"))) .def("get_physics_control", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetPhysicsControl)) - .def("set_autopilot", CALL_WITHOUT_GIL_2(cc::Vehicle, SetAutopilot, bool, uint16_t), (arg("enabled") = true, arg("tm_port") = TM_DEFAULT_PORT)) + .def("set_autopilot", CALL_WITHOUT_GIL_2(cc::Vehicle, SetAutopilot, bool, uint16_t), (arg("enabled") = true, arg("tm_port") = ctm::TM_DEFAULT_PORT)) .def("get_speed_limit", &cc::Vehicle::GetSpeedLimit) .def("get_traffic_light_state", &cc::Vehicle::GetTrafficLightState) .def("is_at_traffic_light", &cc::Vehicle::IsAtTrafficLight) diff --git a/PythonAPI/carla/source/libcarla/Client.cpp b/PythonAPI/carla/source/libcarla/Client.cpp index 421c121e8..4c24ee323 100644 --- a/PythonAPI/carla/source/libcarla/Client.cpp +++ b/PythonAPI/carla/source/libcarla/Client.cpp @@ -15,6 +15,8 @@ #include +namespace ctm = carla::traffic_manager; + static void SetTimeout(carla::client::Client &client, double seconds) { client.SetTimeout(TimeDurationFromSeconds(seconds)); } @@ -192,6 +194,6 @@ void export_client() { .def("set_replayer_ignore_hero", &cc::Client::SetReplayerIgnoreHero, (arg("ignore_hero"))) .def("apply_batch", &ApplyBatchCommands, (arg("commands"), arg("do_tick")=false)) .def("apply_batch_sync", &ApplyBatchCommandsSync, (arg("commands"), arg("do_tick")=false)) - .def("get_trafficmanager", CONST_CALL_WITHOUT_GIL_1(cc::Client, GetInstanceTM, uint16_t), (arg("port")=TM_DEFAULT_PORT)) + .def("get_trafficmanager", CONST_CALL_WITHOUT_GIL_1(cc::Client, GetInstanceTM, uint16_t), (arg("port")=ctm::TM_DEFAULT_PORT)) ; }