Planning: use init planning point on last planning trajectory.

modules/planning/common/frame.cc

@@ -94,8 +94,8 @@ bool Frame::InitReferenceLineInfo(
     const std::vector<ReferenceLine> &reference_lines) {
   reference_line_info_.clear();
   for (const auto &reference_line : reference_lines) {
-    reference_line_info_.emplace_back(pnc_map_, reference_line,
-                                      smoother_config_);
+    reference_line_info_.emplace_back(planning_start_point_, pnc_map_,
+                                      reference_line, smoother_config_);
     if (!reference_line_info_.back().Init()) {
       AERROR << "Failed to init reference line info";
       return false;
@@ -122,8 +122,8 @@ bool Frame::Init(const PlanningConfig &config,
     return false;
   }
   smoother_config_ = config.reference_line_smoother_config();
-  auto reference_lines = CreateReferenceLineFromRouting(init_pose_.position(),
-                                                        routing_response_);
+  auto reference_lines =
+      CreateReferenceLineFromRouting(init_pose_.position(), routing_response_);
   if (reference_lines.empty()) {
     AERROR << "Failed to create reference line from position: "
            << init_pose_.DebugString();

modules/planning/common/reference_line_info.cc

@@ -34,10 +34,16 @@
 namespace apollo {
 namespace planning {
 
+using TrajectoryPoint = apollo::common::TrajectoryPoint;
+using Vec2d = apollo::common::math::Vec2d;
+using SLPoint = apollo::common::SLPoint;
+
 ReferenceLineInfo::ReferenceLineInfo(
-    const hdmap::PncMap* pnc_map, const ReferenceLine& reference_line,
+    const TrajectoryPoint& init_adc_point, const hdmap::PncMap* pnc_map,
+    const ReferenceLine& reference_line,
     const ReferenceLineSmootherConfig& smoother_config)
-    : pnc_map_(pnc_map),
+    : init_adc_point_(init_adc_point),
+      pnc_map_(pnc_map),
       reference_line_(reference_line),
       smoother_config_(smoother_config) {}
 
@@ -58,14 +64,23 @@ bool ReferenceLineInfo::Init() {
 }
 
 bool ReferenceLineInfo::CalculateAdcSmoothReferenLinePoint() {
+  SLPoint adc_sl_on_raw_ref;
+  if (!reference_line_.XYToSL(Vec2d(init_adc_point_.path_point().x(),
+                                    init_adc_point_.path_point().y()),
+                              &adc_sl_on_raw_ref)) {
+    AERROR << "Fail to get sl point for init_adc_point. init_adc_point: "
+           << init_adc_point_.DebugString();
+    return false;
+  }
+
   const double backward_s = -5.0;
   const double forward_s = 10.0;
   const double delta_s = 2.0;
-  double s = backward_s + adc_sl_boundary_.start_s();
+  double s = backward_s + adc_sl_on_raw_ref.s();
 
   std::vector<ReferencePoint> local_ref_points;
 
-  while (s <= forward_s + adc_sl_boundary_.end_s()) {
+  while (s <= forward_s + adc_sl_on_raw_ref.s()) {
     local_ref_points.push_back(reference_line_.GetReferencePoint(s));
     s += delta_s;
   }
@@ -78,8 +93,8 @@ bool ReferenceLineInfo::CalculateAdcSmoothReferenLinePoint() {
     AERROR << "Failed to smooth reference line";
     return false;
   }
-  adc_smooth_ref_point_ =
-      smoothed_segment.GetReferencePoint(adc_sl_boundary_.end_s());
+  adc_smooth_ref_point_ = smoothed_segment.GetReferencePoint(
+      init_adc_point_.path_point().x(), init_adc_point_.path_point().y());
   return true;
 }
 

modules/planning/common/reference_line_info.h

@@ -43,6 +43,7 @@ namespace planning {
 class ReferenceLineInfo {
  public:
   explicit ReferenceLineInfo(
+      const common::TrajectoryPoint& init_adc_point,
       const hdmap::PncMap* pnc_map, const ReferenceLine& reference_line,
       const ReferenceLineSmootherConfig& smoother_config);
 
@@ -112,6 +113,7 @@ class ReferenceLineInfo {
   bool InitPerceptionSLBoundary(PathObstacle* path_obstacle);
   bool CalculateAdcSmoothReferenLinePoint();
 
+  const common::TrajectoryPoint& init_adc_point_;
   const hdmap::PncMap* pnc_map_ = nullptr;
 
   /**

modules/planning/planner/rtk/rtk_replay_planner_test.cc

@@ -52,7 +52,7 @@ TEST_F(RTKReplayPlannerTest, ComputeTrajectory) {
   localization.mutable_pose()->mutable_linear_acceleration()->set_z(0.0);
   common::VehicleState::instance()->Update(localization, chassis);
   ReferenceLineSmootherConfig smooth_config;
-  ReferenceLineInfo info(nullptr, reference_line, smooth_config);
+  ReferenceLineInfo info(start_point, nullptr, reference_line, smooth_config);
   auto status = planner.Plan(start_point, nullptr, &info);
 
   const auto& trajectory = info.trajectory();
@@ -93,7 +93,7 @@ TEST_F(RTKReplayPlannerTest, ErrorTest) {
   common::VehicleState::instance()->Update(localization, chassis);
   ReferenceLine ref;
   ReferenceLineSmootherConfig smooth_config;
-  ReferenceLineInfo info(nullptr, ref, smooth_config);
+  ReferenceLineInfo info(start_point, nullptr, ref, smooth_config);
   EXPECT_TRUE(!(planner_with_error_csv.Plan(start_point, nullptr, &info)).ok());
 }