From 99d0b71dfd4f08bfc4698a35c027ef32fa6d2e0b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 22 Mar 2024 19:14:33 +0900 Subject: [PATCH] fix(motion_utils): check size after overlap points removal (#6653) * fix(motion_utils): check size after overlap points removal Signed-off-by: Muhammad Zulfaqar Azmi * change implementation to not return warning Signed-off-by: Muhammad Zulfaqar Azmi * fix comparison sign Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../include/motion_utils/trajectory/trajectory.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f2e0c1c0c184c..6cb7d34e3aab1 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -590,8 +590,12 @@ double calcLateralOffset( return std::nan(""); } - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_indices = overlap_removed_points.size() - 2; + const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices; + const auto p_back_idx = p_front_idx + 1; + + const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx)); + const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};