fix(obstacle_cruise_planner): fix node dying (tier4#1332)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Oct 3, 2022 · 747d909 · 747d909
1 parent 8dd423a
commit 747d909
Showing 1 changed file with 4 additions and 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
@@ -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;
       }
     }
   }