fix(behavior_velocity): fix detection_area findForwardOffsetSegment

autowarefoundation · Jan 26, 2022 · a554e6e · a554e6e
1 parent 45446dd
commit a554e6e
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp
@@ -193,12 +193,13 @@ boost::optional<PathIndexWithOffset> findBackwardOffsetSegment(
     const auto p_front = to_bg2d(path.points.at(i).point.pose.position);
     const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position);
 
-    sum_length += bg::distance(p_front, p_back);
+    const auto segment_length = bg::distance(p_front, p_back);
+    sum_length += segment_length;
 
     // If it's over offset point, return front index and remain offset length
     if (sum_length >= offset_length) {
-      const auto k = static_cast<std::size_t>(i);
-      return std::make_pair(k, sum_length - offset_length);
+      const auto remain_length = sum_length - offset_length;
+      return std::make_pair(i, segment_length - remain_length);
     }
   }