fix(obstacle_avoidance_planner): edge case for path size is lower tha…

…n nearest index + 1 (tier4#1320)

* fix: edge case for path size is lower than nearest index + 1

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* ci(pre-commit): autofix

* Update planning/obstacle_avoidance_planner/src/node.cpp

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

planning/obstacle_avoidance_planner/src/node.cpp

@@ -1135,12 +1135,14 @@ void ObstacleAvoidancePlanner::calcVelocity(
         path_points, traj_points.at(i).pose.position);
     }();
 
+    // add this line not to exceed max index size
+    const size_t max_idx = std::min(nearest_seg_idx + 1, path_points.size() - 1);
     // NOTE: std::max, not std::min, is used here since traj_points' sampling width may be longer
     // than path_points' sampling width. A zero velocity point is guaranteed to be inserted in an
     // output trajectory in the alignVelocity function
     traj_points.at(i).longitudinal_velocity_mps = std::max(
       path_points.at(nearest_seg_idx).longitudinal_velocity_mps,
-      path_points.at(nearest_seg_idx + 1).longitudinal_velocity_mps);
+      path_points.at(max_idx).longitudinal_velocity_mps);
   }
 }