From 25b3f61c63473867c2f66f55c0ca6ac4c24c864c Mon Sep 17 00:00:00 2001 From: beyza Date: Fri, 24 May 2024 11:32:51 +0300 Subject: [PATCH] add an option for collision check Signed-off-by: beyza --- .../src/scene_dynamic_obstacle_stop.cpp | 30 ++++++++++++++----- 1 file changed, 22 insertions(+), 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 index 022a079f7d7bf..e62eb07918def 100644 --- 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 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 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; }