From 5dbea4847a99d845b1aafde369347d882f081884 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 10 Mar 2022 12:52:34 +0900 Subject: [PATCH] fix(behavior_velocity): fix interpolation add missing last point (#501) * fix(behavior_velocity): fix interpolation add missing last point Signed-off-by: tanaka3 * fix(behavior_velocity): add missing last point of clipped path Signed-off-by: tanaka3 * chore(behavior_velocity): make implementation simpler Signed-off-by: tanaka3 Co-authored-by: Takayuki Murooka --- .../scene_module/occlusion_spot/occlusion_spot_utils.cpp | 5 +++-- .../src/utilization/path_utilization.cpp | 7 +++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 8ba933fd2db40..b316bc08e063b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -157,8 +157,9 @@ void clipPathByLength( const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length) { double length_sum = 0; - for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { - length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + clipped.points.emplace_back(path.points.front()); + for (int i = 1; i < static_cast(path.points.size()); i++) { + length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); if (length_sum > max_length) return; clipped.points.emplace_back(path.points.at(i)); } diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp index 7513346ed93d2..3139fc2847649 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp @@ -106,6 +106,13 @@ bool splineInterpolate( output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation; } + + // insert final point on path if its position is not same point as interpolated + const auto & op = output->points.back().point.pose.position; + const auto & ip = input.points.back().point.pose.position; + if (std::hypot(op.x - ip.x, op.y - ip.y) > ep) { + output->points.emplace_back(input.points.back()); + } return true; }