refactor reference line smoother

modules/planning/common/frame.cc

@@ -40,7 +40,12 @@ void Frame::SetMap(hdmap::PncMap *pnc_map) { pnc_map_ = pnc_map; }
 FrameHistory::FrameHistory()
     : IndexedQueue<uint32_t, Frame>(FLAGS_max_history_frame_num) {}
 
-Frame::Frame(const uint32_t sequence_num) : sequence_num_(sequence_num) {}
+Frame::Frame(const uint32_t sequence_num) : sequence_num_(sequence_num) {
+  DCHECK(common::util::GetProtoFromFile(
+      FLAGS_reference_line_smoother_config_file, &smoother_config_))
+      << "Failed to init reference line smoother config with file "
+      << FLAGS_reference_line_smoother_config_file;
+}
 
 void Frame::SetVehicleInitPose(const localization::Pose &pose) {
   init_pose_ = pose;
@@ -151,45 +156,12 @@ bool Frame::CreateReferenceLineFromRouting(
     return false;
   }
   reference_lines->emplace_back(ReferenceLine());
-  if (!SmoothReferenceLine(hdmap_path, &reference_lines->back())) {
-    AERROR << "Failed to smooth reference line";
-    return false;
-  }
-  return true;
-}
-
-bool Frame::SmoothReferenceLine(const hdmap::Path &hdmap_path,
-                                ReferenceLine *const reference_line) {
-  CHECK_NOTNULL(reference_line);
-  std::vector<ReferencePoint> ref_points;
-
-  for (const auto &point : hdmap_path.path_points()) {
-    if (point.lane_waypoints().empty()) {
-      AERROR << "path point has no lane_waypoint";
-      return false;
-    }
-    const auto &lane_waypoint = point.lane_waypoints()[0];
-    ref_points.emplace_back(point, point.heading(), 0.0, 0.0, lane_waypoint);
-  }
-  if (ref_points.empty()) {
-    AERROR << "Found no reference points from map";
-    return false;
-  }
-
-  *reference_line = ReferenceLine(ref_points);
-  std::vector<ReferencePoint> smoothed_ref_points;
   ReferenceLineSmoother smoother;
-  if (!smoother.Init(FLAGS_reference_line_smoother_config_file)) {
-    AERROR << "Failed to load file "
-           << FLAGS_reference_line_smoother_config_file;
-    return false;
-  }
-  if (!smoother.smooth(*reference_line, &smoothed_ref_points)) {
-    AERROR << "Fail to smooth a reference line from map";
+  smoother.Init(smoother_config_);
+  if (!smoother.smooth(ReferenceLine(hdmap_path), &reference_lines->back())) {
+    AERROR << "Failed to smooth reference line";
     return false;
   }
-  *reference_line = ReferenceLine(smoothed_ref_points);
-  ADEBUG << "smooth reference points num:" << smoothed_ref_points.size();
   return true;
 }
 

modules/planning/common/frame.h

@@ -27,6 +27,9 @@
 
 #include "modules/common/proto/geometry.pb.h"
 #include "modules/localization/proto/pose.pb.h"
+#include "modules/planning/proto/reference_line_smoother_config.pb.h"
+#include "modules/prediction/proto/prediction_obstacle.pb.h"
+#include "modules/routing/proto/routing.pb.h"
 
 #include "modules/map/hdmap/hdmap.h"
 #include "modules/map/pnc_map/pnc_map.h"
@@ -35,8 +38,6 @@
 #include "modules/planning/common/path_decision.h"
 #include "modules/planning/common/planning_data.h"
 #include "modules/planning/proto/planning.pb.h"
-#include "modules/prediction/proto/prediction_obstacle.pb.h"
-#include "modules/routing/proto/routing.pb.h"
 
 namespace apollo {
 namespace planning {
@@ -96,8 +97,6 @@ class Frame {
    * TODO move this function to a helper class in
    * modules/planning/reference_line folder
    */
-  bool SmoothReferenceLine(const hdmap::Path &hdmap_path,
-                           ReferenceLine *const reference_line);
 
   /**
    * @brief create obstacles from prediction input.
@@ -133,6 +132,7 @@ class Frame {
   ReferenceLine reference_line_;
   PlanningData _planning_data;
   static const hdmap::PncMap *pnc_map_;
+  ReferenceLineSmootherConfig smoother_config_;
 
   ADCTrajectory trajectory_pb_;  // planning output pb
 };

modules/planning/integration_tests/planning_test_base.cc

@@ -68,7 +68,8 @@ bool PlanningTestBase::SetUpAdapters() {
            << FLAGS_adapter_config_path;
     return false;
   }
-  if (!AdapterManager::FeedRoutingResponseFile(FLAGS_test_routing_result_file)) {
+  if (!AdapterManager::FeedRoutingResponseFile(
+          FLAGS_test_routing_result_file)) {
     AERROR << "failed to routing file: " << FLAGS_test_routing_result_file;
     return false;
   }

modules/planning/optimizer/qp_spline_path/BUILD

@@ -50,6 +50,9 @@ cc_test(
     srcs = [
         "qp_spline_path_optimizer_test.cc",
     ],
+    data = [
+        "//modules/planning:planning_testdata",
+    ],
     deps = [
         ":qp_spline_path",
         "@gtest//:main",

modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer_test.cc

@@ -18,6 +18,7 @@
 #include <utility>
 
 #include "gtest/gtest.h"
+#include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h"
 
 namespace apollo {
@@ -26,6 +27,8 @@ namespace planning {
 class QpSplinePathOptimizerTest : public ::testing::Test {
  public:
   virtual void SetUp() {
+    FLAGS_reference_line_smoother_config_file =
+        "modules/planning/testdata/conf/reference_line_smoother_config.pb.txt";
     qp_spline_path_optimizer_.reset(new QpSplinePathOptimizer("QP_PATH"));
   }
 

modules/planning/planning_test.cc

@@ -36,6 +36,8 @@ class PlanningTest : public ::testing::Test {
         "modules/planning/testdata/conf/planning_config.pb.txt";
 
     FLAGS_adapter_config_path = "modules/planning/testdata/conf/adapter.conf";
+    FLAGS_reference_line_smoother_config_file =
+        "modules/planning/testdata/conf/reference_line_smoother_config.pb.txt";
 
     FLAGS_enable_record_debug = false;
   }

modules/planning/reference_line/reference_line.cc

@@ -46,6 +46,16 @@ ReferenceLine::ReferenceLine(
       map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
           reference_points.begin(), reference_points.end()))) {}
 
+ReferenceLine::ReferenceLine(const MapPath& hdmap_path)
+    : map_path_(hdmap_path) {
+  for (const auto& point : hdmap_path.path_points()) {
+    DCHECK(!point.lane_waypoints().empty());
+    const auto& lane_waypoint = point.lane_waypoints()[0];
+    reference_points_.emplace_back(point, point.heading(), 0.0, 0.0,
+                                   lane_waypoint);
+  }
+}
+
 ReferenceLine::ReferenceLine(
     const std::vector<ReferencePoint>& reference_points,
     const std::vector<hdmap::LaneSegment>& lane_segments,

modules/planning/reference_line/reference_line.h

@@ -38,6 +38,7 @@ class ReferenceLine {
  public:
   ReferenceLine() = default;
   ReferenceLine(const std::vector<ReferencePoint>& reference_points);
+  ReferenceLine(const hdmap::Path& hdmap_path);
   ReferenceLine(const std::vector<ReferencePoint>& reference_points,
                 const std::vector<hdmap::LaneSegment>& lane_segments,
                 const double max_approximation_error);
@@ -80,6 +81,7 @@ class ReferenceLine {
                                         const double s1, const double x,
                                         const double y);
 
+ private:
   std::vector<ReferencePoint> reference_points_;
   hdmap::Path map_path_;
 };

modules/planning/reference_line/reference_line_smoother.cc

@@ -54,8 +54,9 @@ void ReferenceLineSmoother::Reset() {
 
 bool ReferenceLineSmoother::smooth(
     const ReferenceLine& raw_reference_line,
-    std::vector<ReferencePoint>* const smoothed_ref_line) {
+    ReferenceLine* const smoothed_reference_line) {
   Reset();
+  std::vector<ReferencePoint> ref_points;
   if (!sampling(raw_reference_line)) {
     AERROR << "Fail to sample reference line smoother points!";
     return false;
@@ -85,8 +86,8 @@ bool ReferenceLineSmoother::smooth(
   const double resolution =
       (end_t - start_t) / (smoother_config_.num_of_total_points() - 1);
   double t = start_t;
-  for (std::uint32_t i = 0; i < smoother_config_.num_of_total_points() &&
-       t < end_t; ++i) {
+  for (std::uint32_t i = 0;
+       i < smoother_config_.num_of_total_points() && t < end_t; ++i) {
     std::pair<double, double> xy = spline_solver_->spline()(t);
     const double heading = std::atan2(spline_solver_->spline().derivative_y(t),
                                       spline_solver_->spline().derivative_x(t));
@@ -108,19 +109,20 @@ bool ReferenceLineSmoother::smooth(
       return false;
     }
     ReferencePoint rlp = raw_reference_line.get_reference_point(s);
-    ReferencePoint new_rlp(common::math::Vec2d(xy.first, xy.second), heading,
-                           kappa, dkappa, rlp.lane_waypoints());
-    smoothed_ref_line->push_back(std::move(new_rlp));
+    ref_points.emplace_back(
+        ReferencePoint(common::math::Vec2d(xy.first, xy.second), heading, kappa,
+                       dkappa, rlp.lane_waypoints()));
     t = start_t + (i + 1) * resolution;
   }
+  *smoothed_reference_line = ReferenceLine(ref_points);
   return true;
 }
 
 bool ReferenceLineSmoother::sampling(const ReferenceLine& raw_reference_line) {
   const double length = raw_reference_line.length();
   const double resolution = length / smoother_config_.num_spline();
-  for (std::uint32_t i = 0; i <= smoother_config_.num_spline() &&
-       i * resolution <= length; ++i) {
+  for (std::uint32_t i = 0;
+       i <= smoother_config_.num_spline() && i * resolution <= length; ++i) {
     ReferencePoint rlp = raw_reference_line.get_reference_point(resolution * i);
     common::PathPoint path_point;
     path_point.set_x(rlp.x());

modules/planning/reference_line/reference_line_smoother.h

@@ -40,7 +40,7 @@ class ReferenceLineSmoother {
   bool Init(const std::string& config_file);
   void Init(const ReferenceLineSmootherConfig& refline_smooth_config);
   bool smooth(const ReferenceLine& raw_reference_line,
-              std::vector<ReferencePoint>* const smoothed_ref_line);
+              ReferenceLine* const smoothed_reference_line);
 
  private:
   void Reset();

modules/planning/reference_line/reference_line_smoother_test.cc

@@ -43,7 +43,8 @@ class ReferenceLineSmootherTest : public ::testing::Test {
              << map_file;
       return;
     }
-    smoother_.Init(config_);  // use the default value in config.
+    ReferenceLineSmootherConfig config;
+    smoother_.Init(config);  // use the default value in config.
 
     std::vector<ReferencePoint> ref_points;
     const auto& points = lane_info_ptr->points();
@@ -56,7 +57,6 @@ class ReferenceLineSmootherTest : public ::testing::Test {
   }
 
   const std::string map_file = "modules/planning/testdata/base_map.txt";
-  ReferenceLineSmootherConfig config_;
 
   hdmap::HDMap hdmap_;
   ReferenceLineSmoother smoother_;
@@ -66,9 +66,10 @@ class ReferenceLineSmootherTest : public ::testing::Test {
 };
 
 TEST_F(ReferenceLineSmootherTest, smooth) {
-  std::vector<ReferencePoint> smoothed_ref_points;
-  EXPECT_TRUE(smoother_.smooth(*reference_line_, &smoothed_ref_points));
-  EXPECT_FALSE(smoothed_ref_points.empty());
+  ReferenceLine smoothed_reference_line;
+  EXPECT_FLOAT_EQ(153.87421, reference_line_->length());
+  EXPECT_TRUE(smoother_.smooth(*reference_line_, &smoothed_reference_line));
+  EXPECT_FLOAT_EQ(153.84697, smoothed_reference_line.length());
 }
 
 }  // namespace planning