planning: refactor coding style

modules/planning/common/obstacle.cc

@@ -67,18 +67,23 @@ Obstacle::Obstacle(const std::string& id,
                    const PerceptionObstacle& perception_obstacle,
                    const prediction::Trajectory& trajectory)
     : Obstacle(id, perception_obstacle) {
-  has_trajectory_ = true;
   trajectory_ = trajectory;
   auto& trajectory_points = *trajectory_.mutable_trajectory_point();
   double cumulative_s = 0.0;
   if (trajectory_points.size() > 0) {
     trajectory_points[0].mutable_path_point()->set_s(0.0);
+    has_trajectory_ = true;
   }
   for (int i = 1; i < trajectory_points.size(); ++i) {
+    const auto& prev = trajectory_points[i - 1];
+    const auto& cur = trajectory_points[i];
+    if (prev.relative_time() >= cur.relative_time()) {
+      AERROR << "prediction time is not increasing."
+             << "current point: " << cur.ShortDebugString()
+             << "previous point: " << prev.ShortDebugString();
+    }
     cumulative_s +=
-        common::util::DistanceXY(trajectory_points[i - 1].path_point(),
-                                 trajectory_points[i].path_point());
-
+        common::util::DistanceXY(prev.path_point(), cur.path_point());
     trajectory_points[i].mutable_path_point()->set_s(cumulative_s);
   }
 }

modules/planning/reference_line/reference_line.h

@@ -91,11 +91,19 @@ class ReferenceLine {
               common::math::Vec2d* const xy_point) const;
   bool XYToSL(const common::math::Vec2d& xy_point,
               common::SLPoint* const sl_point) const;
+  template <class XYPoint>
+  bool XYToSL(const XYPoint& xy, common::SLPoint* const sl_point) const {
+    return XYToSL(common::math::Vec2d(xy.x(), xy.y()), sl_point);
+  }
 
   bool GetLaneWidth(const double s, double* const left_width,
                     double* const right_width) const;
   bool IsOnRoad(const common::SLPoint& sl_point) const;
   bool IsOnRoad(const common::math::Vec2d& vec2d_point) const;
+  template <class XYPoint>
+  bool IsOnRoad(const XYPoint& xy) const {
+    return IsOnRoad(common::math::Vec2d(xy.x(), xy.y()));
+  }
   bool IsOnRoad(const SLBoundary& sl_boundary) const;
 
   /**

modules/planning/tasks/dp_st_speed/dp_st_graph.cc

@@ -63,19 +63,19 @@ DpStGraph::DpStGraph(const ReferenceLine& reference_line,
     : reference_line_(reference_line),
       dp_st_speed_config_(dp_config),
       st_graph_data_(st_graph_data),
+      vehicle_param_(VehicleConfigHelper::GetConfig().vehicle_param()),
       adc_sl_boundary_(adc_sl_boundary),
       dp_st_cost_(dp_config),
       init_point_(st_graph_data.init_point()) {
   dp_st_speed_config_.set_total_path_length(
       std::fmin(dp_st_speed_config_.total_path_length(),
                 st_graph_data_.path_data_length()));
-  vehicle_param_ = VehicleConfigHelper::GetConfig().vehicle_param();
 }
 
 Status DpStGraph::Search(PathDecision* const path_decision,
                          SpeedData* const speed_data) {
   constexpr double kBounadryEpsilon = 1e-2;
-  for (auto boundary : st_graph_data_.st_boundaries()) {
+  for (const auto& boundary : st_graph_data_.st_boundaries()) {
     if (boundary->IsPointInBoundary({0.0, 0.0}) ||
         (std::fabs(boundary->min_t()) < kBounadryEpsilon &&
          std::fabs(boundary->min_s()) < kBounadryEpsilon)) {

modules/planning/tasks/dp_st_speed/dp_st_graph.h

@@ -83,7 +83,7 @@ class DpStGraph {
   const StGraphData& st_graph_data_;
 
   // vehicle configuration parameter
-  common::VehicleParam vehicle_param_;
+  const common::VehicleParam& vehicle_param_;
 
   const SLBoundary& adc_sl_boundary_;
 

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -49,6 +49,7 @@ using apollo::common::TrajectoryPoint;
 using apollo::common::VehicleParam;
 using apollo::common::math::Box2d;
 using apollo::common::math::Vec2d;
+using apollo::common::util::StrCat;
 
 namespace {
 constexpr double boundary_t_buffer = 0.1;
@@ -65,8 +66,7 @@ StBoundaryMapper::StBoundaryMapper(const SLBoundary& adc_sl_boundary,
       st_boundary_config_(config),
       reference_line_(reference_line),
       path_data_(path_data),
-      vehicle_param_(
-          common::VehicleConfigHelper::instance()->GetConfig().vehicle_param()),
+      vehicle_param_(common::VehicleConfigHelper::GetConfig().vehicle_param()),
       planning_distance_(planning_distance),
       planning_time_(planning_time) {}
 
@@ -95,10 +95,9 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
     StBoundary boundary;
     boundary.SetId(path_obstacle->Id());
     if (!path_obstacle->HasLongitudinalDecision()) {
-      const auto ret = MapWithoutDecision(path_obstacle);
-      if (!ret.ok()) {
-        std::string msg = common::util::StrCat(
-            "Fail to map obstacle ", path_obstacle->Id(), " without decision.");
+      if (!MapWithoutDecision(path_obstacle).ok()) {
+        std::string msg = StrCat("Fail to map obstacle ", path_obstacle->Id(),
+                                 " without decision.");
         AERROR << msg;
         return Status(ErrorCode::PLANNING_ERROR, msg);
       }
@@ -118,26 +117,21 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
                << adc_sl_boundary_.end_s();
         return Status(ErrorCode::PLANNING_ERROR, "invalid decision");
       }
-      if (!stop_obstacle) {
-        stop_obstacle = path_obstacle;
-        stop_decision = decision;
-        min_stop_s = stop_s;
-      } else if (stop_s < min_stop_s) {
+      if (stop_s < min_stop_s) {
         stop_obstacle = path_obstacle;
         min_stop_s = stop_s;
         stop_decision = decision;
       }
     } else if (decision.has_follow() || decision.has_overtake() ||
                decision.has_yield()) {
-      const auto ret = MapWithPredictionTrajectory(path_obstacle);
-      if (!ret.ok()) {
+      if (!MapWithPredictionTrajectory(path_obstacle).ok()) {
         AERROR << "Fail to map obstacle " << path_obstacle->Id()
                << " with decision: " << decision.DebugString();
         return Status(ErrorCode::PLANNING_ERROR,
                       "Fail to map overtake/yield decision");
       }
     } else {
-      ADEBUG << "No mapping for decision: " << decision.DebugString();
+      AWARN << "No mapping for decision: " << decision.DebugString();
     }
   }
 
@@ -276,20 +270,10 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
     }
     for (int i = 0; i < trajectory.trajectory_point_size(); ++i) {
       const auto& trajectory_point = trajectory.trajectory_point(i);
-      if (i > 0) {
-        const auto& pre_point = trajectory.trajectory_point(i - 1);
-        if (trajectory_point.relative_time() <= pre_point.relative_time()) {
-          AERROR << "Fail to map because prediction time is not increasing."
-                 << "current point: " << trajectory_point.ShortDebugString()
-                 << "previous point: " << pre_point.ShortDebugString();
-          return false;
-        }
-      }
-
       const Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
 
       double trajectory_point_time = trajectory_point.relative_time();
-      const double kNegtiveTimeThreshold = -1.0;
+      constexpr double kNegtiveTimeThreshold = -1.0;
       if (trajectory_point_time < kNegtiveTimeThreshold) {
         continue;
       }

modules/planning/tasks/st_graph/st_boundary_mapper.h

@@ -84,10 +84,10 @@ class StBoundaryMapper {
 
  private:
   const SLBoundary& adc_sl_boundary_;
-  StBoundaryConfig st_boundary_config_;
+  const StBoundaryConfig& st_boundary_config_;
   const ReferenceLine& reference_line_;
   const PathData& path_data_;
-  const apollo::common::VehicleParam vehicle_param_;
+  const apollo::common::VehicleParam& vehicle_param_;
   const double planning_distance_;
   const double planning_time_;
 };