Planning: (1) fixed a bug in dp_st_graph. (2) used array instead of map in dp_st_cost.

modules/planning/tasks/dp_st_speed/dp_st_cost.cc

@@ -36,9 +36,10 @@ DpStCost::DpStCost(const DpStSpeedConfig& config,
     : config_(config),
       obstacles_(obstacles),
       init_point_(init_point),
-      unit_s_(config_.total_path_length() / config_.matrix_dimension_s()),
-      unit_t_(config_.total_time() / config_.matrix_dimension_t()),
-      unit_v_(unit_s_ / unit_t_) {}
+      unit_t_(config_.total_time() / config_.matrix_dimension_t()) {
+  accel_cost_.fill(-1.0);
+  jerk_cost_.fill(-1.0);
+}
 
 double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
   const double s = st_graph_point.point().s();
@@ -123,9 +124,14 @@ double DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,
 double DpStCost::GetAccelCost(const double accel) {
   double cost = 0.0;
   constexpr double kEpsilon = 0.1;
-  const int accel_key = static_cast<int>(accel / kEpsilon + 0.5);
-  auto p_val = accel_cost_map_.find(accel_key);
-  if (p_val == accel_cost_map_.end()) {
+  constexpr int kShift = 100;
+  const int accel_key = static_cast<int>(accel / kEpsilon + 0.5) + kShift;
+  DCHECK_LT(accel_key, accel_cost_.size());
+  if (accel_key < 0 || accel_key >= accel_cost_.size()) {
+    return kInf;
+  }
+
+  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();
@@ -141,9 +147,9 @@ double DpStCost::GetAccelCost(const double accel) {
                 (1 + std::exp(1.0 * (accel - max_dec))) +
             accel_sq * accel_penalty * accel_penalty /
                 (1 + std::exp(-1.0 * (accel - max_acc)));
-    accel_cost_map_[accel_key] = cost;
+    accel_cost_.at(accel_key) = cost;
   } else {
-    cost = p_val->second;
+    cost = accel_cost_.at(accel_key);
   }
   return cost * unit_t_;
 }
@@ -166,18 +172,23 @@ double DpStCost::GetAccelCostByTwoPoints(const double pre_speed,
 double DpStCost::JerkCost(const double jerk) {
   double cost = 0.0;
   constexpr double kEpsilon = 0.1;
-  const int jerk_key = static_cast<int>(jerk / kEpsilon + 0.5);
-  auto p_val = jerk_cost_map_.find(jerk_key);
-  if (p_val == jerk_cost_map_.end()) {
+  constexpr int kShift = 200;
+  const int jerk_key = static_cast<int>(jerk / kEpsilon + 0.5) + kShift;
+  DCHECK_LT(jerk_key, jerk_cost_.size());
+  if (jerk_key < 0 || jerk_key >= jerk_cost_.size()) {
+    return kInf;
+  }
+
+  if (jerk_cost_.at(jerk_key) < 0.0) {
     double jerk_sq = jerk * jerk;
     if (jerk > 0) {
       cost = config_.positive_jerk_coeff() * jerk_sq * unit_t_;
     } else {
       cost = config_.negative_jerk_coeff() * jerk_sq * unit_t_;
     }
-    jerk_cost_map_[jerk_key] = cost;
+    jerk_cost_.at(jerk_key) = cost;
   } else {
-    cost = p_val->second;
+    cost = jerk_cost_.at(jerk_key);
   }
 
   // TODO(All): normalize to unit_t_

modules/planning/tasks/dp_st_speed/dp_st_cost.h

@@ -74,14 +74,13 @@ class DpStCost {
   const DpStSpeedConfig& config_;
   const std::vector<const PathObstacle*>& obstacles_;
   const common::TrajectoryPoint& init_point_;
-  double unit_s_ = 0.0;
+
   double unit_t_ = 0.0;
-  double unit_v_ = 0.0;
 
   std::unordered_map<std::string, std::pair<double, double>>
       boundary_range_map_;
-  std::unordered_map<int, double> accel_cost_map_;
-  std::unordered_map<int, double> jerk_cost_map_;
+  std::array<double, 200> accel_cost_;
+  std::array<double, 400> jerk_cost_;
 };
 
 }  // namespace planning

modules/planning/tasks/dp_st_speed/dp_st_graph.cc

@@ -40,6 +40,7 @@ using apollo::common::math::Vec2d;
 using apollo::common::VehicleParam;
 
 namespace {
+constexpr double kInf = std::numeric_limits<double>::infinity();
 
 bool CheckOverlapOnDpStGraph(const std::vector<const StBoundary*>& boundaries,
                              const StGraphPoint& p1, const StGraphPoint& p2) {
@@ -232,6 +233,12 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
   double 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_;
+    if (acc < dp_st_speed_config_.max_deceleration() ||
+        acc > dp_st_speed_config_.max_acceleration()) {
+      return;
+    }
+
     if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                 cost_init)) {
       return;
@@ -252,6 +259,13 @@ 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 =
+          (static_cast<int>(r - 2 * r_pre)) * unit_s_ / (unit_t_ * unit_t_);
+      if (acc < dp_st_speed_config_.max_deceleration() ||
+          acc > dp_st_speed_config_.max_acceleration()) {
+        continue;
+      }
+
       if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                   pre_col[r_pre])) {
         continue;
@@ -282,41 +296,31 @@ void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
         curr_a < vehicle_param_.max_deceleration()) {
       continue;
     }
-
-    uint32_t lower_bound = 0;
-    uint32_t upper_bound = 0;
-    if (!CalculateFeasibleAccelRange(static_cast<double>(r_pre),
-                                     static_cast<double>(r), &lower_bound,
-                                     &upper_bound)) {
-      continue;
-    }
-
     if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                 pre_col[r_pre])) {
       continue;
     }
 
-    for (uint32_t r_prepre = lower_bound; r_prepre <= upper_bound; ++r_prepre) {
-      const StGraphPoint& prepre_graph_point = cost_table_[c - 2][r_prepre];
-      if (std::isinf(prepre_graph_point.total_cost())) {
-        continue;
-      }
-
-      if (!prepre_graph_point.pre_point()) {
-        continue;
-      }
-      const STPoint& triple_pre_point = prepre_graph_point.pre_point()->point();
-      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() +
-                    CalculateEdgeCost(triple_pre_point, prepre_point, pre_point,
-                                      curr_point, speed_limit);
+    uint32_t r_prepre = pre_col[r_pre].pre_point()->index_s();
+    const StGraphPoint& prepre_graph_point = cost_table_[c - 2][r_prepre];
+    if (std::isinf(prepre_graph_point.total_cost())) {
+      continue;
+    }
 
-      if (cost < cost_cr.total_cost()) {
-        cost_cr.SetTotalCost(cost);
-        cost_cr.SetPrePoint(pre_col[r_pre]);
-      }
+    if (!prepre_graph_point.pre_point()) {
+      continue;
+    }
+    const STPoint& triple_pre_point = prepre_graph_point.pre_point()->point();
+    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() +
+                  CalculateEdgeCost(triple_pre_point, prepre_point, pre_point,
+                                    curr_point, speed_limit);
+
+    if (cost < cost_cr.total_cost()) {
+      cost_cr.SetTotalCost(cost);
+      cost_cr.SetPrePoint(pre_col[r_pre]);
     }
   }
 }