From 9095be2375c035d4c38a2a9a5274ed3300b27483 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 25 Jun 2024 14:26:23 +0900 Subject: [PATCH] fix(autoware_behavior_path_planner_common): fix containerOutOfBounds warning (#7675) * fix(autoware_behavior_path_planner_common): fix containerOutOfBounds warning Signed-off-by: Ryuta Kambe * fix type Signed-off-by: Ryuta Kambe --------- Signed-off-by: Ryuta Kambe --- .../src/utils/path_utils.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 46ca155d04daa..032e5a42069b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -471,14 +471,15 @@ std::vector interpolatePose( { std::vector interpolated_poses{}; // output - const std::vector base_s{ - 0, autoware::universe_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const double distance = + autoware::universe_utils::calcDistance2d(start_pose.position, end_pose.position); + const std::vector base_s{0.0, distance}; const std::vector base_x{start_pose.position.x, end_pose.position.x}; const std::vector base_y{start_pose.position.y, end_pose.position.y}; std::vector new_s; constexpr double eps = 0.3; // prevent overlapping - for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + for (double s = eps; s < distance - eps; s += resample_interval) { new_s.push_back(s); } @@ -491,7 +492,7 @@ std::vector interpolatePose( for (size_t i = 0; i < interpolated_x.size(); ++i) { Pose pose{}; pose = autoware::universe_utils::calcInterpolatedPose( - end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + end_pose, start_pose, (distance - new_s.at(i)) / distance); pose.position.x = interpolated_x.at(i); pose.position.y = interpolated_y.at(i); pose.position.z = end_pose.position.z;