From f5f3efb73e1a5d4be9a2d47c18a893658ef228a7 Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Sun, 31 Dec 2017 01:38:00 -0800 Subject: [PATCH] Planning: (1) fixed a bug in dp_st_graph. (2) used array instead of map in dp_st_cost. --- .../planning/tasks/dp_st_speed/dp_st_cost.cc | 37 +++++++---- .../planning/tasks/dp_st_speed/dp_st_cost.h | 7 +-- .../planning/tasks/dp_st_speed/dp_st_graph.cc | 62 ++++++++++--------- 3 files changed, 60 insertions(+), 46 deletions(-) diff --git a/modules/planning/tasks/dp_st_speed/dp_st_cost.cc b/modules/planning/tasks/dp_st_speed/dp_st_cost.cc index e5c33b0210..ace87b38f7 100644 --- a/modules/planning/tasks/dp_st_speed/dp_st_cost.cc +++ b/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(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(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(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(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_ diff --git a/modules/planning/tasks/dp_st_speed/dp_st_cost.h b/modules/planning/tasks/dp_st_speed/dp_st_cost.h index 08320b0c1b..2e48a19659 100644 --- a/modules/planning/tasks/dp_st_speed/dp_st_cost.h +++ b/modules/planning/tasks/dp_st_speed/dp_st_cost.h @@ -74,14 +74,13 @@ class DpStCost { const DpStSpeedConfig& config_; const std::vector& obstacles_; const common::TrajectoryPoint& init_point_; - double unit_s_ = 0.0; + double unit_t_ = 0.0; - double unit_v_ = 0.0; std::unordered_map> boundary_range_map_; - std::unordered_map accel_cost_map_; - std::unordered_map jerk_cost_map_; + std::array accel_cost_; + std::array jerk_cost_; }; } // namespace planning diff --git a/modules/planning/tasks/dp_st_speed/dp_st_graph.cc b/modules/planning/tasks/dp_st_speed/dp_st_graph.cc index f88cf40506..8e0dda9f4c 100644 --- a/modules/planning/tasks/dp_st_speed/dp_st_graph.cc +++ b/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::infinity(); bool CheckOverlapOnDpStGraph(const std::vector& 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(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(r_pre), - static_cast(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]); } } } -- GitLab