From 9ff76463eb9228c325ee5ac84377501dd24dd3c5 Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo Date: Mon, 13 Dec 2021 16:07:09 +0100 Subject: [PATCH] Unique seed for Traffic Manager --- LibCarla/source/carla/trafficmanager/ALSM.cpp | 7 ++---- LibCarla/source/carla/trafficmanager/ALSM.h | 5 +--- .../carla/trafficmanager/CollisionStage.cpp | 8 +++---- .../carla/trafficmanager/CollisionStage.h | 4 ++-- .../trafficmanager/LocalizationStage.cpp | 14 +++++------ .../carla/trafficmanager/LocalizationStage.h | 4 ++-- .../carla/trafficmanager/MotionPlanStage.cpp | 6 ++--- .../carla/trafficmanager/MotionPlanStage.h | 4 ++-- .../carla/trafficmanager/RandomGenerator.h | 2 -- .../trafficmanager/TrafficLightStage.cpp | 8 +++---- .../carla/trafficmanager/TrafficLightStage.h | 4 ++-- .../trafficmanager/TrafficManagerLocal.cpp | 23 +++++++------------ .../trafficmanager/TrafficManagerLocal.h | 4 ++-- 13 files changed, 39 insertions(+), 54 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/ALSM.cpp b/LibCarla/source/carla/trafficmanager/ALSM.cpp index 9c3873882..c9066f39a 100644 --- a/LibCarla/source/carla/trafficmanager/ALSM.cpp +++ b/LibCarla/source/carla/trafficmanager/ALSM.cpp @@ -27,8 +27,7 @@ ALSM::ALSM( CollisionStage &collision_stage, TrafficLightStage &traffic_light_stage, MotionPlanStage &motion_plan_stage, - VehicleLightStage &vehicle_light_stage, - RandomGeneratorMap &random_devices) + VehicleLightStage &vehicle_light_stage) : registered_vehicles(registered_vehicles), buffer_map(buffer_map), track_traffic(track_traffic), @@ -41,8 +40,7 @@ ALSM::ALSM( collision_stage(collision_stage), traffic_light_stage(traffic_light_stage), motion_plan_stage(motion_plan_stage), - vehicle_light_stage(vehicle_light_stage), - random_devices(random_devices) {} + vehicle_light_stage(vehicle_light_stage) {} void ALSM::Update() { @@ -375,7 +373,6 @@ void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) { registered_vehicles.Remove({actor_id}); buffer_map.erase(actor_id); idle_time.erase(actor_id); - random_devices.erase(actor_id); localization_stage.RemoveActor(actor_id); collision_stage.RemoveActor(actor_id); traffic_light_stage.RemoveActor(actor_id); diff --git a/LibCarla/source/carla/trafficmanager/ALSM.h b/LibCarla/source/carla/trafficmanager/ALSM.h index 42b99f1b3..9d6dae6d0 100644 --- a/LibCarla/source/carla/trafficmanager/ALSM.h +++ b/LibCarla/source/carla/trafficmanager/ALSM.h @@ -64,8 +64,6 @@ private: // Time elapsed since last vehicle destruction due to being idle for too long. double elapsed_last_actor_destruction {0.0}; cc::Timestamp current_timestamp; - // Random devices. - RandomGeneratorMap &random_devices; std::unordered_map has_physics_enabled; // Updates the duration for which a registered vehicle is stuck at a location. @@ -104,8 +102,7 @@ public: CollisionStage &collision_stage, TrafficLightStage &traffic_light_stage, MotionPlanStage &motion_plan_stage, - VehicleLightStage &vehicle_light_stage, - RandomGeneratorMap &random_devices); + VehicleLightStage &vehicle_light_stage); void Update(); diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 62a91a862..9d806fc4b 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -22,14 +22,14 @@ CollisionStage::CollisionStage( const TrackTraffic &track_traffic, const Parameters ¶meters, CollisionFrame &output_array, - RandomGeneratorMap &random_devices) + RandomGenerator &random_device) : vehicle_id_list(vehicle_id_list), simulation_state(simulation_state), buffer_map(buffer_map), track_traffic(track_traffic), parameters(parameters), output_array(output_array), - random_devices(random_devices) {} + random_device(random_device) {} void CollisionStage::Update(const unsigned long index) { ActorId obstacle_id = 0u; @@ -91,9 +91,9 @@ void CollisionStage::Update(const unsigned long index) { look_ahead_index); if (negotiation_result.first) { if ((other_actor_type == ActorType::Vehicle - && parameters.GetPercentageIgnoreVehicles(ego_actor_id) <= random_devices.at(ego_actor_id).next()) + && parameters.GetPercentageIgnoreVehicles(ego_actor_id) <= random_device.next()) || (other_actor_type == ActorType::Pedestrian - && parameters.GetPercentageIgnoreWalkers(ego_actor_id) <= random_devices.at(ego_actor_id).next())) { + && parameters.GetPercentageIgnoreWalkers(ego_actor_id) <= random_device.next())) { collision_hazard = true; obstacle_id = other_actor_id; available_distance_margin = negotiation_result.second; diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index e86804867..717d4e67b 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -57,7 +57,7 @@ private: // to avoid repeated computation within a cycle. GeometryComparisonMap geometry_cache; GeodesicBoundaryMap geodesic_boundary_map; - RandomGeneratorMap &random_devices; + RandomGenerator &random_device; // Method to determine if a vehicle is on a collision path to another. std::pair NegotiateCollision(const ActorId reference_vehicle_id, @@ -90,7 +90,7 @@ public: const TrackTraffic &track_traffic, const Parameters ¶meters, CollisionFrame &output_array, - RandomGeneratorMap &random_devices); + RandomGenerator &random_device); void Update (const unsigned long index) override; diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index 58d329dd5..516586cca 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -22,7 +22,7 @@ LocalizationStage::LocalizationStage( Parameters ¶meters, std::vector& marked_for_removal, LocalizationFrame &output_array, - RandomGeneratorMap &random_devices) + RandomGenerator &random_device) : vehicle_id_list(vehicle_id_list), buffer_map(buffer_map), simulation_state(simulation_state), @@ -31,7 +31,7 @@ LocalizationStage::LocalizationStage( parameters(parameters), marked_for_removal(marked_for_removal), output_array(output_array), - random_devices(random_devices){} + random_device(random_device){} void LocalizationStage::Update(const unsigned long index) { @@ -120,9 +120,9 @@ void LocalizationStage::Update(const unsigned long index) { const float perc_keep_right = parameters.GetKeepRightPercentage(actor_id); const float perc_random_leftlanechange = parameters.GetRandomLeftLaneChangePercentage(actor_id); const float perc_random_rightlanechange = parameters.GetRandomRightLaneChangePercentage(actor_id); - const bool is_keep_right = perc_keep_right > random_devices.at(actor_id).next(); - const bool is_random_left_change = perc_random_leftlanechange >= random_devices.at(actor_id).next(); - const bool is_random_right_change = perc_random_rightlanechange >= random_devices.at(actor_id).next(); + const bool is_keep_right = perc_keep_right > random_device.next(); + const bool is_random_left_change = perc_random_leftlanechange >= random_device.next(); + const bool is_random_right_change = perc_random_rightlanechange >= random_device.next(); // Determine which of the parameters we should apply. if (is_keep_right || is_random_right_change) { @@ -135,7 +135,7 @@ void LocalizationStage::Update(const unsigned long index) { lane_change_direction = false; } else { // Both a left and right lane changes are forced. Choose between one of them. - lane_change_direction = FIFTYPERC > random_devices.at(actor_id).next(); + lane_change_direction = FIFTYPERC > random_device.next(); } } } @@ -194,7 +194,7 @@ void LocalizationStage::Update(const unsigned long index) { uint64_t selection_index = 0u; // Pseudo-randomized path selection if found more than one choice. if (next_waypoints.size() > 1) { - double r_sample = random_devices.at(actor_id).next(); + double r_sample = random_device.next(); selection_index = static_cast(r_sample*next_waypoints.size()*0.01); } else if (next_waypoints.size() == 0) { if (!parameters.GetOSMMode()) { diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index 451e80ea9..4f3444a59 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -44,7 +44,7 @@ private: ActorIdSet vehicles_at_junction; using SimpleWaypointPair = std::pair; std::unordered_map vehicles_at_junction_entrance; - RandomGeneratorMap &random_devices; + RandomGenerator &random_device; SimpleWaypointPtr AssignLaneChange(const ActorId actor_id, const cg::Location vehicle_location, @@ -74,7 +74,7 @@ public: Parameters ¶meters, std::vector& marked_for_removal, LocalizationFrame &output_array, - RandomGeneratorMap &random_devices); + RandomGenerator &random_device); void Update(const unsigned long index) override; diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp index 768b8293d..386f5c7d4 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -41,7 +41,7 @@ MotionPlanStage::MotionPlanStage( const TLFrame &tl_frame, const cc::World &world, ControlFrame &output_array, - RandomGeneratorMap &random_devices, + RandomGenerator &random_device, const LocalMapPtr &local_map) : vehicle_id_list(vehicle_id_list), simulation_state(simulation_state), @@ -57,7 +57,7 @@ MotionPlanStage::MotionPlanStage( tl_frame(tl_frame), world(world), output_array(output_array), - random_devices(random_devices), + random_device(random_device), local_map(local_map) { // Adding structure to avoid retrieving traffic lights when checking for landmarks. @@ -111,7 +111,7 @@ void MotionPlanStage::Update(const unsigned long index) { double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds; if (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT) { - float random_sample = (static_cast(random_devices.at(actor_id).next())*dilate_factor) + lower_bound; + float random_sample = (static_cast(random_device.next())*dilate_factor) + lower_bound; NodeList teleport_waypoint_list = local_map->GetWaypointsInDelta(hero_location, ATTEMPTS_TO_TELEPORT, random_sample); if (!teleport_waypoint_list.empty()) { for (auto &teleport_waypoint : teleport_waypoint_list) { diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.h b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h index 4e9b4fa5a..3309ab079 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h @@ -42,7 +42,7 @@ private: std::unordered_map teleportation_instance; ControlFrame &output_array; cc::Timestamp current_timestamp; - RandomGeneratorMap &random_devices; + RandomGenerator &random_device; const LocalMapPtr &local_map; TLMap tl_map; @@ -83,7 +83,7 @@ public: const TLFrame &tl_frame, const cc::World &world, ControlFrame &output_array, - RandomGeneratorMap &random_devices, + RandomGenerator &random_device, const LocalMapPtr &local_map); void Update(const unsigned long index); diff --git a/LibCarla/source/carla/trafficmanager/RandomGenerator.h b/LibCarla/source/carla/trafficmanager/RandomGenerator.h index c3b23e3fb..e405c5ca6 100644 --- a/LibCarla/source/carla/trafficmanager/RandomGenerator.h +++ b/LibCarla/source/carla/trafficmanager/RandomGenerator.h @@ -18,7 +18,5 @@ private: std::uniform_real_distribution dist; }; -using RandomGeneratorMap = std::unordered_map; - } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index 796e59b0f..72e3b6df5 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -17,14 +17,14 @@ TrafficLightStage::TrafficLightStage( const Parameters ¶meters, const cc::World &world, TLFrame &output_array, - RandomGeneratorMap &random_devices) + RandomGenerator &random_device) : vehicle_id_list(vehicle_id_list), simulation_state(simulation_state), buffer_map(buffer_map), parameters(parameters), world(world), output_array(output_array), - random_devices(random_devices) {} + random_device(random_device) {} void TrafficLightStage::Update(const unsigned long index) { bool traffic_light_hazard = false; @@ -46,7 +46,7 @@ void TrafficLightStage::Update(const unsigned long index) { if (is_at_traffic_light && traffic_light_state != TLS::Green && traffic_light_state != TLS::Off && - parameters.GetPercentageRunningLight(ego_actor_id) <= random_devices.at(ego_actor_id).next()) { + parameters.GetPercentageRunningLight(ego_actor_id) <= random_device.next()) { traffic_light_hazard = true; } @@ -55,7 +55,7 @@ void TrafficLightStage::Update(const unsigned long index) { !is_at_traffic_light && traffic_light_state != TLS::Green && traffic_light_state != TLS::Off && - parameters.GetPercentageRunningSign(ego_actor_id) <= random_devices.at(ego_actor_id).next()) { + parameters.GetPercentageRunningSign(ego_actor_id) <= random_device.next()) { traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_timestamp); } diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h index f3c19311d..5a7a75d6d 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.h +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.h @@ -26,7 +26,7 @@ private: /// Map containing the previous junction visited by a vehicle. std::unordered_map vehicle_last_junction; TLFrame &output_array; - RandomGeneratorMap &random_devices; + RandomGenerator &random_device; cc::Timestamp current_timestamp; bool HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id, @@ -39,7 +39,7 @@ public: const Parameters ¶meters, const cc::World &world, TLFrame &output_array, - RandomGeneratorMap &random_devices); + RandomGenerator &random_device); void Update(const unsigned long index) override; diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp index 4d4739311..22c173a52 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp @@ -42,7 +42,7 @@ TrafficManagerLocal::TrafficManagerLocal( parameters, marked_for_removal, localization_frame, - random_devices)), + random_device)), collision_stage(CollisionStage(vehicle_id_list, simulation_state, @@ -50,7 +50,7 @@ TrafficManagerLocal::TrafficManagerLocal( track_traffic, parameters, collision_frame, - random_devices)), + random_device)), traffic_light_stage(TrafficLightStage(vehicle_id_list, simulation_state, @@ -58,7 +58,7 @@ TrafficManagerLocal::TrafficManagerLocal( parameters, world, tl_frame, - random_devices)), + random_device)), motion_plan_stage(MotionPlanStage(vehicle_id_list, simulation_state, @@ -74,7 +74,7 @@ TrafficManagerLocal::TrafficManagerLocal( tl_frame, world, control_frame, - random_devices, + random_device, local_map)), vehicle_light_stage(VehicleLightStage(vehicle_id_list, @@ -95,8 +95,7 @@ TrafficManagerLocal::TrafficManagerLocal( collision_stage, traffic_light_stage, motion_plan_stage, - vehicle_light_stage, - random_devices)), + vehicle_light_stage)), server(TrafficManagerServer(RPCportTM, static_cast(this))) { @@ -190,6 +189,7 @@ void TrafficManagerLocal::Run() { 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(); + std::sort(vehicle_id_list.begin(), vehicle_id_list.end()); number_of_vehicles = vehicle_id_list.size(); // Reserve more space if needed. @@ -282,7 +282,6 @@ void TrafficManagerLocal::Stop() { track_traffic.Clear(); previous_update_instance = chr::system_clock::now(); current_reserved_capacity = 0u; - random_devices.clear(); simulation_state.Reset(); localization_stage.Reset(); @@ -322,14 +321,6 @@ void TrafficManagerLocal::RegisterVehicles(const std::vector &vehicle_ std::vector sorted_vehicle_list = vehicle_list; std::sort(sorted_vehicle_list.begin(), sorted_vehicle_list.end(), [](ActorPtr &a, ActorPtr &b) {return a->GetId() > b->GetId(); }); registered_vehicles.Insert(sorted_vehicle_list); - for (const ActorPtr &vehicle: sorted_vehicle_list) { - if (!is_custom_seed) { - seed = vehicle->GetId() + seed; - } else { - seed = 1 + seed; - } - random_devices.insert({vehicle->GetId(), RandomGenerator(seed)}); - } } void TrafficManagerLocal::UnregisterVehicles(const std::vector &actor_list) { @@ -490,6 +481,8 @@ std::vector TrafficManagerLocal::GetRegisteredVehiclesIDs() { void TrafficManagerLocal::SetRandomDeviceSeed(const uint64_t _seed) { seed = _seed; is_custom_seed = true; + random_device = RandomGenerator(seed); + std::cout << random_device.next() << std::endl; world.ResetAllTrafficLights(); } diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h index 61f801a3e..190ad5762 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h @@ -109,11 +109,11 @@ private: std::condition_variable step_end_trigger; /// Single worker thread for sequential execution of sub-components. std::unique_ptr worker_thread; - /// Structure holding random devices per vehicle. - RandomGeneratorMap random_devices; /// Randomization seed. uint64_t seed {static_cast(time(NULL))}; bool is_custom_seed {false}; + /// Structure holding random devices per vehicle. + RandomGenerator random_device = RandomGenerator(seed); std::vector marked_for_removal; /// Mutex to prevent vehicle registration during frame array re-allocation. std::mutex registration_mutex;