Planning: used float in dp_poly_path and dp_st_graph.

modules/common/util/util.cc

@@ -88,20 +88,6 @@ PathPoint MakePathPoint(const double x, const double y, const double z,
   return path_point;
 }
 
-void uniform_slice(double start, double end, uint32_t num,
-                   std::vector<double>* sliced) {
-  if (!sliced || num == 0) {
-    return;
-  }
-  const double delta = (end - start) / num;
-  sliced->resize(num + 1);
-  double s = start;
-  for (uint32_t i = 0; i < num; ++i, s += delta) {
-    sliced->at(i) = s;
-  }
-  sliced->at(num) = end;
-}
-
 PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint& p1,
                                             const PathPoint& p2,
                                             const double w1, const double w2) {

modules/common/util/util.h

@@ -102,8 +102,20 @@ PathPoint MakePathPoint(const double x, const double y, const double z,
  * the result sliced will contain the n + 1 points that slices the provided
  * segment. `start` and `end` will be the first and last element in `sliced`.
  */
-void uniform_slice(double start, double end, uint32_t num,
-                   std::vector<double>* sliced);
+template <typename T>
+void uniform_slice(const T start, const T end, uint32_t num,
+                   std::vector<T>* sliced) {
+  if (!sliced || num == 0) {
+    return;
+  }
+  const T delta = (end - start) / num;
+  sliced->resize(num + 1);
+  T s = start;
+  for (uint32_t i = 0; i < num; ++i, s += delta) {
+    sliced->at(i) = s;
+  }
+  sliced->at(num) = end;
+}
 
 template <typename Container>
 typename Container::value_type MaxElement(const Container& elements) {

modules/planning/tasks/dp_poly_path/comparable_cost.h

@@ -34,8 +34,8 @@ class ComparableCost {
  public:
   ComparableCost() = default;
   ComparableCost(const bool has_collision, const bool out_of_boundary,
-                 const bool out_of_lane, const double safety_cost_,
-                 const double smoothness_cost_)
+                 const bool out_of_lane, const float safety_cost_,
+                 const float smoothness_cost_)
       : safety_cost(safety_cost_), smoothness_cost(smoothness_cost_) {
     cost_items[HAS_COLLISION] = has_collision;
     cost_items[OUT_OF_BOUNDARY] = out_of_boundary;
@@ -60,9 +60,9 @@ class ComparableCost {
       }
     }
 
-    constexpr double kEpsilon = 1e-12;
-    const double diff = safety_cost + smoothness_cost - other.safety_cost -
-                        other.smoothness_cost;
+    constexpr float kEpsilon = 1e-12;
+    const float diff = safety_cost + smoothness_cost - other.safety_cost -
+                       other.smoothness_cost;
     if (std::fabs(diff) < kEpsilon) {
       return 0;
     } else if (diff > 0) {
@@ -73,7 +73,8 @@ class ComparableCost {
   }
   ComparableCost operator+(const ComparableCost &other) {
     ComparableCost lhs = *this;
-    return lhs += other;
+    lhs += other;
+    return lhs;
   }
   ComparableCost &operator+=(const ComparableCost &other) {
     for (size_t i = 0; i < cost_items.size(); ++i) {
@@ -110,9 +111,9 @@ class ComparableCost {
   std::array<bool, 3> cost_items = {{false, false, false}};
 
   // cost from distance to obstacles or boundaries
-  double safety_cost = 0.0;
+  float safety_cost = 0.0f;
   // cost from deviation from lane center, path curvature etc
-  double smoothness_cost = 0.0;
+  float smoothness_cost = 0.0f;
 };
 
 }  // namespace planning

modules/planning/tasks/dp_poly_path/comparable_cost_test.cc

@@ -23,8 +23,8 @@ namespace planning {
 
 TEST(ComparableCost, simple) {
   ComparableCost cc;
-  EXPECT_DOUBLE_EQ(cc.safety_cost, 0.0);
-  EXPECT_DOUBLE_EQ(cc.smoothness_cost, 0.0);
+  EXPECT_FLOAT_EQ(cc.safety_cost, 0.0);
+  EXPECT_FLOAT_EQ(cc.smoothness_cost, 0.0);
   EXPECT_FALSE(cc.cost_items[ComparableCost::HAS_COLLISION]);
   EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
   EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_LANE]);
@@ -39,8 +39,8 @@ TEST(ComparableCost, add_cost) {
   EXPECT_TRUE(cc.cost_items[ComparableCost::HAS_COLLISION]);
   EXPECT_FALSE(cc.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
   EXPECT_TRUE(cc.cost_items[ComparableCost::OUT_OF_LANE]);
-  EXPECT_DOUBLE_EQ(cc.safety_cost, 16.22);
-  EXPECT_DOUBLE_EQ(cc.smoothness_cost, 5.96);
+  EXPECT_FLOAT_EQ(cc.safety_cost, 16.22);
+  EXPECT_FLOAT_EQ(cc.smoothness_cost, 5.96);
 
   EXPECT_TRUE(cc1 > cc2);
 
@@ -49,8 +49,8 @@ TEST(ComparableCost, add_cost) {
   EXPECT_TRUE(cc1.cost_items[ComparableCost::HAS_COLLISION]);
   EXPECT_FALSE(cc1.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
   EXPECT_TRUE(cc1.cost_items[ComparableCost::OUT_OF_LANE]);
-  EXPECT_DOUBLE_EQ(cc1.safety_cost, 16.22);
-  EXPECT_DOUBLE_EQ(cc1.smoothness_cost, 5.96);
+  EXPECT_FLOAT_EQ(cc1.safety_cost, 16.22);
+  EXPECT_FLOAT_EQ(cc1.smoothness_cost, 5.96);
 
   ComparableCost cc3(true, false, false, 10.12, 2.51);
   ComparableCost cc4(false, true, true, 6.1, 3.45);
@@ -67,11 +67,33 @@ TEST(ComparableCost, add_cost) {
   EXPECT_FALSE(cc7.cost_items[ComparableCost::HAS_COLLISION]);
   EXPECT_TRUE(cc7.cost_items[ComparableCost::OUT_OF_BOUNDARY]);
   EXPECT_TRUE(cc7.cost_items[ComparableCost::OUT_OF_LANE]);
-  EXPECT_DOUBLE_EQ(cc7.safety_cost, 16.22);
-  EXPECT_DOUBLE_EQ(cc7.smoothness_cost, 5.96);
+  EXPECT_FLOAT_EQ(cc7.safety_cost, 16.22);
+  EXPECT_FLOAT_EQ(cc7.smoothness_cost, 5.96);
 
   EXPECT_TRUE(cc5 < cc6);
 }
 
+TEST(ComparableCost, compare_to) {
+  ComparableCost cc1(true, false, false, 10.12, 2.51);
+  ComparableCost cc2(false, false, true, 6.1, 3.45);
+  EXPECT_TRUE(cc1 > cc2);
+
+  ComparableCost cc3(false, true, false, 10.12, 2.51);
+  ComparableCost cc4(false, false, false, 6.1, 3.45);
+  EXPECT_TRUE(cc3 > cc4);
+
+  ComparableCost cc5(false, false, true, 10.12, 2.51);
+  ComparableCost cc6(false, false, false, 6.1, 3.45);
+  EXPECT_TRUE(cc5 > cc6);
+
+  ComparableCost cc7(false, false, false, 10.12, 2.51);
+  ComparableCost cc8(false, false, false, 6.1, 3.45);
+  EXPECT_TRUE(cc7 > cc8);
+
+  ComparableCost cc9(false, false, false, 0.12, 2.51);
+  ComparableCost cc10(false, false, false, 6.1, 3.45);
+  EXPECT_TRUE(cc9 < cc10);
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/tasks/dp_poly_path/dp_road_graph.cc

@@ -79,20 +79,20 @@ bool DPRoadGraph::FindPathTunnel(
     return false;
   }
   std::vector<common::FrenetFramePoint> frenet_path;
-  double accumulated_s = init_sl_point_.s();
-  const double path_resolution = config_.path_resolution();
+  float accumulated_s = init_sl_point_.s();
+  const float path_resolution = config_.path_resolution();
 
   for (std::size_t i = 1; i < min_cost_path.size(); ++i) {
     const auto &prev_node = min_cost_path[i - 1];
     const auto &cur_node = min_cost_path[i];
 
-    const double path_length = cur_node.sl_point.s() - prev_node.sl_point.s();
-    double current_s = 0.0;
+    const float path_length = cur_node.sl_point.s() - prev_node.sl_point.s();
+    float current_s = 0.0;
     const auto &curve = cur_node.min_cost_curve;
     while (current_s + path_resolution / 2.0 < path_length) {
-      const double l = curve.Evaluate(0, current_s);
-      const double dl = curve.Evaluate(1, current_s);
-      const double ddl = curve.Evaluate(2, current_s);
+      const float l = curve.Evaluate(0, current_s);
+      const float dl = curve.Evaluate(1, current_s);
+      const float ddl = curve.Evaluate(2, current_s);
       common::FrenetFramePoint frenet_frame_point;
       frenet_frame_point.set_s(accumulated_s + current_s);
       frenet_frame_point.set_l(l);
@@ -218,8 +218,8 @@ void DPRoadGraph::UpdateNode(const std::list<DPRoadGraphNode> &prev_nodes,
   for (const auto &prev_dp_node : prev_nodes) {
     const auto &prev_sl_point = prev_dp_node.sl_point;
     const auto &cur_point = cur_node->sl_point;
-    double init_dl = 0.0;
-    double init_ddl = 0.0;
+    float init_dl = 0.0;
+    float init_ddl = 0.0;
     if (level == 1) {
       init_dl = init_frenet_frame_point_.dl();
       init_ddl = init_frenet_frame_point_.ddl();
@@ -260,26 +260,26 @@ bool DPRoadGraph::SamplePathWaypoints(
     std::vector<std::vector<common::SLPoint>> *const points) {
   CHECK_NOTNULL(points);
 
-  const double kMinSampleDistance = 40.0;
-  const double total_length = std::fmin(
+  const float kMinSampleDistance = 40.0;
+  const float total_length = std::fmin(
       init_sl_point_.s() + std::fmax(init_point.v() * 8.0, kMinSampleDistance),
       reference_line_.Length());
 
-  constexpr double kSamplePointLookForwardTime = 4.0;
-  const double step_length =
+  constexpr float kSamplePointLookForwardTime = 4.0;
+  const float step_length =
       common::math::Clamp(init_point.v() * kSamplePointLookForwardTime,
                           config_.step_length_min(), config_.step_length_max());
-  const double level_distance = (init_point.v() > FLAGS_max_stop_speed) ?
-      step_length : step_length / 2.0;
-  double accumulated_s = init_sl_point_.s();
-  double prev_s = accumulated_s;
+  const float level_distance =
+      (init_point.v() > FLAGS_max_stop_speed) ? step_length : step_length / 2.0;
+  float accumulated_s = init_sl_point_.s();
+  float prev_s = accumulated_s;
   for (std::size_t i = 0; accumulated_s < total_length; ++i) {
     accumulated_s += level_distance;
     if (accumulated_s + level_distance / 2.0 > total_length) {
       accumulated_s = total_length;
     }
-    const double s = std::fmin(accumulated_s, total_length);
-    constexpr double kMinAllowedSampleStep = 1.0;
+    const float s = std::fmin(accumulated_s, total_length);
+    constexpr float kMinAllowedSampleStep = 1.0;
     if (std::fabs(s - prev_s) < kMinAllowedSampleStep) {
       continue;
     }
@@ -289,26 +289,26 @@ bool DPRoadGraph::SamplePathWaypoints(
     double right_width = 0.0;
     reference_line_.GetLaneWidth(s, &left_width, &right_width);
 
-    constexpr double kBoundaryBuff = 0.20;
+    constexpr float kBoundaryBuff = 0.20;
     const auto &vehicle_config =
         common::VehicleConfigHelper::instance()->GetConfig();
-    const double half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
-    const double eff_right_width = right_width - half_adc_width - kBoundaryBuff;
-    const double eff_left_width = left_width - half_adc_width - kBoundaryBuff;
+    const float half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
+    const float eff_right_width = right_width - half_adc_width - kBoundaryBuff;
+    const float eff_left_width = left_width - half_adc_width - kBoundaryBuff;
 
     const size_t num_sample_per_level =
         FLAGS_use_navigation_mode ? config_.navigator_sample_num_each_level()
                                   : config_.sample_points_num_each_level();
 
-    double kDefaultUnitL = 1.2 / (num_sample_per_level - 1);
+    float kDefaultUnitL = 1.2 / (num_sample_per_level - 1);
     if (reference_line_info_.IsChangeLanePath() && !IsSafeForLaneChange()) {
       kDefaultUnitL = 1.0;
     }
-    const double sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
-    double sample_right_boundary = -eff_right_width;
-    double sample_left_boundary = eff_left_width;
+    const float sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
+    float sample_right_boundary = -eff_right_width;
+    float sample_left_boundary = eff_left_width;
 
-    const double kLargeDeviationL = 1.75;
+    const float kLargeDeviationL = 1.75;
     if (reference_line_info_.IsChangeLanePath() ||
         std::fabs(init_sl_point_.l()) > kLargeDeviationL) {
       sample_right_boundary = std::fmin(-eff_right_width, init_sl_point_.l());
@@ -324,7 +324,7 @@ bool DPRoadGraph::SamplePathWaypoints(
       }
     }
 
-    std::vector<double> sample_l;
+    std::vector<float> sample_l;
     if (reference_line_info_.IsChangeLanePath() && !IsSafeForLaneChange()) {
       sample_l.push_back(reference_line_info_.OffsetToOtherReferenceLine());
     } else {
@@ -351,8 +351,8 @@ bool DPRoadGraph::SamplePathWaypoints(
     std::vector<common::SLPoint> level_points;
     planning_internal::SampleLayerDebug sample_layer_debug;
     for (size_t j = 0; j < sample_l.size(); ++j) {
-      const double l = sample_l[j];
-      constexpr double kResonateDistance = 1e-3;
+      const float l = sample_l[j];
+      constexpr float kResonateDistance = 1e-3;
       common::SLPoint sl;
       if (j % 2 == 0 ||
           total_length - accumulated_s < 2.0 * kResonateDistance) {
@@ -392,22 +392,26 @@ bool DPRoadGraph::IsSafeForLaneChange() {
     const auto &sl_boundary = path_obstacle->PerceptionSLBoundary();
     const auto &adc_sl_boundary = reference_line_info_.AdcSlBoundary();
 
-    constexpr double kLateralShift = 2.5;
+    constexpr float kLateralShift = 2.5;
     if (sl_boundary.start_l() < -kLateralShift ||
         sl_boundary.end_l() > kLateralShift) {
       continue;
     }
 
-    constexpr double kSafeTime = 3.0;
-    constexpr double kForwardMinSafeDistance = 6.0;
-    constexpr double kBackwardMinSafeDistance = 8.0;
-
-    const double kForwardSafeDistance = std::max(
-        kForwardMinSafeDistance,
-        (init_point_.v() - path_obstacle->obstacle()->Speed()) * kSafeTime);
-    const double kBackwardSafeDistance = std::max(
-        kBackwardMinSafeDistance,
-        (path_obstacle->obstacle()->Speed() - init_point_.v()) * kSafeTime);
+    constexpr float kSafeTime = 3.0;
+    constexpr float kForwardMinSafeDistance = 6.0;
+    constexpr float kBackwardMinSafeDistance = 8.0;
+
+    const float kForwardSafeDistance =
+        std::max(kForwardMinSafeDistance,
+                 static_cast<float>(
+                     (init_point_.v() - path_obstacle->obstacle()->Speed()) *
+                     kSafeTime));
+    const float kBackwardSafeDistance =
+        std::max(kBackwardMinSafeDistance,
+                 static_cast<float>(
+                     (path_obstacle->obstacle()->Speed() - init_point_.v()) *
+                     kSafeTime));
     if (sl_boundary.end_s() >
             adc_sl_boundary.start_s() - kBackwardSafeDistance &&
         sl_boundary.start_s() <
@@ -430,20 +434,20 @@ bool DPRoadGraph::CalculateFrenetPoint(
   frenet_frame_point->set_s(sl_point.s());
   frenet_frame_point->set_l(sl_point.l());
 
-  const double theta = traj_point.path_point().theta();
-  const double kappa = traj_point.path_point().kappa();
-  const double l = frenet_frame_point->l();
+  const float theta = traj_point.path_point().theta();
+  const float kappa = traj_point.path_point().kappa();
+  const float l = frenet_frame_point->l();
 
   ReferencePoint ref_point;
   ref_point = reference_line_.GetReferencePoint(frenet_frame_point->s());
 
-  const double theta_ref = ref_point.heading();
-  const double kappa_ref = ref_point.kappa();
-  const double dkappa_ref = ref_point.dkappa();
+  const float theta_ref = ref_point.heading();
+  const float kappa_ref = ref_point.kappa();
+  const float dkappa_ref = ref_point.dkappa();
 
-  const double dl = CartesianFrenetConverter::CalculateLateralDerivative(
+  const float dl = CartesianFrenetConverter::CalculateLateralDerivative(
       theta_ref, theta, l, kappa_ref);
-  const double ddl =
+  const float ddl =
       CartesianFrenetConverter::CalculateSecondOrderLateralDerivative(
           theta_ref, theta, kappa_ref, kappa, dkappa_ref, l);
   frenet_frame_point->set_dl(dl);
@@ -452,9 +456,9 @@ bool DPRoadGraph::CalculateFrenetPoint(
 }
 
 bool DPRoadGraph::IsValidCurve(const QuinticPolynomialCurve1d &curve) const {
-  constexpr double kMaxLateralDistance = 20.0;
-  for (double s = 0.0; s < curve.ParamLength(); s += 2.0) {
-    const double l = curve.Evaluate(0, s);
+  constexpr float kMaxLateralDistance = 20.0;
+  for (float s = 0.0; s < curve.ParamLength(); s += 2.0) {
+    const float l = curve.Evaluate(0, s);
     if (std::fabs(l) > kMaxLateralDistance) {
       return false;
     }
@@ -464,7 +468,7 @@ bool DPRoadGraph::IsValidCurve(const QuinticPolynomialCurve1d &curve) const {
 
 void DPRoadGraph::GetCurveCost(TrajectoryCost trajectory_cost,
                                const QuinticPolynomialCurve1d &curve,
-                               const double start_s, const double end_s,
+                               const float start_s, const float end_s,
                                const uint32_t curr_level,
                                const uint32_t total_level,
                                ComparableCost *cost) {

modules/planning/tasks/dp_poly_path/dp_road_graph.h

@@ -88,8 +88,8 @@ class DPRoadGraph {
     common::SLPoint sl_point;
     const DPRoadGraphNode *min_cost_prev_node = nullptr;
     ComparableCost min_cost = {true, true, true,
-                               std::numeric_limits<double>::infinity(),
-                               std::numeric_limits<double>::infinity()};
+                               std::numeric_limits<float>::infinity(),
+                               std::numeric_limits<float>::infinity()};
     QuinticPolynomialCurve1d min_cost_curve;
   };
 
@@ -107,8 +107,8 @@ class DPRoadGraph {
   bool IsValidCurve(const QuinticPolynomialCurve1d &curve) const;
 
   void GetCurveCost(TrajectoryCost trajectory_cost,
-                    const QuinticPolynomialCurve1d &curve, const double start_s,
-                    const double end_s, const uint32_t curr_level,
+                    const QuinticPolynomialCurve1d &curve, const float start_s,
+                    const float end_s, const uint32_t curr_level,
                     const uint32_t total_level, ComparableCost *cost);
 
   void UpdateNode(const std::list<DPRoadGraphNode> &prev_nodes,

modules/planning/tasks/dp_poly_path/trajectory_cost.cc

@@ -52,7 +52,7 @@ TrajectoryCost::TrajectoryCost(
       vehicle_param_(vehicle_param),
       heuristic_speed_data_(heuristic_speed_data),
       init_sl_point_(init_sl_point) {
-  const double total_time =
+  const float total_time =
       std::min(heuristic_speed_data_.TotalTime(), FLAGS_prediction_total_time);
 
   num_of_time_stamps_ = static_cast<uint32_t>(
@@ -64,9 +64,9 @@ TrajectoryCost::TrajectoryCost(
     }
     auto sl_boundary = ptr_path_obstacle->PerceptionSLBoundary();
 
-    const double adc_left_l =
+    const float adc_left_l =
         init_sl_point_.l() + vehicle_param_.left_edge_to_center();
-    const double adc_right_l =
+    const float adc_right_l =
         init_sl_point_.l() - vehicle_param_.right_edge_to_center();
 
     if (adc_left_l + FLAGS_lateral_ignore_buffer < sl_boundary.start_l() ||
@@ -94,7 +94,7 @@ TrajectoryCost::TrajectoryCost(
             ptr_obstacle->GetPointAtTime(t * config.eval_time_interval());
 
         Box2d obstacle_box = ptr_obstacle->GetBoundingBox(trajectory_point);
-        constexpr double kBuff = 0.5;
+        constexpr float kBuff = 0.5;
         Box2d expanded_obstacle_box =
             Box2d(obstacle_box.center(), obstacle_box.heading(),
                   obstacle_box.length() + kBuff, obstacle_box.width() + kBuff);
@@ -106,24 +106,24 @@ TrajectoryCost::TrajectoryCost(
 }
 
 ComparableCost TrajectoryCost::CalculatePathCost(
-    const QuinticPolynomialCurve1d &curve, const double start_s,
-    const double end_s, const uint32_t curr_level, const uint32_t total_level) {
+    const QuinticPolynomialCurve1d &curve, const float start_s,
+    const float end_s, const uint32_t curr_level, const uint32_t total_level) {
   ComparableCost cost;
-  double path_cost = 0.0;
-  std::function<double(const double)> quasi_softmax = [this](const double x) {
-    const double l0 = this->config_.path_l_cost_param_l0();
-    const double b = this->config_.path_l_cost_param_b();
-    const double k = this->config_.path_l_cost_param_k();
+  float path_cost = 0.0;
+  std::function<float(const float)> quasi_softmax = [this](const float x) {
+    const float l0 = this->config_.path_l_cost_param_l0();
+    const float b = this->config_.path_l_cost_param_b();
+    const float k = this->config_.path_l_cost_param_k();
     return (b + std::exp(-k * (x - l0))) / (1.0 + std::exp(-k * (x - l0)));
   };
 
   const auto &vehicle_config =
       common::VehicleConfigHelper::instance()->GetConfig();
-  const double width = vehicle_config.vehicle_param().width();
+  const float width = vehicle_config.vehicle_param().width();
 
-  for (double curve_s = 0.0; curve_s < (end_s - start_s);
+  for (float curve_s = 0.0; curve_s < (end_s - start_s);
        curve_s += config_.path_resolution()) {
-    const double l = curve.Evaluate(0, curve_s);
+    const float l = curve.Evaluate(0, curve_s);
 
     path_cost += l * l * config_.path_l_cost() * quasi_softmax(std::fabs(l));
 
@@ -131,22 +131,22 @@ ComparableCost TrajectoryCost::CalculatePathCost(
     double right_width = 0.0;
     reference_line_->GetLaneWidth(curve_s + start_s, &left_width, &right_width);
 
-    constexpr double kBuff = 0.2;
+    constexpr float kBuff = 0.2;
     if (!is_change_lane_path_ && (l + width / 2.0 + kBuff > left_width ||
                                   l - width / 2.0 - kBuff < -right_width)) {
       cost.cost_items[ComparableCost::OUT_OF_BOUNDARY] = true;
     }
 
-    const double dl = std::fabs(curve.Evaluate(1, curve_s));
+    const float dl = std::fabs(curve.Evaluate(1, curve_s));
     path_cost += dl * dl * config_.path_dl_cost();
 
-    const double ddl = std::fabs(curve.Evaluate(2, curve_s));
+    const float ddl = std::fabs(curve.Evaluate(2, curve_s));
     path_cost += ddl * ddl * config_.path_ddl_cost();
   }
   path_cost *= config_.path_resolution();
 
   if (curr_level == total_level) {
-    const double end_l = curve.Evaluate(0, end_s - start_s);
+    const float end_l = curve.Evaluate(0, end_s - start_s);
     path_cost +=
         std::sqrt(end_l - init_sl_point_.l() / 2.0) * config_.path_end_l_cost();
   }
@@ -155,12 +155,12 @@ ComparableCost TrajectoryCost::CalculatePathCost(
 }
 
 ComparableCost TrajectoryCost::CalculateStaticObstacleCost(
-    const QuinticPolynomialCurve1d &curve, const double start_s,
-    const double end_s) {
+    const QuinticPolynomialCurve1d &curve, const float start_s,
+    const float end_s) {
   ComparableCost obstacle_cost;
-  for (double curr_s = start_s; curr_s <= end_s;
+  for (float curr_s = start_s; curr_s <= end_s;
        curr_s += config_.path_resolution()) {
-    const double curr_l = curve.Evaluate(0, curr_s - start_s);
+    const float curr_l = curve.Evaluate(0, curr_s - start_s);
     for (const auto &obs_sl_boundary : static_obstacle_sl_boundaries_) {
       obstacle_cost += GetCostFromObsSL(curr_s, curr_l, obs_sl_boundary);
     }
@@ -170,15 +170,15 @@ ComparableCost TrajectoryCost::CalculateStaticObstacleCost(
 }
 
 ComparableCost TrajectoryCost::CalculateDynamicObstacleCost(
-    const QuinticPolynomialCurve1d &curve, const double start_s,
-    const double end_s) const {
+    const QuinticPolynomialCurve1d &curve, const float start_s,
+    const float end_s) const {
   ComparableCost obstacle_cost;
-  double time_stamp = 0.0;
+  float time_stamp = 0.0;
   for (size_t index = 0; index < num_of_time_stamps_;
        ++index, time_stamp += config_.eval_time_interval()) {
     common::SpeedPoint speed_point;
     heuristic_speed_data_.EvaluateByTime(time_stamp, &speed_point);
-    double ref_s = speed_point.s() + init_sl_point_.s();
+    float ref_s = speed_point.s() + init_sl_point_.s();
     if (ref_s < start_s) {
       continue;
     }
@@ -186,9 +186,9 @@ ComparableCost TrajectoryCost::CalculateDynamicObstacleCost(
       break;
     }
 
-    const double s = ref_s - start_s;  // s on spline curve
-    const double l = curve.Evaluate(0, s);
-    const double dl = curve.Evaluate(1, s);
+    const float s = ref_s - start_s;  // s on spline curve
+    const float l = curve.Evaluate(0, s);
+    const float dl = curve.Evaluate(1, s);
 
     const common::SLPoint sl = common::util::MakeSLPoint(ref_s, l);
     const Box2d ego_box = GetBoxFromSLPoint(sl, dl);
@@ -197,23 +197,23 @@ ComparableCost TrajectoryCost::CalculateDynamicObstacleCost(
           GetCostBetweenObsBoxes(ego_box, obstacle_trajectory.at(index));
     }
   }
-  constexpr double kDynamicObsWeight = 1e-6;
+  constexpr float kDynamicObsWeight = 1e-6;
   obstacle_cost.safety_cost *=
       (config_.eval_time_interval() * kDynamicObsWeight);
   return obstacle_cost;
 }
 
 ComparableCost TrajectoryCost::GetCostFromObsSL(
-    const double adc_s, const double adc_l, const SLBoundary &obs_sl_boundary) {
+    const float adc_s, const float adc_l, const SLBoundary &obs_sl_boundary) {
   const auto &vehicle_param =
       common::VehicleConfigHelper::instance()->GetConfig().vehicle_param();
 
   ComparableCost obstacle_cost;
 
-  const double adc_front_s = adc_s + vehicle_param.front_edge_to_center();
-  const double adc_end_s = adc_s - vehicle_param.back_edge_to_center();
-  const double adc_left_l = adc_l + vehicle_param.left_edge_to_center();
-  const double adc_right_l = adc_l - vehicle_param.right_edge_to_center();
+  const float adc_front_s = adc_s + vehicle_param.front_edge_to_center();
+  const float adc_end_s = adc_s - vehicle_param.back_edge_to_center();
+  const float adc_left_l = adc_l + vehicle_param.left_edge_to_center();
+  const float adc_right_l = adc_l - vehicle_param.right_edge_to_center();
 
   if (adc_left_l + FLAGS_lateral_ignore_buffer < obs_sl_boundary.start_l() ||
       adc_right_l - FLAGS_lateral_ignore_buffer > obs_sl_boundary.end_l()) {
@@ -231,14 +231,14 @@ ComparableCost TrajectoryCost::GetCostFromObsSL(
     obstacle_cost.cost_items[ComparableCost::HAS_COLLISION] = true;
   }
 
-  const double delta_l = std::fabs(
+  const float delta_l = std::fabs(
       adc_l - (obs_sl_boundary.start_l() + obs_sl_boundary.end_l()) / 2.0);
 
   obstacle_cost.safety_cost +=
       config_.obstacle_collision_cost() *
       Sigmoid(config_.obstacle_collision_distance() - delta_l);
 
-  const double delta_s = std::fabs(
+  const float delta_s = std::fabs(
       adc_s - (obs_sl_boundary.start_s() + obs_sl_boundary.end_s()) / 2.0);
   obstacle_cost.safety_cost +=
       config_.obstacle_collision_cost() *
@@ -251,7 +251,7 @@ ComparableCost TrajectoryCost::GetCostBetweenObsBoxes(
     const Box2d &ego_box, const Box2d &obstacle_box) const {
   ComparableCost obstacle_cost;
 
-  const double distance = obstacle_box.DistanceTo(ego_box);
+  const float distance = obstacle_box.DistanceTo(ego_box);
   if (distance > config_.obstacle_ignore_distance()) {
     return obstacle_cost;
   }
@@ -266,15 +266,15 @@ ComparableCost TrajectoryCost::GetCostBetweenObsBoxes(
 }
 
 Box2d TrajectoryCost::GetBoxFromSLPoint(const common::SLPoint &sl,
-                                        const double dl) const {
+                                        const float dl) const {
   Vec2d xy_point;
   reference_line_->SLToXY(sl, &xy_point);
 
   ReferencePoint reference_point = reference_line_->GetReferencePoint(sl.s());
 
-  const double one_minus_kappa_r_d = 1 - reference_point.kappa() * sl.l();
-  const double delta_theta = std::atan2(dl, one_minus_kappa_r_d);
-  const double theta =
+  const float one_minus_kappa_r_d = 1 - reference_point.kappa() * sl.l();
+  const float delta_theta = std::atan2(dl, one_minus_kappa_r_d);
+  const float theta =
       common::math::NormalizeAngle(delta_theta + reference_point.heading());
   return Box2d(xy_point, theta, vehicle_param_.length(),
                vehicle_param_.width());
@@ -282,8 +282,7 @@ Box2d TrajectoryCost::GetBoxFromSLPoint(const common::SLPoint &sl,
 
 // TODO(All): optimize obstacle cost calculation time
 ComparableCost TrajectoryCost::Calculate(const QuinticPolynomialCurve1d &curve,
-                                         const double start_s,
-                                         const double end_s,
+                                         const float start_s, const float end_s,
                                          const uint32_t curr_level,
                                          const uint32_t total_level) {
   ComparableCost total_cost;

modules/planning/tasks/dp_poly_path/trajectory_cost.h

@@ -48,31 +48,31 @@ class TrajectoryCost {
                           const SpeedData &heuristic_speed_data,
                           const common::SLPoint &init_sl_point);
   ComparableCost Calculate(const QuinticPolynomialCurve1d &curve,
-                           const double start_s, const double end_s,
+                           const float start_s, const float end_s,
                            const uint32_t curr_level,
                            const uint32_t total_level);
 
  private:
   ComparableCost CalculatePathCost(const QuinticPolynomialCurve1d &curve,
-                                   const double start_s, const double end_s,
+                                   const float start_s, const float end_s,
                                    const uint32_t curr_level,
                                    const uint32_t total_level);
   ComparableCost CalculateStaticObstacleCost(
-      const QuinticPolynomialCurve1d &curve, const double start_s,
-      const double end_s);
+      const QuinticPolynomialCurve1d &curve, const float start_s,
+      const float end_s);
   ComparableCost CalculateDynamicObstacleCost(
-      const QuinticPolynomialCurve1d &curve, const double start_s,
-      const double end_s) const;
+      const QuinticPolynomialCurve1d &curve, const float start_s,
+      const float end_s) const;
   ComparableCost GetCostBetweenObsBoxes(
       const common::math::Box2d &ego_box,
       const common::math::Box2d &obstacle_box) const;
 
   FRIEND_TEST(AllTrajectoryTests, GetCostFromObsSL);
-  ComparableCost GetCostFromObsSL(const double adc_s, const double adc_l,
+  ComparableCost GetCostFromObsSL(const float adc_s, const float adc_l,
                                   const SLBoundary &obs_sl_boundary);
 
   common::math::Box2d GetBoxFromSLPoint(const common::SLPoint &sl,
-                                        const double dl) const;
+                                        const float dl) const;
 
   const DpPolyPathConfig config_;
   const ReferenceLine *reference_line_ = nullptr;
@@ -82,7 +82,7 @@ class TrajectoryCost {
   const common::SLPoint init_sl_point_;
   uint32_t num_of_time_stamps_ = 0;
   std::vector<std::vector<common::math::Box2d>> dynamic_obstacle_boxes_;
-  std::vector<double> obstacle_probabilities_;
+  std::vector<float> obstacle_probabilities_;
 
   std::vector<SLBoundary> static_obstacle_sl_boundaries_;
 };

modules/planning/tasks/dp_poly_path/trajectory_cost_test.cc

@@ -30,8 +30,8 @@ TEST(AllTrajectoryTests, GetCostFromObsSL) {
   obs_sl_boundary.set_start_l(-1.5);
   obs_sl_boundary.set_end_l(-0.2);
   auto cost = tc.GetCostFromObsSL(5.0, 0.5, obs_sl_boundary);
-  EXPECT_DOUBLE_EQ(cost.safety_cost, 240.48911372030088);
-  EXPECT_DOUBLE_EQ(cost.smoothness_cost, 0.0);
+  EXPECT_FLOAT_EQ(cost.safety_cost, 240.48911372030088);
+  EXPECT_FLOAT_EQ(cost.smoothness_cost, 0.0);
   EXPECT_FALSE(cost.cost_items.at(0));
   EXPECT_FALSE(cost.cost_items.at(1));
   EXPECT_FALSE(cost.cost_items.at(2));
@@ -45,8 +45,8 @@ TEST(AllTrajectoryTests, GetCostFromObsSL) {
   obs_sl_boundary1.set_end_l(-0.2);
   auto cost1 = tc.GetCostFromObsSL(21.0, -0.5, obs_sl_boundary1);
 
-  EXPECT_DOUBLE_EQ(cost1.safety_cost, 676.73517161369182);
-  EXPECT_DOUBLE_EQ(cost1.smoothness_cost, 0.0);
+  EXPECT_FLOAT_EQ(cost1.safety_cost, 676.73517161369182);
+  EXPECT_FLOAT_EQ(cost1.smoothness_cost, 0.0);
   EXPECT_TRUE(cost1.cost_items.at(0));
   EXPECT_FALSE(cost1.cost_items.at(1));
   EXPECT_FALSE(cost1.cost_items.at(2));

modules/planning/tasks/dp_st_speed/dp_st_cost.cc

@@ -29,7 +29,7 @@
 namespace apollo {
 namespace planning {
 namespace {
-constexpr double kInf = std::numeric_limits<double>::infinity();
+constexpr float kInf = std::numeric_limits<float>::infinity();
 }
 
 DpStCost::DpStCost(const DpStSpeedConfig& config,
@@ -65,15 +65,15 @@ void DpStCost::AddToKeepClearRange(
       continue;
     }
 
-    double start_s = obstacle->st_boundary().min_s();
-    double end_s = obstacle->st_boundary().max_s();
+    float start_s = obstacle->st_boundary().min_s();
+    float end_s = obstacle->st_boundary().max_s();
     keep_clear_range_.emplace_back(start_s, end_s);
   }
   SortAndMergeRange(&keep_clear_range_);
 }
 
 void DpStCost::SortAndMergeRange(
-    std::vector<std::pair<double, double>>* keep_clear_range) {
+    std::vector<std::pair<float, float>>* keep_clear_range) {
   if (!keep_clear_range || keep_clear_range->empty()) {
     return;
   }
@@ -93,7 +93,7 @@ void DpStCost::SortAndMergeRange(
   keep_clear_range->resize(i + 1);
 }
 
-bool DpStCost::InKeepClearRange(double s) const {
+bool DpStCost::InKeepClearRange(float s) const {
   if (keep_clear_range_.empty()) {
     return false;
   }
@@ -105,18 +105,18 @@ bool DpStCost::InKeepClearRange(double s) const {
   return false;
 }
 
-double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
-  const double s = st_graph_point.point().s();
-  const double t = st_graph_point.point().t();
+float DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
+  const float s = st_graph_point.point().s();
+  const float t = st_graph_point.point().t();
 
-  double cost = 0.0;
+  float cost = 0.0;
   for (const auto* obstacle : obstacles_) {
     if (!obstacle->IsBlockingObstacle()) {
       continue;
     }
 
     auto boundary = obstacle->st_boundary();
-    const double kIgnoreDistance = 200.0;
+    const float kIgnoreDistance = 200.0;
     if (boundary.min_s() > kIgnoreDistance) {
       continue;
     }
@@ -140,8 +140,8 @@ double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
       s_lower = boundary_cost_[boundary_index][st_graph_point.index_t()].second;
     }
     if (s < s_lower) {
-      constexpr double kSafeTimeBuffer = 3.0;
-      const double len = obstacle->obstacle()->Speed() * kSafeTimeBuffer;
+      constexpr float kSafeTimeBuffer = 3.0;
+      const float len = obstacle->obstacle()->Speed() * kSafeTimeBuffer;
       if (s + len < s_lower) {
         continue;
       } else {
@@ -149,7 +149,7 @@ double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
                 std::pow((len - s_lower + s), 2);
       }
     } else if (s > s_upper) {
-      const double kSafeDistance = 20.0;  // or calculated from velocity
+      const float kSafeDistance = 20.0;  // or calculated from velocity
       if (s > s_upper + kSafeDistance) {
         continue;
       } else {
@@ -161,16 +161,16 @@ double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
   return cost * unit_t_;
 }
 
-double DpStCost::GetReferenceCost(const STPoint& point,
-                                  const STPoint& reference_point) const {
+float DpStCost::GetReferenceCost(const STPoint& point,
+                                 const STPoint& reference_point) const {
   return config_.reference_weight() * (point.s() - reference_point.s()) *
          (point.s() - reference_point.s()) * unit_t_;
 }
 
-double DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,
-                              const double speed_limit) const {
-  double cost = 0.0;
-  const double speed = (second.s() - first.s()) / unit_t_;
+float DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,
+                             const float speed_limit) const {
+  float cost = 0.0;
+  const float speed = (second.s() - first.s()) / unit_t_;
   if (speed < 0) {
     return kInf;
   }
@@ -181,7 +181,7 @@ double DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,
             config_.default_speed_cost();
   }
 
-  double det_speed = (speed - speed_limit) / speed_limit;
+  float det_speed = (speed - speed_limit) / speed_limit;
   if (det_speed > 0) {
     cost += config_.exceed_speed_penalty() * config_.default_speed_cost() *
             fabs(speed * speed) * unit_t_;
@@ -192,9 +192,9 @@ double DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,
   return cost;
 }
 
-double DpStCost::GetAccelCost(const double accel) {
-  double cost = 0.0;
-  constexpr double kEpsilon = 0.1;
+float DpStCost::GetAccelCost(const float accel) {
+  float cost = 0.0;
+  constexpr float kEpsilon = 0.1;
   constexpr size_t kShift = 100;
   const size_t accel_key = static_cast<size_t>(accel / kEpsilon + 0.5 + kShift);
   DCHECK_LT(accel_key, accel_cost_.size());
@@ -203,11 +203,11 @@ double DpStCost::GetAccelCost(const double accel) {
   }
 
   if (accel_cost_.at(accel_key) < 0.0) {
-    const double accel_sq = accel * accel;
-    double max_acc = config_.max_acceleration();
-    double max_dec = config_.max_deceleration();
-    double accel_penalty = config_.accel_penalty();
-    double decel_penalty = config_.decel_penalty();
+    const float accel_sq = accel * accel;
+    float max_acc = config_.max_acceleration();
+    float max_dec = config_.max_deceleration();
+    float accel_penalty = config_.accel_penalty();
+    float decel_penalty = config_.decel_penalty();
 
     if (accel > 0.0) {
       cost = accel_penalty * accel_sq;
@@ -225,24 +225,24 @@ double DpStCost::GetAccelCost(const double accel) {
   return cost * unit_t_;
 }
 
-double DpStCost::GetAccelCostByThreePoints(const STPoint& first,
-                                           const STPoint& second,
-                                           const STPoint& third) {
-  double accel = (first.s() + third.s() - 2 * second.s()) / (unit_t_ * unit_t_);
+float DpStCost::GetAccelCostByThreePoints(const STPoint& first,
+                                          const STPoint& second,
+                                          const STPoint& third) {
+  float accel = (first.s() + third.s() - 2 * second.s()) / (unit_t_ * unit_t_);
   return GetAccelCost(accel);
 }
 
-double DpStCost::GetAccelCostByTwoPoints(const double pre_speed,
-                                         const STPoint& pre_point,
-                                         const STPoint& curr_point) {
-  double current_speed = (curr_point.s() - pre_point.s()) / unit_t_;
-  double accel = (current_speed - pre_speed) / unit_t_;
+float DpStCost::GetAccelCostByTwoPoints(const float pre_speed,
+                                        const STPoint& pre_point,
+                                        const STPoint& curr_point) {
+  float current_speed = (curr_point.s() - pre_point.s()) / unit_t_;
+  float accel = (current_speed - pre_speed) / unit_t_;
   return GetAccelCost(accel);
 }
 
-double DpStCost::JerkCost(const double jerk) {
-  double cost = 0.0;
-  constexpr double kEpsilon = 0.1;
+float DpStCost::JerkCost(const float jerk) {
+  float cost = 0.0;
+  constexpr float kEpsilon = 0.1;
   constexpr size_t kShift = 200;
   const size_t jerk_key = static_cast<size_t>(jerk / kEpsilon + 0.5 + kShift);
   if (jerk_key >= jerk_cost_.size()) {
@@ -250,7 +250,7 @@ double DpStCost::JerkCost(const double jerk) {
   }
 
   if (jerk_cost_.at(jerk_key) < 0.0) {
-    double jerk_sq = jerk * jerk;
+    float jerk_sq = jerk * jerk;
     if (jerk > 0) {
       cost = config_.positive_jerk_coeff() * jerk_sq * unit_t_;
     } else {
@@ -265,34 +265,34 @@ double DpStCost::JerkCost(const double jerk) {
   return cost;
 }
 
-double DpStCost::GetJerkCostByFourPoints(const STPoint& first,
-                                         const STPoint& second,
-                                         const STPoint& third,
-                                         const STPoint& fourth) {
-  double jerk = (fourth.s() - 3 * third.s() + 3 * second.s() - first.s()) /
-                (unit_t_ * unit_t_ * unit_t_);
+float DpStCost::GetJerkCostByFourPoints(const STPoint& first,
+                                        const STPoint& second,
+                                        const STPoint& third,
+                                        const STPoint& fourth) {
+  float jerk = (fourth.s() - 3 * third.s() + 3 * second.s() - first.s()) /
+               (unit_t_ * unit_t_ * unit_t_);
   return JerkCost(jerk);
 }
 
-double DpStCost::GetJerkCostByTwoPoints(const double pre_speed,
-                                        const double pre_acc,
-                                        const STPoint& pre_point,
-                                        const STPoint& curr_point) {
-  const double curr_speed = (curr_point.s() - pre_point.s()) / unit_t_;
-  const double curr_accel = (curr_speed - pre_speed) / unit_t_;
-  const double jerk = (curr_accel - pre_acc) / unit_t_;
+float DpStCost::GetJerkCostByTwoPoints(const float pre_speed,
+                                       const float pre_acc,
+                                       const STPoint& pre_point,
+                                       const STPoint& curr_point) {
+  const float curr_speed = (curr_point.s() - pre_point.s()) / unit_t_;
+  const float curr_accel = (curr_speed - pre_speed) / unit_t_;
+  const float jerk = (curr_accel - pre_acc) / unit_t_;
   return JerkCost(jerk);
 }
 
-double DpStCost::GetJerkCostByThreePoints(const double first_speed,
-                                          const STPoint& first,
-                                          const STPoint& second,
-                                          const STPoint& third) {
-  const double pre_speed = (second.s() - first.s()) / unit_t_;
-  const double pre_acc = (pre_speed - first_speed) / unit_t_;
-  const double curr_speed = (third.s() - second.s()) / unit_t_;
-  const double curr_acc = (curr_speed - pre_speed) / unit_t_;
-  const double jerk = (curr_acc - pre_acc) / unit_t_;
+float DpStCost::GetJerkCostByThreePoints(const float first_speed,
+                                         const STPoint& first,
+                                         const STPoint& second,
+                                         const STPoint& third) {
+  const float pre_speed = (second.s() - first.s()) / unit_t_;
+  const float pre_acc = (pre_speed - first_speed) / unit_t_;
+  const float curr_speed = (third.s() - second.s()) / unit_t_;
+  const float curr_acc = (curr_speed - pre_speed) / unit_t_;
+  const float jerk = (curr_acc - pre_acc) / unit_t_;
   return JerkCost(jerk);
 }
 

modules/planning/tasks/dp_st_speed/dp_st_cost.h

@@ -43,52 +43,52 @@ class DpStCost {
                     const std::vector<const PathObstacle*>& obstacles,
                     const common::TrajectoryPoint& init_point);
 
-  double GetObstacleCost(const StGraphPoint& point);
+  float GetObstacleCost(const StGraphPoint& point);
 
-  double GetReferenceCost(const STPoint& point,
+  float GetReferenceCost(const STPoint& point,
                           const STPoint& reference_point) const;
 
-  double GetSpeedCost(const STPoint& first, const STPoint& second,
-                      const double speed_limit) const;
+  float GetSpeedCost(const STPoint& first, const STPoint& second,
+                      const float speed_limit) const;
 
-  double GetAccelCostByTwoPoints(const double pre_speed, const STPoint& first,
+  float GetAccelCostByTwoPoints(const float pre_speed, const STPoint& first,
                                  const STPoint& second);
-  double GetAccelCostByThreePoints(const STPoint& first, const STPoint& second,
+  float GetAccelCostByThreePoints(const STPoint& first, const STPoint& second,
                                    const STPoint& third);
 
-  double GetJerkCostByTwoPoints(const double pre_speed, const double pre_acc,
+  float GetJerkCostByTwoPoints(const float pre_speed, const float pre_acc,
                                 const STPoint& pre_point,
                                 const STPoint& curr_point);
-  double GetJerkCostByThreePoints(const double first_speed,
+  float GetJerkCostByThreePoints(const float first_speed,
                                   const STPoint& first_point,
                                   const STPoint& second_point,
                                   const STPoint& third_point);
 
-  double GetJerkCostByFourPoints(const STPoint& first, const STPoint& second,
+  float GetJerkCostByFourPoints(const STPoint& first, const STPoint& second,
                                  const STPoint& third, const STPoint& fourth);
 
  private:
-  double GetAccelCost(const double accel);
-  double JerkCost(const double jerk);
+  float GetAccelCost(const float accel);
+  float JerkCost(const float jerk);
 
   void AddToKeepClearRange(const std::vector<const PathObstacle*>& obstacles);
   static void SortAndMergeRange(
-      std::vector<std::pair<double, double>>* keep_clear_range_);
-  bool InKeepClearRange(double s) const;
+      std::vector<std::pair<float, float>>* keep_clear_range_);
+  bool InKeepClearRange(float s) const;
 
   const DpStSpeedConfig& config_;
   const std::vector<const PathObstacle*>& obstacles_;
   const common::TrajectoryPoint& init_point_;
 
-  double unit_t_ = 0.0;
+  float unit_t_ = 0.0;
 
   std::unordered_map<std::string, int> boundary_map_;
-  std::vector<std::vector<std::pair<double, double>>> boundary_cost_;
+  std::vector<std::vector<std::pair<float, float>>> boundary_cost_;
 
-  std::vector<std::pair<double, double>> keep_clear_range_;
+  std::vector<std::pair<float, float>> keep_clear_range_;
 
-  std::array<double, 200> accel_cost_;
-  std::array<double, 400> jerk_cost_;
+  std::array<float, 200> accel_cost_;
+  std::array<float, 400> jerk_cost_;
 };
 
 }  // namespace planning

modules/planning/tasks/dp_st_speed/dp_st_graph.cc

@@ -42,7 +42,7 @@ using apollo::common::VehicleParam;
 using apollo::common::math::Vec2d;
 
 namespace {
-constexpr double kInf = std::numeric_limits<double>::infinity();
+constexpr float kInf = std::numeric_limits<float>::infinity();
 
 bool CheckOverlapOnDpStGraph(const std::vector<const StBoundary*>& boundaries,
                              const StGraphPoint& p1, const StGraphPoint& p2) {
@@ -80,7 +80,7 @@ DpStGraph::DpStGraph(const StGraphData& st_graph_data,
 }
 
 Status DpStGraph::Search(SpeedData* const speed_data) {
-  constexpr double kBounadryEpsilon = 1e-2;
+  constexpr float kBounadryEpsilon = 1e-2;
   for (const auto& boundary : st_graph_data_.st_boundaries()) {
     if (boundary->boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
       continue;
@@ -89,7 +89,7 @@ Status DpStGraph::Search(SpeedData* const speed_data) {
         (std::fabs(boundary->min_t()) < kBounadryEpsilon &&
          std::fabs(boundary->min_s()) < kBounadryEpsilon)) {
       std::vector<SpeedPoint> speed_profile;
-      double t = 0.0;
+      float t = 0.0;
       for (int i = 0; i < dp_st_speed_config_.matrix_dimension_t();
            ++i, t += unit_t_) {
         SpeedPoint speed_point;
@@ -105,15 +105,15 @@ Status DpStGraph::Search(SpeedData* const speed_data) {
   if (st_graph_data_.st_boundaries().empty()) {
     ADEBUG << "No path obstacles, dp_st_graph output default speed profile.";
     std::vector<SpeedPoint> speed_profile;
-    double s = 0.0;
-    double t = 0.0;
+    float s = 0.0;
+    float t = 0.0;
     for (int i = 0; i < dp_st_speed_config_.matrix_dimension_t() &&
                     i < dp_st_speed_config_.matrix_dimension_s();
          ++i, t += unit_t_, s += unit_s_) {
       SpeedPoint speed_point;
       speed_point.set_s(s);
       speed_point.set_t(t);
-      const double v_default = unit_s_ / unit_t_;
+      const float v_default = unit_s_ / unit_t_;
       speed_point.set_v(v_default);
       speed_point.set_a(0.0);
       speed_profile.emplace_back(std::move(speed_point));
@@ -150,10 +150,10 @@ Status DpStGraph::InitCostTable() {
   cost_table_ = std::vector<std::vector<StGraphPoint>>(
       dim_t, std::vector<StGraphPoint>(dim_s, StGraphPoint()));
 
-  double curr_t = 0.0;
+  float curr_t = 0.0;
   for (uint32_t i = 0; i < cost_table_.size(); ++i, curr_t += unit_t_) {
     auto& cost_table_i = cost_table_[i];
-    double curr_s = 0.0;
+    float curr_s = 0.0;
     for (uint32_t j = 0; j < cost_table_i.size(); ++j, curr_s += unit_s_) {
       cost_table_i[j].Init(i, j, STPoint(curr_s, curr_t));
     }
@@ -186,7 +186,7 @@ Status DpStGraph::CalculateTotalCost() {
 
     for (uint32_t r = next_lowest_row; r <= next_highest_row; ++r) {
       const auto& cost_cr = cost_table_[c][r];
-      if (cost_cr.total_cost() < std::numeric_limits<double>::infinity()) {
+      if (cost_cr.total_cost() < std::numeric_limits<float>::infinity()) {
         int h_r = 0;
         int l_r = 0;
         GetRowRange(cost_cr, &h_r, &l_r);
@@ -203,7 +203,7 @@ Status DpStGraph::CalculateTotalCost() {
 
 void DpStGraph::GetRowRange(const StGraphPoint& point, int* next_highest_row,
                             int* next_lowest_row) {
-  double v0 = 0.0;
+  float v0 = 0.0;
   if (!point.pre_point()) {
     v0 = init_point_.v();
   } else {
@@ -212,9 +212,9 @@ void DpStGraph::GetRowRange(const StGraphPoint& point, int* next_highest_row,
 
   const int max_s_size = cost_table_.back().size() - 1;
 
-  const double speed_coeff = unit_t_ * unit_t_;
+  const float speed_coeff = unit_t_ * unit_t_;
 
-  const double delta_s_upper_bound =
+  const float delta_s_upper_bound =
       v0 * unit_t_ + vehicle_param_.max_acceleration() * speed_coeff;
   *next_highest_row =
       point.index_s() + static_cast<int>(delta_s_upper_bound / unit_s_);
@@ -222,7 +222,7 @@ void DpStGraph::GetRowRange(const StGraphPoint& point, int* next_highest_row,
     *next_highest_row = max_s_size;
   }
 
-  const double delta_s_lower_bound = std::fmax(
+  const float delta_s_lower_bound = std::fmax(
       0.0, v0 * unit_t_ + vehicle_param_.max_deceleration() * speed_coeff);
   *next_lowest_row =
       point.index_s() - static_cast<int>(delta_s_lower_bound / unit_s_);
@@ -236,7 +236,7 @@ void DpStGraph::GetRowRange(const StGraphPoint& point, int* next_highest_row,
 void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
   auto& cost_cr = cost_table_[c][r];
   cost_cr.SetObstacleCost(dp_st_cost_.GetObstacleCost(cost_cr));
-  if (cost_cr.obstacle_cost() > std::numeric_limits<double>::max()) {
+  if (cost_cr.obstacle_cost() > std::numeric_limits<float>::max()) {
     return;
   }
 
@@ -247,10 +247,10 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
     return;
   }
 
-  double speed_limit =
+  float speed_limit =
       st_graph_data_.speed_limit().GetSpeedLimitByS(unit_s_ * r);
   if (c == 1) {
-    const double acc = (r * unit_s_ / unit_t_ - init_point_.v()) / unit_t_;
+    const float acc = (r * unit_s_ / unit_t_ - init_point_.v()) / unit_t_;
     if (acc < dp_st_speed_config_.max_deceleration() ||
         acc > dp_st_speed_config_.max_acceleration()) {
       return;
@@ -266,7 +266,7 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
     return;
   }
 
-  constexpr double kSpeedRangeBuffer = 0.20;
+  constexpr float kSpeedRangeBuffer = 0.20;
   const uint32_t max_s_diff =
       static_cast<uint32_t>(FLAGS_planning_upper_speed_limit *
                             (1 + kSpeedRangeBuffer) * unit_t_ / unit_s_);
@@ -276,7 +276,7 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
 
   if (c == 2) {
     for (uint32_t r_pre = r_low; r_pre <= r; ++r_pre) {
-      const double acc =
+      const float acc =
           (r * unit_s_ - 2 * r_pre * unit_s_) / (unit_t_ * unit_t_);
       if (acc < dp_st_speed_config_.max_deceleration() ||
           acc > dp_st_speed_config_.max_acceleration()) {
@@ -288,7 +288,7 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
         continue;
       }
 
-      const double cost = cost_cr.obstacle_cost() +
+      const float cost = cost_cr.obstacle_cost() +
                           pre_col[r_pre].total_cost() +
                           CalculateEdgeCostForThirdCol(r, r_pre, speed_limit);
 
@@ -305,7 +305,7 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
       continue;
     }
 
-    const double curr_a = (cost_cr.index_s() * unit_s_ +
+    const float curr_a = (cost_cr.index_s() * unit_s_ +
                            pre_col[r_pre].pre_point()->index_s() * unit_s_ -
                            2 * pre_col[r_pre].index_s() * unit_s_) /
                           (unit_t_ * unit_t_);
@@ -331,7 +331,7 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
     const STPoint& prepre_point = prepre_graph_point.point();
     const STPoint& pre_point = pre_col[r_pre].point();
     const STPoint& curr_point = cost_cr.point();
-    double cost = cost_cr.obstacle_cost() + pre_col[r_pre].total_cost() +
+    float cost = cost_cr.obstacle_cost() + pre_col[r_pre].total_cost() +
                   CalculateEdgeCost(triple_pre_point, prepre_point, pre_point,
                                     curr_point, speed_limit);
 
@@ -343,7 +343,7 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
 }
 
 Status DpStGraph::RetrieveSpeedProfile(SpeedData* const speed_data) {
-  double min_cost = std::numeric_limits<double>::infinity();
+  float min_cost = std::numeric_limits<float>::infinity();
   const StGraphPoint* best_end_point = nullptr;
   for (const StGraphPoint& cur_point : cost_table_.back()) {
     if (!std::isinf(cur_point.total_cost()) &&
@@ -379,7 +379,7 @@ Status DpStGraph::RetrieveSpeedProfile(SpeedData* const speed_data) {
   }
   std::reverse(speed_profile.begin(), speed_profile.end());
 
-  constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
+  constexpr float kEpsilon = std::numeric_limits<float>::epsilon();
   if (speed_profile.front().t() > kEpsilon ||
       speed_profile.front().s() > kEpsilon) {
     const std::string msg = "Fail to retrieve speed profile.";
@@ -390,18 +390,18 @@ Status DpStGraph::RetrieveSpeedProfile(SpeedData* const speed_data) {
   return Status::OK();
 }
 
-double DpStGraph::CalculateEdgeCost(const STPoint& first, const STPoint& second,
+float DpStGraph::CalculateEdgeCost(const STPoint& first, const STPoint& second,
                                     const STPoint& third, const STPoint& forth,
-                                    const double speed_limit) {
+                                    const float speed_limit) {
   return dp_st_cost_.GetSpeedCost(third, forth, speed_limit) +
          dp_st_cost_.GetAccelCostByThreePoints(second, third, forth) +
          dp_st_cost_.GetJerkCostByFourPoints(first, second, third, forth);
 }
 
-double DpStGraph::CalculateEdgeCostForSecondCol(const uint32_t row,
-                                                const double speed_limit) {
-  double init_speed = init_point_.v();
-  double init_acc = init_point_.a();
+float DpStGraph::CalculateEdgeCostForSecondCol(const uint32_t row,
+                                                const float speed_limit) {
+  float init_speed = init_point_.v();
+  float init_acc = init_point_.a();
   const STPoint& pre_point = cost_table_[0][0].point();
   const STPoint& curr_point = cost_table_[1][row].point();
   return dp_st_cost_.GetSpeedCost(pre_point, curr_point, speed_limit) +
@@ -411,10 +411,10 @@ double DpStGraph::CalculateEdgeCostForSecondCol(const uint32_t row,
                                             curr_point);
 }
 
-double DpStGraph::CalculateEdgeCostForThirdCol(const uint32_t curr_row,
+float DpStGraph::CalculateEdgeCostForThirdCol(const uint32_t curr_row,
                                                const uint32_t pre_row,
-                                               const double speed_limit) {
-  double init_speed = init_point_.v();
+                                               const float speed_limit) {
+  float init_speed = init_point_.v();
   const STPoint& first = cost_table_[0][0].point();
   const STPoint& second = cost_table_[1][pre_row].point();
   const STPoint& third = cost_table_[2][curr_row].point();

modules/planning/tasks/dp_st_speed/dp_st_graph.h

@@ -58,14 +58,14 @@ class DpStGraph {
   apollo::common::Status CalculateTotalCost();
   void CalculateCostAt(const uint32_t r, const uint32_t c);
 
-  double CalculateEdgeCost(const STPoint& first, const STPoint& second,
+  float CalculateEdgeCost(const STPoint& first, const STPoint& second,
                            const STPoint& third, const STPoint& forth,
-                           const double speed_limit);
-  double CalculateEdgeCostForSecondCol(const uint32_t row,
-                                       const double speed_limit);
-  double CalculateEdgeCostForThirdCol(const uint32_t curr_r,
+                           const float speed_limit);
+  float CalculateEdgeCostForSecondCol(const uint32_t row,
+                                       const float speed_limit);
+  float CalculateEdgeCostForThirdCol(const uint32_t curr_r,
                                       const uint32_t pre_r,
-                                      const double speed_limit);
+                                      const float speed_limit);
 
   void GetRowRange(const StGraphPoint& point, int* highest_row,
                    int* lowest_row);
@@ -91,8 +91,8 @@ class DpStGraph {
 
   const SLBoundary& adc_sl_boundary_;
 
-  double unit_s_ = 0.0;
-  double unit_t_ = 0.0;
+  float unit_s_ = 0.0;
+  float unit_t_ = 0.0;
 
   // cost_table_[t][s]
   // row: s, col: t --- NOTICE: Please do NOT change.

modules/planning/tasks/dp_st_speed/dp_st_graph_test.cc

@@ -46,7 +46,7 @@ class DpStGraphTest : public ::testing::Test {
     dp_config_ = config.em_planner_config().dp_st_speed_config();
 
     // speed_limit:
-    for (double s = 0; s < 200.0; s += 1.0) {
+    for (float s = 0; s < 200.0; s += 1.0) {
       speed_limit_.AppendSpeedLimit(s, 25.0);
     }
   }
@@ -96,7 +96,7 @@ TEST_F(DpStGraphTest, simple) {
   init_point_.set_v(10.0);
   init_point_.set_a(0.0);
 
-  const double path_data_length = 120.0;
+  const float path_data_length = 120.0;
 
   st_graph_data_ =
       StGraphData(boundaries, init_point_, speed_limit_, path_data_length);

modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc

@@ -109,7 +109,7 @@ bool DpStSpeedOptimizer::SearchStGraph(
     return false;
   }
 
-  const double path_length = path_data.discretized_path().Length();
+  const float path_length = path_data.discretized_path().Length();
   StGraphData st_graph_data(boundaries, init_point_, speed_limit, path_length);
 
   DpStGraph st_graph(

modules/planning/tasks/dp_st_speed/st_graph_point.cc

@@ -33,11 +33,11 @@ const STPoint& StGraphPoint::point() const { return point_; }
 
 const StGraphPoint* StGraphPoint::pre_point() const { return pre_point_; }
 
-double StGraphPoint::reference_cost() const { return reference_cost_; }
+float StGraphPoint::reference_cost() const { return reference_cost_; }
 
-double StGraphPoint::obstacle_cost() const { return obstacle_cost_; }
+float StGraphPoint::obstacle_cost() const { return obstacle_cost_; }
 
-double StGraphPoint::total_cost() const { return total_cost_; }
+float StGraphPoint::total_cost() const { return total_cost_; }
 
 void StGraphPoint::Init(const std::uint32_t index_t,
                         const std::uint32_t index_s, const STPoint& st_point) {
@@ -46,15 +46,15 @@ void StGraphPoint::Init(const std::uint32_t index_t,
   point_ = st_point;
 }
 
-void StGraphPoint::SetReferenceCost(const double reference_cost) {
+void StGraphPoint::SetReferenceCost(const float reference_cost) {
   reference_cost_ = reference_cost;
 }
 
-void StGraphPoint::SetObstacleCost(const double obs_cost) {
+void StGraphPoint::SetObstacleCost(const float obs_cost) {
   obstacle_cost_ = obs_cost;
 }
 
-void StGraphPoint::SetTotalCost(const double total_cost) {
+void StGraphPoint::SetTotalCost(const float total_cost) {
   total_cost_ = total_cost;
 }
 

modules/planning/tasks/dp_st_speed/st_graph_point.h

@@ -36,21 +36,21 @@ class StGraphPoint {
   const STPoint& point() const;
   const StGraphPoint* pre_point() const;
 
-  double reference_cost() const;
-  double obstacle_cost() const;
-  double total_cost() const;
+  float reference_cost() const;
+  float obstacle_cost() const;
+  float total_cost() const;
 
   void Init(const std::uint32_t index_t, const std::uint32_t index_s,
             const STPoint& st_point);
 
   // given reference speed profile, reach the cost, including position
-  void SetReferenceCost(const double reference_cost);
+  void SetReferenceCost(const float reference_cost);
 
   // given obstacle info, get the cost;
-  void SetObstacleCost(const double obs_cost);
+  void SetObstacleCost(const float obs_cost);
 
   // total cost
-  void SetTotalCost(const double total_cost);
+  void SetTotalCost(const float total_cost);
 
   void SetPrePoint(const StGraphPoint& pre_point);
 
@@ -60,9 +60,9 @@ class StGraphPoint {
   std::uint32_t index_s_ = 0;
   std::uint32_t index_t_ = 0;
 
-  double reference_cost_ = 0.0;
-  double obstacle_cost_ = 0.0;
-  double total_cost_ = std::numeric_limits<double>::infinity();
+  float reference_cost_ = 0.0;
+  float obstacle_cost_ = 0.0;
+  float total_cost_ = std::numeric_limits<float>::infinity();
 };
 
 }  // namespace planning