Implemented path oriented collision avoidance

Fixed incorrect target grid logic for collision avoidance

LibCarla/source/carla/trafficmanager/CollisionStage.cpp

@@ -12,6 +12,7 @@ namespace CollisionStageConstants {
   static const float HIGHWAY_TIME_HORIZON = 5.0f;
   static const float CRAWL_SPEED = 10.0f / 3.6f;
   static const float BOUNDARY_EDGE_LENGTH = 2.0f;
+  static const float MINIMUM_GRID_EXTENSION = 10.0f;
 }
   using namespace  CollisionStageConstants;
 
@@ -95,19 +96,10 @@ namespace CollisionStageConstants {
 
       // DrawBoundary(GetGeodesicBoundary(ego_actor));
 
-      vicinity_grid.UpdateGrid(ego_actor);
-      if (vehicle_id_to_index.find(ego_actor->GetId()) == vehicle_id_to_index.end()) {
-        std::cout << " Yep, that definitely happend ! " << std::endl;
-      }
-
       // Retrieve actors around the path of the ego vehicle.
-      float bbox_extension = GetBoundingBoxExtention(ego_actor);
-      auto &waypoint_buffer =  localization_frame->at(
-        vehicle_id_to_index.at(ego_actor->GetId())).buffer;
-      std::unordered_set<ActorId> actor_id_list = vicinity_grid.GetActors(
-        ego_actor, waypoint_buffer, bbox_extension);
-      bool collision_hazard = false;
+      std::unordered_set<ActorId> actor_id_list = GetPotentialVehicleObstacles(ego_actor);
 
+      bool collision_hazard = false;
       // Check every actor in the vicinity if it poses a collision hazard.
       for (auto j = actor_id_list.begin(); (j != actor_id_list.end()) && !collision_hazard; ++j) {
         ActorId actor_id = *j;
@@ -120,6 +112,12 @@ namespace CollisionStageConstants {
             actor = unregistered_actors.at(actor_id);
           }
 
+          debug_helper.DrawLine(
+            ego_actor->GetLocation() + cg::Location(0, 0, 2),
+            actor->GetLocation() + cg::Location(0, 0, 2), 0.2,
+            {255u, 0u, 0u}, 0.1
+          );
+
           if (actor_id != ego_actor_id &&
               parameters.GetCollisionDetection(ego_actor, actor) &&
               NegotiateCollision(ego_actor, actor)) {
@@ -346,6 +344,21 @@ namespace CollisionStageConstants {
     return bbox_extension;
   }
 
+  std::unordered_set<ActorId> CollisionStage::GetPotentialVehicleObstacles(const Actor &ego_vehicle) {
+
+    vicinity_grid.UpdateGrid(ego_vehicle);
+    float grid_extension = std::max(GetBoundingBoxExtention(ego_vehicle), MINIMUM_GRID_EXTENSION);
+    std::unordered_set<ActorId> actor_id_list;
+
+    Buffer &waypoint_buffer =  localization_frame->at(
+      vehicle_id_to_index.at(ego_vehicle->GetId())).buffer;
+
+    actor_id_list = vicinity_grid.GetActors(
+      ego_vehicle, waypoint_buffer, grid_extension);
+
+    return actor_id_list;
+  }
+
   LocationList CollisionStage::GetBoundary(const Actor &actor) const {
     auto actor_type = actor->GetTypeId();
 

LibCarla/source/carla/trafficmanager/CollisionStage.h

@@ -102,6 +102,9 @@ namespace bg = boost::geometry;
     /// Method to calculate the speed dependent bounding box extention for a vehicle.
     float GetBoundingBoxExtention(const Actor &ego_vehicle) const;
 
+    /// Method to retreive the set of vehicles around the path of the given vehicle.
+    std::unordered_set<ActorId> GetPotentialVehicleObstacles(const Actor &ego_vehicle);
+
     /// A simple method used to draw bounding boxes around vehicles
     void DrawBoundary(const LocationList &boundary) const;
 

LibCarla/source/carla/trafficmanager/VicinityGrid.cpp

@@ -78,7 +78,6 @@ namespace traffic_manager {
         }
       }
     } else {
-
       std::unordered_set<std::string> visited_grids;
       for (uint i = 0u;
            !buffer.empty() &&