Fixed throughput drop off issue for collision stage

Fixed vechicle grid update logic in vicinity grid

LibCarla/source/carla/trafficmanager/CollisionStage.cpp

@@ -45,20 +45,15 @@ namespace CollisionStageConstants {
   CollisionStage::~CollisionStage() {}
 
   void CollisionStage::Action() {
-
     auto current_planner_frame = frame_selector ? planner_frame_a : planner_frame_b;
 
     // Handle vehicles not spawned by TrafficManager.
     auto current_time = chr::system_clock::now();
     chr::duration<double> diff = current_time - last_world_actors_pass_instance;
-    ++throughput_count;
 
     // Periodically check for actors not spawned by TrafficManager.
     if (diff.count() > 1.0f) {
 
-      // std::cout << "Collision stage throughput : " << throughput_count << " frames per second" << std::endl;
-      throughput_count = 0;
-
       auto world_actors = world.GetActors()->Filter("vehicle.*");
       auto world_walker = world.GetActors()->Filter("walker.*");
       // Scanning for vehicles.
@@ -100,8 +95,6 @@ namespace CollisionStageConstants {
       Actor ego_actor = data.actor;
       ActorId ego_actor_id = ego_actor->GetId();
 
-      // DrawBoundary(GetGeodesicBoundary(ego_actor));
-
       // Retrieve actors around ego actor.
       std::unordered_set<ActorId> actor_id_list = vicinity_grid.GetActors(ego_actor);
       bool collision_hazard = false;
@@ -167,6 +160,7 @@ namespace CollisionStageConstants {
   }
 
   void CollisionStage::DataSender() {
+
     DataPacket<std::shared_ptr<CollisionToPlannerFrame>> packet{
       planner_messenger_state,
       frame_selector ? planner_frame_a : planner_frame_b

LibCarla/source/carla/trafficmanager/CollisionStage.h

@@ -81,9 +81,6 @@ namespace bg = boost::geometry;
     /// Number of vehicles registered with the traffic manager.
     uint number_of_vehicles;
 
-    /// Stage throughput count.
-    uint throughput_count = 0;
-
     /// Returns true if there is a possible collision detected between the
     /// vehicles passed to the method.
     /// Collision is predicted by extrapolating a boundary around the vehicle

LibCarla/source/carla/trafficmanager/VicinityGrid.cpp

@@ -25,12 +25,16 @@ namespace traffic_manager {
     if (actor_to_grid_id.find(actor_id) != actor_to_grid_id.end()) {
 
       std::string old_grid_id = actor_to_grid_id.at(actor_id);
+
       // If the actor has moved into a new road/section/lane.
       if (old_grid_id != new_grid_id) {
         std::unique_lock<std::shared_timed_mutex> lock(modification_mutex);
-        // Remove actor from old grid position and update the new position.
+
+        // Update the actor's grid id.
+        actor_to_grid_id.at(actor_id) = new_grid_id;
+
+        // Remove the actor from the old grid position and update the new position.
         grid_to_actor_id.at(old_grid_id).erase(actor_id);
-        actor_to_grid_id.insert({actor_id, new_grid_id});
         if (grid_to_actor_id.find(new_grid_id) != grid_to_actor_id.end()) {
           grid_to_actor_id.at(new_grid_id).insert(actor_id);
         } else {