Planning: use last frame path as speed fallback

modules/planning/common/frame.h

@@ -131,6 +131,15 @@ class Frame {
     return current_frame_planned_trajectory_;
   }
 
+  void set_current_frame_planned_path(
+      DiscretizedPath current_frame_planned_path) {
+    current_frame_planned_path_ = std::move(current_frame_planned_path);
+  }
+
+  const DiscretizedPath &current_frame_planned_path() const {
+    return current_frame_planned_path_;
+  }
+
   const bool is_near_destination() const { return is_near_destination_; }
 
   /**
@@ -194,7 +203,12 @@ class Frame {
       traffic_lights_;
 
   ChangeLaneDecider change_lane_decider_;
-  ADCTrajectory current_frame_planned_trajectory_;  // last published trajectory
+
+  // current frame published trajectory
+  ADCTrajectory current_frame_planned_trajectory_;
+
+  // current frame path for future possible speed fallback
+  DiscretizedPath current_frame_planned_path_;
 
   const ReferenceLineProvider *reference_line_provider_ = nullptr;
 

modules/planning/on_lane_planning.cc

@@ -16,6 +16,7 @@
 
 #include "modules/planning/on_lane_planning.h"
 
+#include <algorithm>
 #include <limits>
 #include <list>
 #include <utility>
@@ -486,6 +487,17 @@ Status OnLanePlanning::Plan(
       }
       return Status(ErrorCode::PLANNING_ERROR, msg);
     }
+    // Store current frame stitched path for possible speed fallback in next
+    // frames
+    DiscretizedPath current_frame_planned_path;
+    for (const auto& trajectory_point : stitching_trajectory) {
+      current_frame_planned_path.push_back(trajectory_point.path_point());
+    }
+    const auto& best_ref_path = best_ref_info->path_data().discretized_path();
+    std::copy(best_ref_path.begin() + 1, best_ref_path.end(),
+              std::back_inserter(current_frame_planned_path));
+    frame_->set_current_frame_planned_path(current_frame_planned_path);
+
     if (FLAGS_export_chart) {
       ExportOnLaneChart(best_ref_info->debug(), ptr_debug);
     } else {

modules/planning/scenarios/lane_follow/lane_follow_stage.cc

@@ -344,14 +344,9 @@ bool LaneFollowStage::RetrieveLastFramePathProfile(
         << "Last frame doesn't succeed, fail to retrieve last frame path data";
     return false;
   }
-  const auto& last_frame_trajectory_pb =
-      ptr_last_frame->current_frame_planned_trajectory();
 
-  DiscretizedPath last_frame_discretized_path;
-  for (const auto& trajectory_point :
-       last_frame_trajectory_pb.trajectory_point()) {
-    last_frame_discretized_path.push_back(trajectory_point.path_point());
-  }
+  const auto& last_frame_discretized_path =
+      ptr_last_frame->current_frame_planned_path();
 
   path_data->SetDiscretizedPath(last_frame_discretized_path);
   const auto adc_frenet_frame_point_ =