fix(avoidance): fix detection area issue in avoidance module (autowar…

…efoundation#6097) * fix(avoidance): fix invalid vector access Signed-off-by: satoshi-ota <[email protected]> * fix(avoidance): fix detection area Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
satoshi-ota · Jan 17, 2024 · dc3349b · dc3349b
1 parent 0466378
commit dc3349b
Showing 1 changed file with 17 additions and 8 deletions.
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -1913,6 +1913,10 @@ std::pair<PredictedObjects, PredictedObjects> 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<PredictedObjects, PredictedObjects> 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<Polygon2d> 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<Polygon2d> 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<PredictedObjects, PredictedObjects> 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;
   }