Implemented TrafficManager::SetDistanceToLeadingVehicle

LibCarla/source/carla/trafficmanager/CollisionStage.cpp

@@ -6,10 +6,10 @@ namespace CollisionStageConstants {
   static const float SEARCH_RADIUS = 20.0f;
   static const float VERTICAL_OVERLAP_THRESHOLD = 2.0f;
   static const float ZERO_AREA = 0.0001f;
-  static const float BOUNDARY_EXTENSION_MINIMUM = 1.5f;
+  static const float BOUNDARY_EXTENSION_MINIMUM = 0.5f;
   static const float EXTENSION_SQUARE_POINT = 7.0f;
   static const float TIME_HORIZON = 0.5f;
-  static const float HIGHWAY_SPEED = 50 / 3.6f;
+  static const float HIGHWAY_SPEED = 50.0f / 3.6f;
   static const float HIGHWAY_TIME_HORIZON = 5.0f;
 }
   using namespace  CollisionStageConstants;
@@ -19,11 +19,13 @@ namespace CollisionStageConstants {
       std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
       cc::World &world,
       AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> &selective_collision,
+      AtomicMap<ActorId, float> &distance_to_leading_vehicle,
       cc::DebugHelper &debug_helper)
     : localization_messenger(localization_messenger),
       planner_messenger(planner_messenger),
       world(world),
       selective_collision(selective_collision),
+      distance_to_leading_vehicle(distance_to_leading_vehicle),
       debug_helper(debug_helper) {
 
     // Initializing clock for checking unregistered actors periodically.
@@ -248,27 +250,41 @@ namespace CollisionStageConstants {
     if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) {
 
       float velocity = actor->GetVelocity().Length();
+      cg::Location vehicle_location = actor->GetLocation();
+
       float bbox_extension =
           (std::max(std::sqrt(EXTENSION_SQUARE_POINT * velocity), BOUNDARY_EXTENSION_MINIMUM) +
           std::max(velocity * TIME_HORIZON, BOUNDARY_EXTENSION_MINIMUM) +
           BOUNDARY_EXTENSION_MINIMUM);
 
       bbox_extension = (velocity > HIGHWAY_SPEED) ? (HIGHWAY_TIME_HORIZON * velocity) : bbox_extension;
+
+      if (distance_to_leading_vehicle.Contains(actor->GetId())) {
+        bbox_extension = std::max(distance_to_leading_vehicle.GetValue(actor->GetId()), bbox_extension);
+      }
+
       auto &waypoint_buffer =  localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer;
 
       LocationList left_boundary;
       LocationList right_boundary;
       auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
       float width = vehicle->GetBoundingBox().extent.y;
+      float length = vehicle->GetBoundingBox().extent.x;
 
       SimpleWaypointPtr boundary_start = waypoint_buffer->front();
-      SimpleWaypointPtr boundary_end = waypoint_buffer->front();
+      uint boundary_start_index = 0u;
+      while (boundary_start->DistanceSquared(vehicle_location) < std::pow(length, 2) &&
+             boundary_start_index < waypoint_buffer->size() -1) {
+        boundary_start = waypoint_buffer->at(boundary_start_index);
+        ++boundary_start_index;
+      }
+      SimpleWaypointPtr boundary_end = waypoint_buffer->at(boundary_start_index);
 
       auto vehicle_reference = boost::static_pointer_cast<cc::Vehicle>(actor);
       // At non-signalized junctions, we extend the boundary across the junction
       // and
       // in all other situations, boundary length is velocity-dependent.
-      for (uint j = 0u;
+      for (uint j = boundary_start_index;
           (boundary_start->DistanceSquared(boundary_end) < std::pow(bbox_extension, 2)) &&
           (j < waypoint_buffer->size());
           ++j) {

LibCarla/source/carla/trafficmanager/CollisionStage.h

@@ -64,6 +64,8 @@ namespace bg = boost::geometry;
     cc::World &world;
     /// Map specifying selective collision avoidance rules between vehicles.
     AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> &selective_collision;
+    /// Map specifying distance to leading vehicle.
+    AtomicMap<ActorId, float> &distance_to_leading_vehicle;
     /// Reference to Carla's debug helper object.
     cc::DebugHelper &debug_helper;
     /// An object used for grid binning vehicles for faster proximity detection.
@@ -110,7 +112,9 @@ namespace bg = boost::geometry;
         std::shared_ptr<CollisionToPlannerMessenger> planner_messenger,
         cc::World &world,
         AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> &selective_collision,
+        AtomicMap<ActorId, float> &distance_to_leading_vehicle,
         cc::DebugHelper &debug_helper);
+
     ~CollisionStage();
 
     void DataReceiver() override;

LibCarla/source/carla/trafficmanager/TrafficManager.cpp

@@ -39,7 +39,7 @@ namespace traffic_manager {
 
     collision_stage = std::make_unique<CollisionStage>(
         localization_collision_messenger, collision_planner_messenger,
-        world, ignore_collision, debug_helper);
+        world, ignore_collision, distance_to_leading_vehicle, debug_helper);
 
     traffic_light_stage = std::make_unique<TrafficLightStage>(
         localization_traffic_light_messenger, traffic_light_planner_messenger);
@@ -88,11 +88,11 @@ namespace traffic_manager {
     return *singleton_pointer.get();
   }
 
-  void TrafficManager::RegisterVehicles(std::vector<ActorPtr> actor_list) {
+  void TrafficManager::RegisterVehicles(const std::vector<ActorPtr> &actor_list) {
     registered_actors.Insert(actor_list);
   }
 
-  void TrafficManager::UnregisterVehicles(std::vector<ActorPtr> actor_list) {
+  void TrafficManager::UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
     registered_actors.Remove(actor_list);
   }
 
@@ -128,15 +128,15 @@ namespace traffic_manager {
     control_stage->Stop();
   }
 
-  void TrafficManager::SetVehicleTargetVelocity(ActorId actor_id, float velocity) {
+  void TrafficManager::SetVehicleTargetVelocity(const ActorPtr &actor, const float velocity) {
 
-    vehicle_target_velocity.AddEntry({actor_id, velocity});
+    vehicle_target_velocity.AddEntry({actor->GetId(), velocity});
   }
 
   void TrafficManager::SetCollisionDetection(
-      ActorPtr reference_actor,
+      const ActorPtr &reference_actor,
-      ActorPtr other_actor,
+      const ActorPtr &other_actor,
-      bool detect_collision) {
+      const bool detect_collision) {
 
     ActorId reference_id = reference_actor->GetId();
     ActorId other_id = other_actor->GetId();
@@ -165,15 +165,21 @@ namespace traffic_manager {
     }
   }
 
-  void TrafficManager::ForceLaneChange(ActorPtr actor, bool direction) {
+  void TrafficManager::ForceLaneChange(const ActorPtr &actor, const bool direction) {
 
     auto entry = std::make_pair(actor->GetId(), direction);
     force_lane_change.AddEntry(entry);
   }
 
-  void TrafficManager::AutoLaneChange(ActorPtr actor, bool enable) {
+  void TrafficManager::AutoLaneChange(const ActorPtr &actor, const bool enable) {
 
     auto entry = std::make_pair(actor->GetId(), enable);
     auto_lane_change.AddEntry(entry);
   }
+
+  void TrafficManager::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
+
+    auto entry = std::make_pair(actor->GetId(), distance);
+    distance_to_leading_vehicle.AddEntry(entry);
+  }
 }

LibCarla/source/carla/trafficmanager/TrafficManager.h

@@ -75,6 +75,8 @@ namespace cc = carla::client;
     static std::unique_ptr<TrafficManager> singleton_pointer;
     /// Singleton lifecycle management mutex.
     // static std::mutex singleton_mutex;
+    /// Map containing distance to leading vehicle command.
+    AtomicMap<ActorId, float> distance_to_leading_vehicle;
 
     /// Private constructor for singleton lifecycle management.
     TrafficManager(
@@ -97,23 +99,29 @@ namespace cc = carla::client;
     static TrafficManager& GetInstance(cc::Client &client_connection);
 
     /// This method registers a vehicle with the traffic manager.
-    void RegisterVehicles(std::vector<ActorPtr> actor_list);
+    void RegisterVehicles(const std::vector<ActorPtr> &actor_list);
 
     /// This method unregisters a vehicle from traffic manager.
-    void UnregisterVehicles(std::vector<ActorPtr> actor_list);
+    void UnregisterVehicles(const std::vector<ActorPtr> &actor_list);
 
     /// Set target velocity specific to a vehicle.
-    void SetVehicleTargetVelocity(ActorId actor_id, float velocity);
+    void SetVehicleTargetVelocity(const ActorPtr &actor, const float velocity);
 
     /// Set collision detection rules between vehicles.
-    void SetCollisionDetection(ActorPtr reference_actor, ActorPtr other_actor, bool detect_collision);
+    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 ForceLaneChange(ActorPtr actor, bool direction);
+    void ForceLaneChange(const ActorPtr &actor, const bool direction);
 
     /// Enable / disable automatic lane change on a vehicle.
-    void AutoLaneChange(ActorPtr actor, bool enable);
+    void AutoLaneChange(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);
 
     /// Destructor.
     ~TrafficManager();

PythonAPI/carla/source/libcarla/TrafficManager.cpp

@@ -22,7 +22,8 @@ void export_trafficmanager() {
         .def("set_vehicle_target_velocity", &traffic_manager::TrafficManager::SetVehicleTargetVelocity)
         .def("set_collision_detection", &traffic_manager::TrafficManager::SetCollisionDetection)
         .def("force_lane_change", &traffic_manager::TrafficManager::ForceLaneChange)
-        .def("set_auto_lane_change", &traffic_manager::TrafficManager::AutoLaneChange);
+        .def("set_auto_lane_change", &traffic_manager::TrafficManager::AutoLaneChange)
+        .def("set_distance_to_leading_vehicle", &traffic_manager::TrafficManager::SetDistanceToLeadingVehicle);
 
     def("GetTrafficManager", &traffic_manager::TrafficManager::GetInstance, return_value_policy<reference_existing_object>());
 

PythonAPI/examples/tm_demo.py

@@ -114,7 +114,10 @@ def main():
         time.sleep(1)
         for v in vehicle_list:
             if (v.id % 2 == 0):
-                traffic_manager.set_vehicle_target_velocity(v.id, 15.0/3.6)
+                traffic_manager.set_vehicle_target_velocity(v, 15.0/3.6)
+                traffic_manager.set_distance_to_leading_vehicle(v, 5)
+            else:
+                traffic_manager.set_distance_to_leading_vehicle(v, 10)
 
         while True:
             time.sleep(1)