Planning: Simple refactor on reference_line utils.

modules/planning/reference_line/reference_line.cc

@@ -40,7 +40,7 @@ namespace apollo {
 namespace planning {
 
 using MapPath = hdmap::Path;
-using SLPoint = apollo::common::SLPoint;
+using apollo::common::SLPoint;
 
 ReferenceLine::ReferenceLine(
     const std::vector<ReferencePoint>& reference_points)
@@ -164,7 +164,7 @@ ReferencePoint ReferenceLine::GetReferencePoint(const double x,
                      reference_points_[index_end], s1, s);
 }
 
-bool ReferenceLine::SLToXY(const common::SLPoint& sl_point,
+bool ReferenceLine::SLToXY(const SLPoint& sl_point,
                            common::math::Vec2d* const xy_point) const {
   CHECK_NOTNULL(xy_point);
   if (map_path_.num_points() < 2) {
@@ -180,7 +180,7 @@ bool ReferenceLine::SLToXY(const common::SLPoint& sl_point,
 }
 
 bool ReferenceLine::XYToSL(const common::math::Vec2d& xy_point,
-                           common::SLPoint* const sl_point) const {
+                           SLPoint* const sl_point) const {
   DCHECK_NOTNULL(sl_point);
   double s = 0;
   double l = 0;
@@ -248,7 +248,7 @@ bool ReferenceLine::GetLaneWidth(const double s, double* const left_width,
   return map_path_.GetWidth(s, left_width, right_width);
 }
 
-bool ReferenceLine::IsOnRoad(const common::SLPoint& sl_point) const {
+bool ReferenceLine::IsOnRoad(const SLPoint& sl_point) const {
   if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) {
     return false;
   }
@@ -275,7 +275,7 @@ bool ReferenceLine::GetSLBoundary(const common::math::Box2d& box,
   std::vector<common::math::Vec2d> corners;
   box.GetAllCorners(&corners);
   for (const auto& point : corners) {
-    common::SLPoint sl_point;
+    SLPoint sl_point;
     if (!XYToSL(point, &sl_point)) {
       AERROR << "failed to get projection for point: " << point.DebugString()
              << " on reference line.";

modules/planning/reference_line/reference_line_smoother.cc

@@ -180,7 +180,7 @@ bool ReferenceLineSmoother::ApplyConstraint(
     xy_points.emplace_back(path_points[i].x(), path_points[i].y());
   }
 
-  constexpr double kFixedBoundLimit = 0.01;
+  static constexpr double kFixedBoundLimit = 0.01;
   if (longitidinal_bound.size() > 0) {
     longitidinal_bound.front() = kFixedBoundLimit;
     longitidinal_bound.back() = kFixedBoundLimit;
@@ -196,15 +196,15 @@ bool ReferenceLineSmoother::ApplyConstraint(
   CHECK_EQ(evaluated_t.size(), longitidinal_bound.size());
   CHECK_EQ(evaluated_t.size(), lateral_bound.size());
 
-  if (!spline_solver_->mutable_constraint()->Add2dBoundary(
+  auto* spline_constraint = spline_solver_->mutable_constraint();
+  if (!spline_constraint->Add2dBoundary(
           evaluated_t, headings, xy_points, longitidinal_bound,
           lateral_bound)) {
     AERROR << "Add 2d boundary constraint failed";
     return false;
   }
 
-  if (!spline_solver_->mutable_constraint()
-           ->AddThirdDerivativeSmoothConstraint()) {
+  if (!spline_constraint->AddThirdDerivativeSmoothConstraint()) {
     AERROR << "Add jointness constraint failed";
     return false;
   }
@@ -246,7 +246,7 @@ bool ReferenceLineSmoother::ExtractEvaluatedPoints(
       AERROR << "get s from " << t << " failed";
       return false;
     }
-    ReferencePoint rlp = raw_reference_line.GetReferencePoint(s);
+    const ReferencePoint rlp = raw_reference_line.GetReferencePoint(s);
     common::PathPoint path_point;
     path_point.set_x(rlp.x());
     path_point.set_y(rlp.y());

modules/planning/reference_line/reference_line_smoother_test.cc

@@ -21,7 +21,7 @@
 
 #include "modules/common/math/vec2d.h"
 #include "modules/map/hdmap/hdmap.h"
-#include "modules/map/hdmap/hdmap_common.h"
+#include "modules/map/hdmap/hdmap_util.h"
 #include "modules/planning/proto/reference_line_smoother_config.pb.h"
 #include "modules/planning/reference_line/reference_line.h"
 #include "modules/planning/reference_line/reference_line_smoother.h"
@@ -34,13 +34,10 @@ class ReferenceLineSmootherTest : public ::testing::Test {
  public:
   virtual void SetUp() {
     hdmap_.LoadMapFromFile(map_file);
-    const std::string lane_id_str = "1_-1";
-    hdmap::Id lane_id;
-    lane_id.set_id(lane_id_str);
-    lane_info_ptr = hdmap_.GetLaneById(lane_id);
+    const std::string lane_id = "1_-1";
+    lane_info_ptr = hdmap_.GetLaneById(hdmap::MakeMapId(lane_id));
     if (!lane_info_ptr) {
-      AERROR << "failed to find lane " << lane_id_str << " from map "
-             << map_file;
+      AERROR << "failed to find lane " << lane_id << " from map " << map_file;
       return;
     }
     ReferenceLineSmootherConfig config;

modules/planning/reference_line/reference_point.cc

@@ -52,7 +52,7 @@ double ReferencePoint::lower_bound() const { return lower_bound_; }
 
 double ReferencePoint::upper_bound() const { return upper_bound_; }
 
-const std::string ReferencePoint::DebugString() const {
+std::string ReferencePoint::DebugString() const {
   // clang-format off
   return apollo::common::util::StrCat("{x: ", std::fixed, x(),
          ", " "y: ", y(), ", " "theta: ", heading(), ", " "kappa: ",

modules/planning/reference_line/reference_point.h

@@ -45,7 +45,7 @@ class ReferencePoint : public hdmap::MapPathPoint {
   double lower_bound() const;
   double upper_bound() const;
 
-  const std::string DebugString() const;
+  std::string DebugString() const;
 
   static void RemoveDuplicates(std::vector<ReferencePoint>* points);
 

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -254,9 +254,8 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
   DCHECK(lower_points->empty());
   DCHECK_GT(path_points.size(), 0);
 
-  if (path_points.size() == 0) {
-    AERROR << "Too few points in path_data_.discretized_path(); size = "
-           << path_points.size();
+  if (path_points.empty()) {
+    AERROR << "No points in path_data_.discretized_path().";
     return false;
   }
 
@@ -313,9 +312,8 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
       }
 
       const double step_length = vehicle_param_.front_edge_to_center();
-      double path_s = 0.0;
-
-      while (path_s < discretized_path.Length()) {
+      for (double path_s = 0.0; path_s < discretized_path.Length();
+           path_s += step_length) {
         const auto curr_adc_path_point =
             discretized_path.EvaluateUsingLinearApproximation(
                 path_s + discretized_path.StartPoint().s());
@@ -373,7 +371,6 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
           }
           break;
         }
-        path_s += step_length;
       }
     }
   }

modules/planning/tasks/st_graph/st_graph_data_test.cc

@@ -43,19 +43,19 @@ TEST(StGraphDataTest, basic_test) {
   traj_point.set_relative_time(1010.022);
 
   SpeedLimit speed_limit;
-  StGraphData st_graph_data_2(boundary_vec, traj_point, speed_limit, 100.0);
-  EXPECT_EQ(st_graph_data_2.st_boundaries().size(), 1);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().path_point().x(), 1.1);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().path_point().y(), 2.1);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().path_point().z(), 0.0);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().path_point().theta(), 0.2);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().path_point().kappa(), 0.02);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().path_point().dkappa(), 0.123);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().path_point().ddkappa(), 0.003);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().v(), 10.001);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().a(), 1.022);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.init_point().relative_time(), 1010.022);
-  EXPECT_DOUBLE_EQ(st_graph_data_2.path_data_length(), 100.0);
+  StGraphData st_graph_data(boundary_vec, traj_point, speed_limit, 100.0);
+  EXPECT_EQ(st_graph_data.st_boundaries().size(), 1);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().path_point().x(), 1.1);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().path_point().y(), 2.1);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().path_point().z(), 0.0);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().path_point().theta(), 0.2);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().path_point().kappa(), 0.02);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().path_point().dkappa(), 0.123);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().path_point().ddkappa(), 0.003);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().v(), 10.001);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().a(), 1.022);
+  EXPECT_DOUBLE_EQ(st_graph_data.init_point().relative_time(), 1010.022);
+  EXPECT_DOUBLE_EQ(st_graph_data.path_data_length(), 100.0);
 }
 
 }  // namespace planning