From dc3349b8974648b6cf136a910e7707388c907fd4 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 17 Jan 2024 18:14:36 +0900 Subject: [PATCH] fix(avoidance): fix detection area issue in avoidance module (#6097) * fix(avoidance): fix invalid vector access Signed-off-by: satoshi-ota * fix(avoidance): fix detection area Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/utils.cpp | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 80aeb2bf0dccf..3a9b5db283304 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1913,6 +1913,10 @@ std::pair separateObjectsByPath( PredictedObjects target_objects; PredictedObjects other_objects; + if (reference_path.points.empty() || spline_path.points.empty()) { + return std::make_pair(target_objects, other_objects); + } + double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; @@ -1925,16 +1929,15 @@ std::pair separateObjectsByPath( createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); const auto ego_idx = planner_data->findEgoIndex(reference_path.points); const auto arc_length_array = - utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 1e-3); + utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 0.0); - double next_longitudinal_distance = 0.0; - std::vector detection_areas; - for (size_t i = 0; i < reference_path.points.size() - 1; ++i) { - const auto & p_reference_ego_front = reference_path.points.at(i).point.pose; - const auto & p_reference_ego_back = reference_path.points.at(i + 1).point.pose; - const auto & p_spline_ego_front = spline_path.points.at(i).point.pose; - const auto & p_spline_ego_back = spline_path.points.at(i + 1).point.pose; + const auto points_size = std::min(reference_path.points.size(), spline_path.points.size()); + std::vector detection_areas; + Pose p_reference_ego_front = reference_path.points.front().point.pose; + Pose p_spline_ego_front = spline_path.points.front().point.pose; + double next_longitudinal_distance = parameters->resample_interval_for_output; + for (size_t i = 0; i < points_size; ++i) { const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; @@ -1944,10 +1947,16 @@ std::pair separateObjectsByPath( continue; } + const auto & p_reference_ego_back = reference_path.points.at(i).point.pose; + const auto & p_spline_ego_back = spline_path.points.at(i).point.pose; + detection_areas.push_back(createOneStepPolygon( p_reference_ego_front, p_reference_ego_back, p_spline_ego_front, p_spline_ego_back, detection_area)); + p_reference_ego_front = p_reference_ego_back; + p_spline_ego_front = p_spline_ego_back; + next_longitudinal_distance += parameters->resample_interval_for_output; }