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 <zulfaqar.azmi@tier4.jp> * change implementation to not return warning Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix comparison sign Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
tier4 · Mar 22, 2024 · 779ad1d · 779ad1d
1 parent 44b76ab
commit 779ad1d
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
@@ -607,8 +607,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};