planning: remove unnecessary sl projection in smoother

modules/map/pnc_map/pnc_map.cc

@@ -465,6 +465,7 @@ void PncMap::CreatePathFromLaneSegments(const LaneSegments &segments,
     append_lane_to_points(segment.lane, segment.start_s, segment.end_s,
                           &points);
   }
+  remove_duplicates(&points);
   *path = hdmap::Path(points, segments, kTrajectoryApproximationMaxError);
 }
 

modules/planning/integration_tests/garage_test.cc

@@ -80,16 +80,16 @@ TEST_F(GarageTest, follow) {
 /*
  * test stop for destination
  */
-// TEST_F(GarageTest, stop_dest) {
-//  FLAGS_test_prediction_file =
-//      "modules/planning/testdata/garage_test/stop_dest_prediction.pb.txt";
-//  FLAGS_test_localization_file =
-//      "modules/planning/testdata/garage_test/stop_dest_localization.pb.txt";
-//  FLAGS_test_chassis_file =
-//      "modules/planning/testdata/garage_test/stop_dest_chassis.pb.txt";
-//  PlanningTestBase::SetUp();
-//  RUN_GOLDEN_TEST;
-//}
+TEST_F(GarageTest, stop_dest) {
+  FLAGS_test_prediction_file =
+      "modules/planning/testdata/garage_test/stop_dest_prediction.pb.txt";
+  FLAGS_test_localization_file =
+      "modules/planning/testdata/garage_test/stop_dest_localization.pb.txt";
+  FLAGS_test_chassis_file =
+      "modules/planning/testdata/garage_test/stop_dest_chassis.pb.txt";
+  PlanningTestBase::SetUp();
+  RUN_GOLDEN_TEST;
+}
 }  // namespace planning
 }  // namespace apollo
 

modules/planning/reference_line/reference_line_smoother.cc

@@ -87,7 +87,8 @@ bool ReferenceLineSmoother::smooth(
       (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) {
+       i < smoother_config_.num_of_total_points() && t < end_t;
+       ++i, t += resolution) {
     std::pair<double, double> xy = spline_solver_->spline()(t);
     const double heading = std::atan2(spline_solver_->spline().derivative_y(t),
                                       spline_solver_->spline().DerivativeX(t));
@@ -108,20 +109,13 @@ bool ReferenceLineSmoother::smooth(
       AERROR << "Get corresponding s failed!";
       return false;
     }
-
-    common::SLPoint ref_sl_point;
-    if (!raw_reference_line.xy_to_sl({xy.first, xy.second}, &ref_sl_point)) {
-      AERROR << "get sl point failed!" << std::endl;
-      return false;
-    }
-    ReferencePoint rlp =
-        raw_reference_line.get_reference_point(ref_sl_point.s());
+    ReferencePoint rlp = raw_reference_line.get_reference_point(s);
     ref_points.emplace_back(ReferencePoint(
         hdmap::MapPathPoint(common::math::Vec2d(xy.first, xy.second), heading,
                             rlp.lane_waypoints()),
         kappa, dkappa, 0.0, 0.0));
-    t = start_t + (i + 1) * resolution;
   }
+  ReferencePoint::remove_duplicates(&ref_points);
   *smoothed_reference_line = ReferenceLine(ref_points);
   return true;
 }

modules/planning/reference_line/reference_line_smoother_test.cc

@@ -73,7 +73,7 @@ TEST_F(ReferenceLineSmootherTest, smooth) {
   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());
+  EXPECT_FLOAT_EQ(154.16287, smoothed_reference_line.length());
 }
 
 }  // namespace planning

modules/planning/reference_line/reference_point.cc

@@ -29,6 +29,10 @@ namespace apollo {
 namespace planning {
 
 using apollo::common::PathPoint;
+namespace {
+// Minimum distance to remove duplicated points.
+const double kDuplicatedPointsEpsilon = 1e-7;
+}
 
 ReferencePoint::ReferencePoint(const MapPathPoint& map_path_point,
                                const double kappa, const double dkappa,
@@ -57,5 +61,20 @@ const std::string ReferencePoint::DebugString() const {
   // clang-format on
 }
 
+void ReferencePoint::remove_duplicates(std::vector<ReferencePoint>* points) {
+  CHECK_NOTNULL(points);
+  int count = 0;
+  const double limit = kDuplicatedPointsEpsilon * kDuplicatedPointsEpsilon;
+  for (size_t i = 0; i < points->size(); ++i) {
+    if (count == 0 ||
+        (*points)[i].DistanceSquareTo((*points)[count - 1]) > limit) {
+      (*points)[count++] = (*points)[i];
+    } else {
+      (*points)[count - 1].add_lane_waypoints((*points)[i].lane_waypoints());
+    }
+  }
+  points->resize(count);
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/reference_line/reference_point.h

@@ -47,6 +47,8 @@ class ReferencePoint : public hdmap::MapPathPoint {
 
   const std::string DebugString() const;
 
+  static void remove_duplicates(std::vector<ReferencePoint>* points);
+
  private:
   double kappa_ = 0.0;
   double dkappa_ = 0.0;