Velocity dependent pedestrian bbox extension

Removed snippet profiling code from localization stage

LibCarla/source/carla/trafficmanager/CollisionStage.cpp

@@ -21,7 +21,7 @@ namespace CollisionStageConstants {
   static const float BOUNDARY_EDGE_LENGTH = 2.0f;
   static const float MAX_COLLISION_RADIUS = 100.0f;
   static const float MIN_COLLISION_RADIUS = 15.0f;
-
+  static const float WALKER_TIME_EXTENSION = 2.0f;
 } // namespace CollisionStageConstants
 
   using namespace CollisionStageConstants;
@@ -76,7 +76,7 @@ namespace CollisionStageConstants {
           try {
             const cg::Location ego_location = ego_actor->GetLocation();
             const cg::Location other_location = actor->GetLocation();
-            
+
             // Collision checks increase with speed (Official formula used)
             float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f);
             collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS);
@@ -201,8 +201,8 @@ namespace CollisionStageConstants {
 
         hazard = true;
       }
-    } 
-    
+    }
+
     const auto actor_type = other_vehicle->GetTypeId();
 
     return hazard;
@@ -323,61 +323,41 @@ namespace CollisionStageConstants {
   }
 
   LocationList CollisionStage::GetBoundary(const Actor &actor) {
+
     const auto actor_type = actor->GetTypeId();
+    cg::Location location = actor->GetLocation();
+    cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
+    heading_vector.z = 0.0f;
+    heading_vector = heading_vector.MakeUnitVector();
 
     cg::BoundingBox bbox;
-    cg::Location location;
-    cg::Vector3D heading_vector;
-
+    float forward_extension = 0.0f;
     if (actor_type[0] == 'v') {
-
       const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
       bbox = vehicle->GetBoundingBox();
-      location = vehicle->GetLocation();
-      heading_vector = vehicle->GetTransform().GetForwardVector();
     } else if (actor_type[0] == 'w') {
-
       const auto walker = boost::static_pointer_cast<cc::Walker>(actor);
       bbox = walker->GetBoundingBox();
-      location = walker->GetLocation();
-      heading_vector = walker->GetTransform().GetForwardVector();
+      // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
+      forward_extension = walker->GetVelocity().Length() * WALKER_TIME_EXTENSION;
     }
 
     const cg::Vector3D extent = bbox.extent;
-    heading_vector.z = 0.0f;
     const cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
 
-
-    // Four corners of the vehicle in top view clockwise order (left-handed
-    // system).
-
-    if (actor_type[0] == 'v'){
     const cg::Vector3D x_boundary_vector = heading_vector * extent.x;
     const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y;
 
-    return {
-        location + cg::Location(x_boundary_vector - y_boundary_vector),
-        location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
-        location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
-        location + cg::Location(x_boundary_vector + y_boundary_vector),
+    // Four corners of the vehicle in top view clockwise order (left-handed
+    // system).
+    LocationList bbox_boundary = {
+      location + cg::Location(heading_vector * (extent.x + forward_extension) - y_boundary_vector),
+      location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
+      location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
+      location + cg::Location(heading_vector * (extent.x + forward_extension) + y_boundary_vector),
     };
 
-     // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
-
-    } else {
-      float walker_extension = 0.0f;
-      walker_extension = 3.0f;
-
-      const cg::Vector3D x_boundary_vector = heading_vector * extent.x;
-      const cg::Vector3D y_boundary_vector = perpendicular_vector * extent.y;
-
-      return {
-        location + cg::Location(heading_vector * (extent.x + walker_extension) - y_boundary_vector),
-        location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
-        location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
-        location + cg::Location(heading_vector * (extent.x + walker_extension) + y_boundary_vector),
-      };
-    }
+    return bbox_boundary;
   }
 
   void CollisionStage::DrawBoundary(const LocationList &boundary) {

LibCarla/source/carla/trafficmanager/LocalizationStage.cpp

@@ -313,12 +313,9 @@ namespace LocalizationConstants {
     if (unregistered_scan_duration == UNREGISTERED_ACTORS_SCAN_INTERVAL) {
       unregistered_scan_duration = 0;
 
-      snippet_profiler.MeasureExecutionTime("Fetching world actors", true);
       const auto world_actors = world.GetActors()->Filter("vehicle.*");
       const auto world_walker = world.GetActors()->Filter("walker.*");
-      snippet_profiler.MeasureExecutionTime("Fetching world actors", false);
       // Scanning for vehicles.
-      snippet_profiler.MeasureExecutionTime("Adding unregistered actors", true);
       for (auto actor: *world_actors.get()) {
         const auto unregistered_id = actor->GetId();
         if (vehicle_id_to_index.find(unregistered_id) == vehicle_id_to_index.end() &&
@@ -333,10 +330,8 @@ namespace LocalizationConstants {
           unregistered_actors.insert({unregistered_id, walker});
         }
       }
-      snippet_profiler.MeasureExecutionTime("Adding unregistered actors", false);
     }
 
-    snippet_profiler.MeasureExecutionTime("Updating grids for unregistered actors", true);
     // Regularly update unregistered actors.
     std::vector<ActorId> actor_ids_to_erase;
     for (auto& actor_info: unregistered_actors) {
@@ -366,7 +361,6 @@ namespace LocalizationConstants {
         actor_ids_to_erase.push_back(actor_info.first);
       }
     }
-    snippet_profiler.MeasureExecutionTime("Updating grids for unregistered actors", false);
     for (auto actor_id: actor_ids_to_erase) {
       unregistered_actors.erase(actor_id);
     }

LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.cpp

@@ -26,9 +26,9 @@ namespace traffic_manager {
       const chr::duration<float> throughput_count_duration = current_time - throughput_clock;
       if (throughput_count_duration.count() > 1.0f) {
 
-        // std::cout << "Stage: " + stage_name + ", throughput: " << throughput_counter
-        //           << ", avg. cycle duration: " << 1000* inter_update_duration.count()
-        //           << " ms" << std::endl;
+        std::cout << "Stage: " + stage_name + ", throughput: " << throughput_counter
+                  << ", avg. cycle duration: " << 1000* inter_update_duration.count()
+                  << " ms" << std::endl;
 
         throughput_clock = current_time;
         throughput_counter = 0u;

LibCarla/source/carla/trafficmanager/PerformanceDiagnostics.h

@@ -77,9 +77,9 @@ namespace traffic_manager {
 
       chr::duration<float> print_duration = current_time - print_clock;
       if (print_duration.count() > 1.0f) {
-        //std::cout << "Snippet name : " << snippet_name << ", "
-        //          << "avg. duration : " << 1000 * snippet_duration.count()
-        //          << " ms" << std::endl;
+        std::cout << "Snippet name : " << snippet_name << ", "
+                 << "avg. duration : " << 1000 * snippet_duration.count()
+                 << " ms" << std::endl;
         print_clock = current_time;
       }
     }

LibCarla/source/carla/trafficmanager/PipelineStage.cpp

@@ -34,7 +34,9 @@ namespace traffic_manager {
 
       // Receive data.
       if (run_stage.load()) {
+        performance_diagnostics.RegisterUpdate(true);
         Action();
+        performance_diagnostics.RegisterUpdate(false);
       }
 
       // Receive data.