add an option for collision check

Signed-off-by: beyza <[email protected]>
autowarefoundation · May 24, 2024 · 25b3f61 · 25b3f61
1 parent 91c6324
commit 25b3f61
Showing 1 changed file with 22 additions and 8 deletions.
diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp
@@ -86,13 +86,28 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
 
   const auto preprocessing_duration_us = stopwatch.toc("preprocessing");
 
-  stopwatch.tic("footprints");
-  const auto obstacle_forward_footprints =
-    make_forward_footprints(dynamic_obstacles, params_, hysteresis);
-  const auto footprints_duration_us = stopwatch.toc("footprints");
-  stopwatch.tic("collisions");
-  auto collisions =
-    find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
+  tier4_autoware_utils::MultiPolygon2d obstacle_forward_footprints;
+  tier4_autoware_utils::MultiPolygon2d obstacle_predicted_footprints;
+  std::vector<Collision> collisions;
+  double footprints_duration_us;
+  if (params_.use_predicted_path){
+    stopwatch.tic("footprints");
+    obstacle_predicted_footprints = create_object_footprints(dynamic_obstacles, params_);
+    footprints_duration_us = stopwatch.toc("footprints");
+    stopwatch.tic("collisions");
+    collisions =
+      find_collisions(ego_data, dynamic_obstacles, obstacle_predicted_footprints, params_);
+    debug_data_.obstacle_footprints = obstacle_predicted_footprints;
+  } else {
+    stopwatch.tic("footprints");
+    obstacle_forward_footprints =
+      make_forward_footprints(dynamic_obstacles, params_, hysteresis);
+    footprints_duration_us = stopwatch.toc("footprints");
+    stopwatch.tic("collisions");
+    collisions =
+      find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
+    debug_data_.obstacle_footprints = obstacle_forward_footprints;
+  }
   update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_);
   std::optional<geometry_msgs::msg::Point> earliest_collision =
     find_earliest_collision(object_map_, ego_data);
@@ -128,7 +143,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
     "%2.2fus\n\tcollisions = %2.2fus\n",
     total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us);
   debug_data_.ego_footprints = ego_data.path_footprints;
-  debug_data_.obstacle_footprints = obstacle_forward_footprints;
   debug_data_.z = ego_data.pose.position.z;
   return true;
 }