fix(obstacle_avoidance_planner): deal with wider path interval (#1616)

* fix(obstacle_avoidance_planner): deal with wider path interval

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_avoidance_planner/src/node.cpp

@@ -1539,7 +1539,13 @@ ObstacleAvoidancePlanner::alignVelocity(
   auto fine_traj_points_with_vel = fine_traj_points_with_path_zero_vel;
   size_t prev_begin_idx = 0;
   for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) {
-    const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0);
+    auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0);
+    if (truncated_points.size() < 2) {
+      // NOTE: At least, two points must be contained in truncated_points
+      truncated_points = std::vector<autoware_auto_planning_msgs::msg::PathPoint>(
+        path_points.begin() + prev_begin_idx,
+        path_points.begin() + std::min(path_points.size(), prev_begin_idx + 2));
+    }
 
     const auto & target_pose = fine_traj_points_with_vel[i].pose;
     const auto closest_seg_idx_optional = motion_utils::findNearestSegmentIndex(