Improved large vehicles at junctions (#9384)

* Added first wide turn implementation

* Smoothed offset

* Removed debug elements

* Added CHANGELOG

* Added maximum radius

---------

Co-authored-by: glopezdiest <[email protected]>

Author: glopezdiest

CHANGELOG.md

@@ -1,6 +1,8 @@
 ## Latest Changes
 
+ * Improved the way the TrafficManager controls large vehicles at junctions, reducing the frequency of collisions with other elements in the simulation.
  * Added a hybrid solid-state LiDAR with adjustable parameters (blueprint attributes)
+
 ## CARLA 0.9.16
 
  * Added NVIDIA Cosmos Transfer1 integration

LibCarla/source/carla/trafficmanager/ALSM.cpp

@@ -23,6 +23,7 @@ ALSM::ALSM(
   const cc::World &world,
   const LocalMapPtr &local_map,
   SimulationState &simulation_state,
+  std::unordered_map<ActorId, std::pair<float, bool>> &large_vehicles,
   LocalizationStage &localization_stage,
   CollisionStage &collision_stage,
   TrafficLightStage &traffic_light_stage,
@@ -36,6 +37,7 @@ ALSM::ALSM(
     world(world),
     local_map(local_map),
     simulation_state(simulation_state),
+    large_vehicles(large_vehicles),
     localization_stage(localization_stage),
     collision_stage(collision_stage),
     traffic_light_stage(traffic_light_stage),
@@ -364,6 +366,20 @@ bool ALSM::IsVehicleStuck(const ActorId& actor_id) {
   return false;
 }
 
+void ALSM::AddActor(const Actor actor){
+  ActorId actor_id = actor->GetId();
+  for (auto&& attribute: actor->GetAttributes()) {
+    if (attribute.GetId() == "base_type"){
+      std::string value = attribute.GetValue();
+      std::transform(value.begin(), value.end(), value.begin(),[](unsigned char c){ return std::tolower(c);});
+      if (std::find(large_vehicle_types.begin(), large_vehicle_types.end(), attribute.GetValue()) != large_vehicle_types.end()) {
+        large_vehicles[actor_id] = std::make_pair(0.0f, 0.0f);
+      } else {
+      }
+    }
+  }
+}
+
 void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
   if (registered_actor) {
     registered_vehicles.Remove({actor_id});
@@ -374,6 +390,10 @@ void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
     traffic_light_stage.RemoveActor(actor_id);
     motion_plan_stage.RemoveActor(actor_id);
     vehicle_light_stage.RemoveActor(actor_id);
+
+    if (large_vehicles.find(actor_id) != large_vehicles.end()){
+      large_vehicles.erase(actor_id);
+    }
   }
   else {
     unregistered_actors.erase(actor_id);

LibCarla/source/carla/trafficmanager/ALSM.h

@@ -65,6 +65,9 @@ class ALSM {
   double elapsed_last_actor_destruction {0.0};
   cc::Timestamp current_timestamp;
   std::unordered_map<ActorId, bool> has_physics_enabled;
+  std::unordered_map<ActorId, std::pair<float, bool>> &large_vehicles;
+  // Base types of the vehicles considered as large ones
+  std::vector<std::string> large_vehicle_types = {"bus", "truck"};
 
   // Updates the duration for which a registered vehicle is stuck at a location.
   void UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id);
@@ -97,6 +100,7 @@ class ALSM {
        const cc::World &world,
        const LocalMapPtr &local_map,
        SimulationState &simulation_state,
+       std::unordered_map<ActorId, std::pair<float, bool>> &large_vehicles,
        LocalizationStage &localization_stage,
        CollisionStage &collision_stage,
        TrafficLightStage &traffic_light_stage,
@@ -105,6 +109,9 @@ class ALSM {
 
   void Update();
 
+  // Initialize an actor to the traffic manager
+  void AddActor(const Actor actor);
+
   // 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);

LibCarla/source/carla/trafficmanager/Constants.h

@@ -57,6 +57,9 @@ static const float JUNCTION_LOOK_AHEAD = 5.0f;
 static const float SAFE_DISTANCE_AFTER_JUNCTION = 4.0f;
 static const float MIN_JUNCTION_LENGTH = 8.0f;
 static const float MIN_SAFE_INTERVAL_LENGTH = 0.5f * SAFE_DISTANCE_AFTER_JUNCTION;
+static const float LARGE_VEHICLES_JUNCTION_OFFSET = 1.5f;
+static const float LARGE_VEHICLES_JUNCTION_POINT = 0.3f;
+static const float LARGE_VEHICLES_JUNCTION_MAX_RADIUS = 20.0f;
 } // namespace WaypointSelection
 
 namespace LaneChange {

LibCarla/source/carla/trafficmanager/LocalizationStage.cpp

@@ -19,6 +19,7 @@ LocalizationStage::LocalizationStage(
   const SimulationState &simulation_state,
   TrackTraffic &track_traffic,
   const LocalMapPtr &local_map,
+  std::unordered_map<ActorId, std::pair<float, bool>> &large_vehicles,
   Parameters &parameters,
   std::vector<ActorId>& marked_for_removal,
   LocalizationFrame &output_array,
@@ -28,6 +29,7 @@ LocalizationStage::LocalizationStage(
     simulation_state(simulation_state),
     track_traffic(track_traffic),
     local_map(local_map),
+    large_vehicles(large_vehicles),
     parameters(parameters),
     marked_for_removal(marked_for_removal),
     output_array(output_array),
@@ -215,6 +217,7 @@ void LocalizationStage::Update(const unsigned long index) {
     }
   }
   ExtendAndFindSafeSpace(actor_id, is_at_junction_entrance, waypoint_buffer);
+  HandleLargeVehicleJunction(actor_id, is_at_junction_entrance, waypoint_buffer);
 
   // Editing output array
   LocalizationData &output = output_array.at(index);
@@ -239,7 +242,7 @@ void LocalizationStage::ExtendAndFindSafeSpace(const ActorId actor_id,
 
   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()) {
 
@@ -321,6 +324,134 @@ void LocalizationStage::ExtendAndFindSafeSpace(const ActorId actor_id,
   }
 }
 
+void LocalizationStage::HandleLargeVehicleJunction(const ActorId actor_id,
+                                                   const bool is_at_junction_entrance,
+                                                   Buffer &waypoint_buffer) {
+
+  if (large_vehicles.find(actor_id) == large_vehicles.end()){
+    return;
+  }
+
+  SimpleWaypointPtr current_waypoint = nullptr;
+  bool is_at_junction = waypoint_buffer.front()->CheckJunction();
+
+  if (is_at_junction_entrance
+      && large_vehicles_at_junction_entrance.find(actor_id) == large_vehicles_at_junction_entrance.end()
+      && large_vehicles_at_junction.find(actor_id) == large_vehicles_at_junction.end()) {
+
+    large_vehicles_at_junction_entrance.insert(actor_id);
+
+    const SimpleWaypointPtr first_waypoint = waypoint_buffer.front();
+    const SimpleWaypointPtr last_waypoint = waypoint_buffer.back();
+    const SimpleWaypointPtr middle_waypoint = waypoint_buffer.at(static_cast<uint16_t>(waypoint_buffer.size() / 2));
+  
+    float radius = GetThreePointCircleRadius(first_waypoint->GetLocation(),
+                                             middle_waypoint->GetLocation(),
+                                             last_waypoint->GetLocation());
+
+    std::cout << "Radius: " << radius << std::endl;
+    if (radius > LARGE_VEHICLES_JUNCTION_MAX_RADIUS){
+      return; // Straight path
+    }
+
+    bool entered_junction = false;
+    float junction_length = 0.0f;
+    bool is_straight_path = true;
+
+    for (unsigned long i = 0u; i < waypoint_buffer.size(); ++i) {
+      current_waypoint = waypoint_buffer.at(i);
+
+      if (!entered_junction && current_waypoint->CheckJunction()) {
+        entered_junction = true;
+      }
+
+      if (i > 0 && entered_junction) {
+        SimpleWaypointPtr prev_waypoint = waypoint_buffer.at(i-1);
+        float new_distance = current_waypoint->Distance(prev_waypoint->GetLocation());
+        junction_length = junction_length + new_distance;
+
+        if (is_straight_path){
+          RoadOption junction_type = current_waypoint->GetRoadOption();
+          if (junction_type == RoadOption::Right){
+            large_vehicles[actor_id].second = true;
+            is_straight_path = false;
+          } else if (junction_type == RoadOption::Left) {
+            large_vehicles[actor_id].second = false;
+            is_straight_path = false;
+          } else {
+          }
+        }
+      }
+
+      if (entered_junction && !current_waypoint->CheckJunction()){
+        break;
+      }
+    }
+    if (!is_straight_path){
+      large_vehicles[actor_id].first = junction_length;
+    }
+
+  }
+  else if (is_at_junction
+      && large_vehicles_at_junction_entrance.find(actor_id) != large_vehicles_at_junction_entrance.end()) {
+
+    large_vehicles_at_junction_entrance.erase(actor_id);
+    large_vehicles_at_junction.insert(actor_id);
+  }
+  else if (!is_at_junction
+      && large_vehicles_at_junction.find(actor_id) != large_vehicles_at_junction.end()) {
+
+    large_vehicles_at_junction.erase(actor_id);
+    if (large_vehicles.find(actor_id) != large_vehicles.end()){
+      large_vehicles[actor_id].first = 0.0f;
+    }
+  }
+}
+
+float LocalizationStage::GetThreePointCircleRadius(cg::Location first_location,
+                                                   cg::Location middle_location,
+                                                   cg::Location last_location) {
+
+    float x1 = first_location.x;
+    float y1 = first_location.y;
+    float x2 = middle_location.x;
+    float y2 = middle_location.y;
+    float x3 = last_location.x;
+    float y3 = last_location.y;
+
+    float x12 = x1 - x2;
+    float x13 = x1 - x3;
+    float y12 = y1 - y2;
+    float y13 = y1 - y3;
+    float y31 = y3 - y1;
+    float y21 = y2 - y1;
+    float x31 = x3 - x1;
+    float x21 = x2 - x1;
+
+    float sx13 = x1 * x1 - x3 * x3;
+    float sy13 = y1 * y1 - y3 * y3;
+    float sx21 = x2 * x2 - x1 * x1;
+    float sy21 = y2 * y2 - y1 * y1;
+
+    float f_denom = 2 * (y31 * x12 - y21 * x13);
+    if (f_denom == 0) {
+      return std::numeric_limits<float>::max();
+    }
+    float f = (sx13 * x12 + sy13 * x12 + sx21 * x13 + sy21 * x13) / f_denom;
+
+    float g_denom = 2 * (x31 * y12 - x21 * y13);
+    if (g_denom == 0) {
+      return std::numeric_limits<float>::max();
+    }
+    float g = (sx13 * y12 + sy13 * y12 + sx21 * y13 + sy21 * y13) / g_denom;
+
+    float c = - (x1 * x1 + y1 * y1) - 2 * g * x1 - 2 * f * y1;
+    float h = -g;
+    float k = -f;
+
+  return std::sqrt(h * h + k * k - c);
+}
+
 void LocalizationStage::RemoveActor(ActorId actor_id) {
     last_lane_change_swpt.erase(actor_id);
     vehicles_at_junction.erase(actor_id);

LibCarla/source/carla/trafficmanager/LocalizationStage.h

@@ -44,7 +44,10 @@ class LocalizationStage : Stage {
   ActorIdSet vehicles_at_junction;
   using SimpleWaypointPair = std::pair<SimpleWaypointPtr, SimpleWaypointPtr>;
   std::unordered_map<ActorId, SimpleWaypointPair> vehicles_at_junction_entrance;
+  std::unordered_set<ActorId> large_vehicles_at_junction_entrance;
+  std::unordered_set<ActorId> large_vehicles_at_junction;
   RandomGenerator &random_device;
+  std::unordered_map<ActorId, std::pair<float, bool>> &large_vehicles;
 
   SimpleWaypointPtr AssignLaneChange(const ActorId actor_id,
                                      const cg::Location vehicle_location,
@@ -55,6 +58,14 @@ class LocalizationStage : Stage {
                               const bool is_at_junction_entrance,
                               Buffer &waypoint_buffer);
 
+  void HandleLargeVehicleJunction(const ActorId actor_id,
+                                  const bool is_at_junction_entrance,
+                                  Buffer &waypoint_buffer);
+
+  float GetThreePointCircleRadius(cg::Location first_location,
+                                  cg::Location middle_location,
+                                  cg::Location last_location);
+
   void ImportPath(Path &imported_path,
                   Buffer &waypoint_buffer,
                   const ActorId actor_id,
@@ -71,6 +82,7 @@ class LocalizationStage : Stage {
                     const SimulationState &simulation_state,
                     TrackTraffic &track_traffic,
                     const LocalMapPtr &local_map,
+                    std::unordered_map<ActorId, std::pair<float, bool>> &large_vehicles,
                     Parameters &parameters,
                     std::vector<ActorId>& marked_for_removal,
                     LocalizationFrame &output_array,