fix

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

planning/obstacle_avoidance_planner/src/node.cpp

@@ -1484,12 +1484,11 @@ ObstacleAvoidancePlanner::alignVelocity(
   size_t prev_begin_idx = 0;
   for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) {
     auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0);
-    if (truncated_points.empty()) {
-      truncated_points = std::vector<autoware_auto_planning_msgs::msg::PathPoint>(
-        path_points.begin() + prev_begin_idx, path_points.end());
-    }
     if (truncated_points.size() < 2) {
-      truncated_points = path_points;
+      // 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;