From 94a512405f7eaf1e6423bd3f7dace420f5661495 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 13 Oct 2023 15:53:55 +0900 Subject: [PATCH] Fix append of new path points to satisfy the resampling interval Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 38 +++++++++---------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 48e5e19a95443..b8d27a39a7eda 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -59,27 +59,23 @@ void reuse_previous_points( } } } - const auto resampled_path_points = - motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; - auto first_path_idx = - cropped_poses.empty() - ? 0LU - : *motion_utils::findNearestSegmentIndex(resampled_path_points, cropped_poses.back()) + 1; - // make sure we do not had the same last point or segment - if ( - !cropped_poses.empty() && resampled_path_points.size() > first_path_idx + 1 && - tier4_autoware_utils::calcDistance2d( - resampled_path_points[first_path_idx + 1], cropped_poses.back()) < 1e-1) - first_path_idx += 2; - else if ( - !cropped_poses.empty() && tier4_autoware_utils::calcDistance2d( - resampled_path_points[first_path_idx], cropped_poses.back()) < 1e-1) - ++first_path_idx; - for (auto idx = first_path_idx; idx < resampled_path_points.size(); ++idx) - cropped_poses.push_back(resampled_path_points[idx].point.pose); - cropped_poses = motion_utils::cropForwardPoints( - cropped_poses, cropped_poses.front().position, 0LU, params.max_path_arc_length); - + if (cropped_poses.empty()) { + const auto resampled_path_points = + motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + for (const auto & p : resampled_path_points) cropped_poses.push_back(p.point.pose); + cropped_poses = + motion_utils::cropForwardPoints(cropped_poses, ego_point, 0LU, params.max_path_arc_length); + } else if (!path.points.empty()) { + const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = motion_utils::calcArcLength(path.points); + const auto first_arc_length = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, cropped_poses.back().position); + for (auto arc_length = first_arc_length + params.resample_interval; + initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length && + arc_length <= max_path_arc_length; + arc_length += params.resample_interval) + cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); + } prev_poses = cropped_poses; prev_curvatures = cropped_curvatures; }