Set planning_init_point explicitly in planning interface

modules/planning/planner/em/em_planner.cc

@@ -102,14 +102,14 @@ void EMPlanner::RecordDebugInfo(const std::string& name,
   em_planner_debugger_.speed_profiles_[name].second = time_diff_ms;
 }
 
-Status EMPlanner::Plan(Frame* frame,
+Status EMPlanner::Plan(const TrajectoryPoint& planning_start_point,
+                       Frame* frame,
                        PublishableTrajectory* ptr_publishable_trajectory) {
   if (!frame) {
     AERROR << "Frame is empty in EMPlanner";
     return Status(ErrorCode::PLANNING_ERROR, "Frame is null");
   }
 
-  const auto& planning_start_point = frame->PlanningStartPoint();
   ADEBUG << "planning start point:" << planning_start_point.DebugString();
   auto* planning_data = frame->mutable_planning_data();
 

modules/planning/planner/em/em_planner.h

@@ -65,7 +65,8 @@ class EMPlanner : public Planner {
    * @param trajectory_pb The computed trajectory
    * @return OK if planning succeeds; error otherwise.
    */
-  apollo::common::Status Plan(Frame* frame,
+  apollo::common::Status Plan(const TrajectoryPoint& planning_init_point,
+                              Frame* frame,
                               PublishableTrajectory* trajectory_pb) override;
   EMPlannerDebugger& em_planner_debugger();
 

modules/planning/planner/planner.h

@@ -63,7 +63,8 @@ class Planner {
    * @param trajectory_pb The computed trajectory
    * @return OK if planning succeeds; error otherwise.
    */
-  virtual apollo::common::Status Plan(Frame* frame,
+  virtual apollo::common::Status Plan(const TrajectoryPoint& planning_init_point,
+                                      Frame* frame,
                                       PublishableTrajectory* trajectory_pb) = 0;
 };
 

modules/planning/planner/rtk/rtk_replay_planner.cc

@@ -36,7 +36,7 @@ RTKReplayPlanner::RTKReplayPlanner() {
 
 Status RTKReplayPlanner::Init(const PlanningConfig&) { return Status::OK(); }
 
-Status RTKReplayPlanner::Plan(
+Status RTKReplayPlanner::Plan(const TrajectoryPoint& planning_init_point,
     Frame* frame, PublishableTrajectory* ptr_publishable_trajectory) {
   if (complete_rtk_trajectory_.empty() || complete_rtk_trajectory_.size() < 2) {
     std::string msg(
@@ -46,10 +46,9 @@ Status RTKReplayPlanner::Plan(
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
-  const TrajectoryPoint& start_point = frame->PlanningStartPoint();
 
   std::uint32_t matched_index =
-      QueryPositionMatchedPoint(start_point, complete_rtk_trajectory_);
+      QueryPositionMatchedPoint(planning_init_point, complete_rtk_trajectory_);
 
   std::uint32_t forward_buffer = FLAGS_rtk_trajectory_forward;
   std::uint32_t end_index =

modules/planning/planner/rtk/rtk_replay_planner.h

@@ -58,7 +58,8 @@ class RTKReplayPlanner : public Planner {
    * @param trajectory_pb The computed trajectory
    * @return OK if planning succeeds; error otherwise.
    */
-  apollo::common::Status Plan(Frame* frame,
+  apollo::common::Status Plan(const TrajectoryPoint& planning_init_point,
+                              Frame* frame,
                               PublishableTrajectory* trajectory_pb) override;
 
   /**

modules/planning/planner/rtk/rtk_replay_planner_test.cc

@@ -37,9 +37,7 @@ TEST_F(RTKReplayPlannerTest, ComputeTrajectory) {
   start_point.mutable_path_point()->set_y(4140674.76063);
 
   PublishableTrajectory trajectory;
-  Frame frame(1);
-  frame.SetPlanningStartPoint(start_point);
-  auto status = planner.Plan(&frame, &trajectory);
+  auto status = planner.Plan(start_point, nullptr, &trajectory);
 
   EXPECT_TRUE(status.ok());
   EXPECT_TRUE(!trajectory.trajectory_points().empty());
@@ -65,9 +63,7 @@ TEST_F(RTKReplayPlannerTest, ErrorTest) {
   start_point.mutable_path_point()->set_x(586385.782842);
   start_point.mutable_path_point()->set_y(4140674.76063);
   PublishableTrajectory trajectory;
-  Frame frame(1);
-  frame.SetPlanningStartPoint(start_point);
-  EXPECT_TRUE(!(planner_with_error_csv.Plan(&frame, &trajectory)).ok());
+  EXPECT_TRUE(!(planner_with_error_csv.Plan(start_point, nullptr, &trajectory)).ok());
 }
 
 }  // namespace planning

modules/planning/planning.cc

@@ -247,7 +247,8 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
   frame_->SetPlanningStartPoint(stitching_trajectory.back());
 
   PublishableTrajectory publishable_trajectory;
-  auto status = planner_->Plan(frame_.get(), &publishable_trajectory);
+  auto status = planner_->Plan(stitching_trajectory.back(),
+      frame_.get(), &publishable_trajectory);
 
   if (FLAGS_enable_record_debug) {
     trajectory_pb->mutable_debug()

modules/planning/planning.h

@@ -76,7 +76,8 @@ class Planning : public apollo::common::ApolloApp {
    * @param is_on_auto_mode whether the current system is on auto-driving mode
    * @param trajectory_pb the computed planning trajectory
    */
-  bool Plan(const bool is_on_auto_mode, const double publish_time,
+  bool Plan(const bool is_on_auto_mode,
+            const double publish_time,
             ADCTrajectory* trajectory_pb);
 
   const Frame* GetFrame() const;