From 0de2f3bb5057e7949c2d7d3c941b57fb8b15b0f4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 13 Jul 2022 14:05:31 +0900 Subject: [PATCH] fix(obstacle_cruise_planner): fix node dying (#1332) Signed-off-by: Takayuki Murooka --- .../include/obstacle_cruise_planner/utils.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 983495bb9a06a..8c391df154bd8 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -96,8 +96,8 @@ size_t getIndexWithLongitudinalOffset( tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); sum_length += segment_length; if (sum_length >= longitudinal_offset) { - const double front_length = segment_length; const double back_length = sum_length - longitudinal_offset; + const double front_length = segment_length - back_length; if (front_length < back_length) { return i; } else { @@ -110,15 +110,15 @@ size_t getIndexWithLongitudinalOffset( for (size_t i = start_idx.get(); i > 0; --i) { const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)); sum_length += segment_length; if (sum_length >= -longitudinal_offset) { - const double front_length = segment_length; const double back_length = sum_length + longitudinal_offset; + const double front_length = segment_length - back_length; if (front_length < back_length) { return i; } else { - return i + 1; + return i - 1; } } }