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,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`

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<ActorId> 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);

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<float>::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<float> LONGITUDIAL_PARAM = {12.0f, 0.05f, 0.02f};
 static const std::vector<float> LONGITUDIAL_HIGHWAY_PARAM = {20.0f, 0.05f, 0.01f};
-static const std::vector<float> LATERAL_PARAM = {5.0f, 0.02f, 0.8f};
-static const std::vector<float> LATERAL_HIGHWAY_PARAM = {3.0f, 0.02f, 0.4f};
+static const std::vector<float> LATERAL_PARAM = {4.0f, 0.02f, 0.08f};
+static const std::vector<float> LATERAL_HIGHWAY_PARAM = {2.0f, 0.02f, 0.04f};
 } // namespace PID
 
 namespace TrackTraffic {

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<int16_t>(angle/MAX_WPT_RADIANS);
           int16_t distance_splits = static_cast<int16_t>((distance*distance)/MAX_WPT_DISTANCE);
           auto max_splits = max(angle_splits, distance_splits);

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<ActorId> &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()) {

LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp

@@ -307,6 +307,7 @@ std::pair<bool, float> 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<bool, float> 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<bool, float> 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};

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<GeoGridId> current_grids;
         uint64_t buffer_size = buffer.size();
-        uint64_t step_size = static_cast<uint64_t>(static_cast<float>(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.

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()) {
 

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h

@@ -55,9 +55,9 @@ private:
   std::vector<float> longitudinal_highway_PID_parameters;
   std::vector<float> lateral_PID_parameters;
   std::vector<float> 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;