diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index bed9bbfa500c6..7eca9b9c7ab70 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -51,7 +51,7 @@ geometry_msgs::msg::Point getStartPoint( { const size_t segment_idx = autoware::motion_utils::findNearestSegmentIndex(bound, point); const auto & curr_seg_point = bound.at(segment_idx); - const auto & next_seg_point = bound.at(segment_idx); + const auto & next_seg_point = bound.at(segment_idx + 1); const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; const Eigen::Vector2d first_to_second{ next_seg_point.x - curr_seg_point.x, next_seg_point.y - curr_seg_point.y};