From 3858c5f04cb7bffbdc6aa71fd341b01f04fe5022 Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com> Date: Tue, 5 Oct 2021 10:57:47 +0200 Subject: [PATCH] Improvements in Traffic Manager for 0.9.12 (#4686) * Changes in constant values Added new constants * Added Ambulance and Firetruck as unsafe vehicles in spawn_npc.py * WIP: Rerouting algorithm * Removing rerouting algorithm. Fixed collisions at high speed. * Changes to constant values and cleaning up. * Cleanup * Changelog * Added a factor of vehicle length when checking for collision at low speed. * Change in horizon rate --- CHANGELOG.md | 1 + .../carla/trafficmanager/CollisionStage.cpp | 19 +++++++----- .../source/carla/trafficmanager/Constants.h | 29 ++++++++++--------- .../carla/trafficmanager/InMemoryMap.cpp | 2 +- .../trafficmanager/LocalizationStage.cpp | 6 +++- .../carla/trafficmanager/MotionPlanStage.cpp | 14 ++++++--- .../carla/trafficmanager/TrackTraffic.cpp | 5 ++-- .../trafficmanager/TrafficLightStage.cpp | 6 ++-- .../trafficmanager/TrafficManagerLocal.h | 4 +-- 9 files changed, 51 insertions(+), 35 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 185c2bb4e..7110e145b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,5 @@ ## Latest + * Improved handling of collisions in Traffic Manager when driving at very high speeds. * Added open/close doors feature for vehicles. * Added API functions to 3D vectors: `squared_length`, `length`, `make_unit_vector`, `dot`, `dot_2d`, `distance`, `distance_2d`, `distance_squared`, `distance_squared_2d`, `get_vector_angle` * Added API functions to 2D vectors: `squared_length`, `length`, `make_unit_vector` diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index b9c6dc153..3a9241860 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -45,12 +45,16 @@ void CollisionStage::Update(const unsigned long index) { ActorIdSet overlapping_actors = track_traffic.GetOverlappingVehicles(ego_actor_id); std::vector collision_candidate_ids; - // Run through vehicles with overlapping paths and filter them; - float value = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN) + parameters.GetDistanceToLeadingVehicle(ego_actor_id); - float collision_radius_square = value; - if (velocity < 1.0f) { - collision_radius_square = SQUARE(COLLISION_RADIUS_STOP) + parameters.GetDistanceToLeadingVehicle(ego_actor_id); + const float distance_to_leading = parameters.GetDistanceToLeadingVehicle(ego_actor_id); + float collision_radius_square = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN) + distance_to_leading;; + if (velocity < 2.0f) { + const float length = simulation_state.GetDimensions(ego_actor_id).x; + const float collision_radius_stop = COLLISION_RADIUS_STOP + length; + collision_radius_square = SQUARE(collision_radius_stop); + if (distance_to_leading > collision_radius_stop) { + collision_radius_square = SQUARE(collision_radius_stop) + distance_to_leading; + } } for (ActorId overlapping_actor_id : overlapping_actors) { // If actor is within maximum collision avoidance and vertical overlap range. @@ -116,8 +120,9 @@ float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id) { 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; + // Using a function to calculate boundary length. + float velocity_extension = VEL_EXT_FACTOR*velocity; + bbox_extension = BOUNDARY_EXTENSION_MINIMUM + velocity_extension * velocity_extension; // 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); diff --git a/LibCarla/source/carla/trafficmanager/Constants.h b/LibCarla/source/carla/trafficmanager/Constants.h index 51797bd73..683bd896d 100644 --- a/LibCarla/source/carla/trafficmanager/Constants.h +++ b/LibCarla/source/carla/trafficmanager/Constants.h @@ -38,15 +38,16 @@ static const float PHYSICS_RADIUS = 50.0f; } // namespace HybridMode namespace SpeedThreshold { -static const float HIGHWAY_SPEED = 50.0f / 3.6f; +static const float HIGHWAY_SPEED = 60.0f / 3.6f; static const float AFTER_JUNCTION_MIN_SPEED = 5.0f / 3.6f; -static const float INITIAL_PERCENTAGE_SPEED_DIFFERENCE = 30.0f; +static const float INITIAL_PERCENTAGE_SPEED_DIFFERENCE = 0.0f; } // namespace SpeedThreshold namespace PathBufferUpdate { static const float MAX_START_DISTANCE = 20.0f; -static const float MINIMUM_HORIZON_LENGTH = 20.0f; -static const float HORIZON_RATE = 1.45f; +static const float MINIMUM_HORIZON_LENGTH = 15.0f; +static const float HORIZON_RATE = 2.0f; +static const float HIGH_SPEED_HORIZON_RATE = 4.0f; } // namespace PathBufferUpdate namespace WaypointSelection { @@ -59,7 +60,7 @@ static const float MIN_SAFE_INTERVAL_LENGTH = 0.5f * SAFE_DISTANCE_AFTER_JUNCTIO } // namespace WaypointSelection namespace LaneChange { -static const float MINIMUM_LANE_CHANGE_DISTANCE = 15.0f; +static const float MINIMUM_LANE_CHANGE_DISTANCE = 20.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; @@ -67,19 +68,21 @@ static const float INTER_LANE_CHANGE_DISTANCE = 10.0f; namespace Collision { static const float BOUNDARY_EXTENSION_MINIMUM = 2.5f; -static const float BOUNDARY_EXTENSION_RATE = 1.35f; +static const float BOUNDARY_EXTENSION_RATE = 4.35f; static const float COS_10_DEGREES = 0.9848f; static const float OVERLAP_THRESHOLD = 0.1f; static const float LOCKING_DISTANCE_PADDING = 4.0f; static const float COLLISION_RADIUS_STOP = 8.0f; static const float COLLISION_RADIUS_MIN = 20.0f; -static const float COLLISION_RADIUS_RATE = 0.65f; +static const float COLLISION_RADIUS_RATE = 2.65f; 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; static const float EPSILON = 2.0f * std::numeric_limits::epsilon(); static const float MIN_REFERENCE_DISTANCE = 1.0f; +static const float MIN_VELOCITY_COLL_RADIUS = 2.0f; +static const float VEL_EXT_FACTOR = 0.36f; } // namespace Collision namespace FrameMemory { @@ -105,11 +108,9 @@ 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 RELATIVE_APPROACH_SPEED = 12.0f / 3.6f; static const float MIN_FOLLOW_LEAD_DISTANCE = 2.0f; -static const float MAX_FOLLOW_LEAD_DISTANCE = 5.0f; -static const float FOLLOW_DISTANCE_RATE = 0.1f; -static const float CRITICAL_BRAKING_MARGIN = 0.25f; +static const float CRITICAL_BRAKING_MARGIN = 0.2f; static const float EPSILON_RELATIVE_SPEED = 0.001f; static const float MAX_JUNCTION_BLOCK_DISTANCE = 1.0f * WaypointSelection::SAFE_DISTANCE_AFTER_JUNCTION; static const float TWO_KM = 2000.0f; @@ -123,6 +124,8 @@ static const float YIELD_TARGET_VELOCITY = 15.0f / 3.6f; static const float FRICTION = 0.6f; static const float GRAVITY = 9.81f; static const float PI = 3.1415927f; +static const float PERC_MAX_SLOWDOWN = 0.08f; +static const float FOLLOW_LEAD_FACTOR = 2.0f; } // namespace MotionPlan namespace PID { @@ -134,8 +137,8 @@ static const float DT = 0.05f; static const float INV_DT = 1.0f / DT; static const std::vector LONGITUDIAL_PARAM = {12.0f, 0.05f, 0.02f}; static const std::vector LONGITUDIAL_HIGHWAY_PARAM = {20.0f, 0.05f, 0.01f}; -static const std::vector LATERAL_PARAM = {5.0f, 0.02f, 0.8f}; -static const std::vector LATERAL_HIGHWAY_PARAM = {3.0f, 0.02f, 0.4f}; +static const std::vector LATERAL_PARAM = {4.0f, 0.02f, 0.08f}; +static const std::vector LATERAL_HIGHWAY_PARAM = {2.0f, 0.02f, 0.04f}; } // namespace PID namespace TrackTraffic { diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index 5453b2659..3cc226060 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -261,7 +261,7 @@ namespace traffic_manager { // Adding more waypoints if the angle is too tight or if they are too distant. for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) { double distance = std::abs(segment_waypoints.at(i)->GetWaypoint()->GetDistance() - segment_waypoints.at(i+1)->GetWaypoint()->GetDistance()); - double angle = wpt_angle(segment_waypoints.at(i)->GetTransform().rotation.GetForwardVector(), segment_waypoints.at(i+1)->GetTransform().rotation.GetForwardVector()); + double angle = wpt_angle(segment_waypoints.at(i)->GetTransform().GetForwardVector(), segment_waypoints.at(i+1)->GetTransform().GetForwardVector()); int16_t angle_splits = static_cast(angle/MAX_WPT_RADIANS); int16_t distance_splits = static_cast((distance*distance)/MAX_WPT_DISTANCE); auto max_splits = max(angle_splits, distance_splits); diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index f7f436c5a..b7ad0612a 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -9,6 +9,7 @@ namespace traffic_manager { using namespace constants::PathBufferUpdate; using namespace constants::LaneChange; using namespace constants::WaypointSelection; +using namespace constants::SpeedThreshold; LocalizationStage::LocalizationStage( const std::vector &vehicle_id_list, @@ -39,7 +40,10 @@ void LocalizationStage::Update(const unsigned long index) { const float vehicle_speed = vehicle_velocity_vector.Length(); // Speed dependent waypoint horizon length. - float horizon_length = vehicle_speed * HORIZON_RATE + MINIMUM_HORIZON_LENGTH; + float horizon_length = std::max(vehicle_speed * HORIZON_RATE, MINIMUM_HORIZON_LENGTH); + if (vehicle_speed > HIGHWAY_SPEED) { + horizon_length = std::max(vehicle_speed * HIGH_SPEED_HORIZON_RATE, MINIMUM_HORIZON_LENGTH); + } const float horizon_square = SQUARE(horizon_length); if (buffer_map.find(actor_id) == buffer_map.end()) { diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp index 6fbc2ed1a..3fd0013dd 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -307,6 +307,7 @@ std::pair MotionPlanStage::CollisionHandling(const CollisionHazardD const float max_target_velocity) { bool collision_emergency_stop = false; float dynamic_target_velocity = max_target_velocity; + const float vehicle_speed = vehicle_velocity.Length(); if (collision_hazard.hazard && !tl_hazard) { const ActorId other_actor_id = collision_hazard.hazard_actor_id; @@ -321,11 +322,11 @@ std::pair MotionPlanStage::CollisionHandling(const CollisionHazardD if (vehicle_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 = std::min(vehicle_relative_speed * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE, MAX_FOLLOW_LEAD_DISTANCE); - if (available_distance_margin > follow_lead_distance && other_velocity.Length() < 1.0f) { + float follow_lead_distance = FOLLOW_LEAD_FACTOR * vehicle_speed + 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; + // by maintaining a relative speed of other_speed_along_heading + dynamic_target_velocity = other_speed_along_heading; } // If vehicle is approaching a lead vehicle and the lead vehicle is further // than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE. @@ -342,6 +343,11 @@ std::pair MotionPlanStage::CollisionHandling(const CollisionHazardD } } + float max_gradual_velocity = PERC_MAX_SLOWDOWN * vehicle_speed; + if (dynamic_target_velocity < vehicle_speed - max_gradual_velocity) { + // Don't slow more than PERC_MAX_SLOWDOWN per frame. + dynamic_target_velocity = vehicle_speed - max_gradual_velocity; + } dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity); return {collision_emergency_stop, dynamic_target_velocity}; diff --git a/LibCarla/source/carla/trafficmanager/TrackTraffic.cpp b/LibCarla/source/carla/trafficmanager/TrackTraffic.cpp index 8b44b04bf..893c085c4 100644 --- a/LibCarla/source/carla/trafficmanager/TrackTraffic.cpp +++ b/LibCarla/source/carla/trafficmanager/TrackTraffic.cpp @@ -56,9 +56,8 @@ void TrackTraffic::UpdateGridPosition(const ActorId actor_id, const Buffer &buff // 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) { - auto waypoint = buffer.at(std::min(i * step_size, buffer_size - 1u)); + for (uint64_t i = 0u; i <= buffer_size - 1u; ++i) { + auto waypoint = buffer.at(i); GeoGridId ggid = waypoint->GetGeodesicGridId(); current_grids.insert(ggid); // Add grid entry if not present. diff --git a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp index f04b2e62c..796e59b0f 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp @@ -69,8 +69,7 @@ bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, bool traffic_light_hazard = false; 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. + // Initializing new map entry for the actor with an arbitrary and different junction id. vehicle_last_junction.insert({ego_actor_id, junction_id + 1}); } @@ -92,8 +91,7 @@ bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, } // If new ticket is needed for the vehicle, then query the junction - // ticket map - // and update the map value to the new ticket value. + // 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()) { diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h index 9ee83ac46..e793e21d9 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h @@ -55,9 +55,9 @@ private: std::vector longitudinal_highway_PID_parameters; std::vector lateral_PID_parameters; std::vector lateral_highway_PID_parameters; - /// Carla's client connection object. + /// CARLA client connection object. carla::client::detail::EpisodeProxy episode_proxy; - /// Carla client and object. + /// CARLA client and object. cc::World world; /// Set of all actors registered with traffic manager. AtomicActorSet registered_vehicles;