planning: use planning_init_point to create adc_sl_boundary

modules/planning/common/frame.cc

@@ -94,14 +94,14 @@ 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(planning_start_point_, pnc_map_,
-                                      reference_line, smoother_config_);
-    if (!reference_line_info_.back().Init()) {
-      AERROR << "Failed to init reference line info";
-      return false;
-    }
+    reference_line_info_.emplace_back(pnc_map_, reference_line,
+                                      planning_start_point_, smoother_config_);
   }
   for (auto &info : reference_line_info_) {
+    if (!info.Init()) {
+      AERROR << "Failed to init adc sl boundary";
+      return false;
+    }
     if (!info.AddObstacles(obstacles_.Items())) {
       AERROR << "Failed to add obstacles to reference line";
       return false;
@@ -131,7 +131,7 @@ bool Frame::Init(const PlanningConfig &config,
   }
 
   ADEBUG << "Enabled align prediction time ? : " << std::boolalpha
-        << FLAGS_align_prediction_time;
+         << FLAGS_align_prediction_time;
   if (FLAGS_align_prediction_time) {
     AlignPredictionTime(current_time_stamp);
   }

modules/planning/common/reference_line_info.cc

@@ -24,9 +24,9 @@
 
 #include "modules/planning/proto/sl_boundary.pb.h"
 
+#include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/util/string_util.h"
 #include "modules/common/util/util.h"
-#include "modules/common/vehicle_state/vehicle_state.h"
 #include "modules/map/hdmap/hdmap_common.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/reference_line/reference_line_smoother.h"
@@ -34,28 +34,29 @@
 namespace apollo {
 namespace planning {
 
+using SLPoint = apollo::common::SLPoint;
 using TrajectoryPoint = apollo::common::TrajectoryPoint;
 using Vec2d = apollo::common::math::Vec2d;
-using SLPoint = apollo::common::SLPoint;
+using apollo::common::VehicleConfigHelper;
 
 ReferenceLineInfo::ReferenceLineInfo(
-    const TrajectoryPoint& init_adc_point, const hdmap::PncMap* pnc_map,
-    const ReferenceLine& reference_line,
+    const hdmap::PncMap* pnc_map, const ReferenceLine& reference_line,
+    const TrajectoryPoint& init_adc_point,
     const ReferenceLineSmootherConfig& smoother_config)
-    : init_adc_point_(init_adc_point),
-      pnc_map_(pnc_map),
+    : pnc_map_(pnc_map),
       reference_line_(reference_line),
+      init_adc_point_(init_adc_point),
       smoother_config_(smoother_config) {}
 
 bool ReferenceLineInfo::Init() {
-  const auto& box = common::VehicleState::instance()->AdcBoundingBox();
+  const auto& vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param();
+  const auto& path_point = init_adc_point_.path_point();
+  common::math::Box2d box({path_point.x(), path_point.y()}, path_point.theta(),
+                          vehicle_param.length(), vehicle_param.width());
   if (!reference_line_.GetSLBoundary(box, &adc_sl_boundary_)) {
     AERROR << "Failed to get ADC boundary from box: " << box.DebugString();
     return false;
   }
-  for (const auto& lane_segment : reference_line_.map_path().lane_segments()) {
-    reference_line_lane_id_set_.insert(lane_segment.lane->id().id());
-  }
   if (!CalculateAdcSmoothReferenLinePoint()) {
     AERROR << "Fail to get ADC smooth reference line point.";
     return false;
@@ -64,23 +65,14 @@ 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_on_raw_ref.s();
+  double s = backward_s + adc_sl_boundary_.start_s();
 
   std::vector<ReferencePoint> local_ref_points;
 
-  while (s <= forward_s + adc_sl_on_raw_ref.s()) {
+  while (s <= forward_s + adc_sl_boundary_.end_s()) {
     local_ref_points.push_back(reference_line_.GetReferencePoint(s));
     s += delta_s;
   }

modules/planning/common/reference_line_info.h

@@ -43,8 +43,8 @@ namespace planning {
 class ReferenceLineInfo {
  public:
   explicit ReferenceLineInfo(
-      const common::TrajectoryPoint& init_adc_point,
       const hdmap::PncMap* pnc_map, const ReferenceLine& reference_line,
+      const common::TrajectoryPoint& init_adc_point,
       const ReferenceLineSmootherConfig& smoother_config);
 
   bool Init();
@@ -113,8 +113,9 @@ class ReferenceLineInfo {
   bool InitPerceptionSLBoundary(PathObstacle* path_obstacle);
   bool CalculateAdcSmoothReferenLinePoint();
 
-  const common::TrajectoryPoint& init_adc_point_;
   const hdmap::PncMap* pnc_map_ = nullptr;
+  const ReferenceLine reference_line_;
+  const common::TrajectoryPoint init_adc_point_;
 
   /**
    * @brief this is the number that measures the goodness of this reference
@@ -124,7 +125,6 @@ class ReferenceLineInfo {
    */
   double cost_ = 0.0;
 
-  const ReferenceLine reference_line_;
   PathDecision path_decision_;
 
   PathData path_data_;
@@ -134,12 +134,10 @@ class ReferenceLineInfo {
 
   SLBoundary adc_sl_boundary_;
   ReferencePoint adc_smooth_ref_point_;
-  ReferenceLineSmootherConfig smoother_config_;
+  const ReferenceLineSmootherConfig smoother_config_;
 
   planning_internal::Debug debug_;
   LatencyStats latency_stats_;
-
-  std::unordered_set<std::string> reference_line_lane_id_set_;
 };
 
 }  // namespace planning

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(start_point, nullptr, reference_line, smooth_config);
+  ReferenceLineInfo info(nullptr, reference_line, start_point, 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(start_point, nullptr, ref, smooth_config);
+  ReferenceLineInfo info(nullptr, ref, start_point, smooth_config);
   EXPECT_TRUE(!(planner_with_error_csv.Plan(start_point, nullptr, &info)).ok());
 }