From 115a4ee2620f6016c059f1124a6b481336d055dc Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 19 Mar 2024 13:43:28 +0900 Subject: [PATCH 1/3] fix(motion_utils): check size after overlap points removal Signed-off-by: Muhammad Zulfaqar Azmi --- .../include/motion_utils/trajectory/trajectory.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index b9136bc4002e3..9a326619bb6c7 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -607,6 +607,19 @@ double calcLateralOffset( return std::nan(""); } + if (overlap_removed_points.size() >= seg_idx) { + const std::string error_message( + "[motion_utils] " + std::string(__func__) + ": Overlap points removed exceeded seg_idx."); + tier4_autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); + 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)); From 6a730ee44de337238cc9eba092c1372e6f718c86 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 19 Mar 2024 18:19:13 +0900 Subject: [PATCH 2/3] change implementation to not return warning Signed-off-by: Muhammad Zulfaqar Azmi --- .../motion_utils/trajectory/trajectory.hpp | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 9a326619bb6c7..9a6a644c85e31 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -607,21 +607,12 @@ double calcLateralOffset( return std::nan(""); } - if (overlap_removed_points.size() >= seg_idx) { - const std::string error_message( - "[motion_utils] " + std::string(__func__) + ": Overlap points removed exceeded seg_idx."); - tier4_autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - return std::nan(""); - } + 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(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_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}; From 2fdbde0062f208f6832087eabf98745608a04fc7 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Wed, 20 Mar 2024 00:22:57 +0900 Subject: [PATCH 3/3] fix comparison sign Signed-off-by: Muhammad Zulfaqar Azmi --- .../motion_utils/include/motion_utils/trajectory/trajectory.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 9a6a644c85e31..7c9b5a5378ab6 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -608,7 +608,7 @@ double calcLateralOffset( } 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_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));