From 129e47599a5c643262a8c3c45057195a79588ebb Mon Sep 17 00:00:00 2001 From: beyza Date: Fri, 24 May 2024 16:45:21 +0300 Subject: [PATCH] update collision check for multipolygon Signed-off-by: beyza --- .../src/collision.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index b9e5427b01b9c..c7e996c8d36f5 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -70,11 +70,21 @@ std::vector find_collisions( const PlannerParam & params) { std::vector collisions; + std::optional collision; for (auto object_idx = 0UL; object_idx < objects.size(); ++object_idx) { const auto & object_pose = objects[object_idx].kinematics.initial_pose_with_covariance.pose; - const auto & object_footprint = object_forward_footprints[object_idx]; - const auto collision = - find_closest_collision_point(ego_data, object_pose, object_footprint, params); + if (!params.ignore_objects_behind_ego){ + tier4_autoware_utils::MultiPolygon2d object_footprint; + for (const auto &polygon : object_forward_footprints) { + object_footprint.push_back(polygon); + collision = + find_closest_collision_point(ego_data, object_pose, polygon, params); + } + } else { + const auto &object_footprint = object_forward_footprints[object_idx]; + collision = + find_closest_collision_point(ego_data, object_pose, object_footprint, params); + } if (collision) { Collision c; c.object_uuid = tier4_autoware_utils::toHexString(objects[object_idx].object_id);