fix(motion_utils): check size after overlap points removal (autowaref…

…oundation#6653) * fix(motion_utils): check size after overlap points removal Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * change implementation to not return warning Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * fix comparison sign Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
tier4 · Oct 21, 2024 · 99d0b71 · 99d0b71
1 parent ddff9fd
commit 99d0b71
Showing 1 changed file with 6 additions and 2 deletions.
diff --git 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};