From fa33241485c3c5c6558b08477cc551dfdfe2a924 Mon Sep 17 00:00:00 2001 From: Liangliang Zhang Date: Mon, 25 Dec 2017 12:48:40 -0800 Subject: [PATCH] Planning: added cost calculation for poly st graph. (#2064) --- modules/planning/tasks/path_optimizer.cc | 2 +- .../tasks/poly_st_speed/poly_st_graph.cc | 35 ++++++------- .../tasks/poly_st_speed/poly_st_graph.h | 3 +- .../tasks/poly_st_speed/speed_profile_cost.cc | 50 +++++++++++++++++-- .../tasks/poly_st_speed/speed_profile_cost.h | 11 +++- 5 files changed, 72 insertions(+), 29 deletions(-) diff --git a/modules/planning/tasks/path_optimizer.cc b/modules/planning/tasks/path_optimizer.cc index 9dc0a0332a..c9c17e8250 100644 --- a/modules/planning/tasks/path_optimizer.cc +++ b/modules/planning/tasks/path_optimizer.cc @@ -15,7 +15,7 @@ *****************************************************************************/ /** - * @file path_optimizer.cpp + * @file **/ #include "modules/planning/tasks/path_optimizer.h" diff --git a/modules/planning/tasks/poly_st_speed/poly_st_graph.cc b/modules/planning/tasks/poly_st_speed/poly_st_graph.cc index 4fccb9c2a0..d2c85b0107 100644 --- a/modules/planning/tasks/poly_st_speed/poly_st_graph.cc +++ b/modules/planning/tasks/poly_st_speed/poly_st_graph.cc @@ -21,9 +21,7 @@ #include "modules/planning/tasks/poly_st_speed/poly_st_graph.h" #include -#include -#include "modules/common/proto/error_code.pb.h" #include "modules/common/proto/pnc_point.pb.h" #include "modules/planning/proto/planning_internal.pb.h" @@ -54,20 +52,23 @@ bool PolyStGraph::FindStTunnel( SpeedData *const speed_data) { CHECK_NOTNULL(speed_data); + // set init point init_point_ = init_point; - std::vector min_cost_path; - if (!GenerateMinCostSpeedProfile(obstacles, &min_cost_path)) { - AERROR << "Fail to generate graph!"; + + // sample end points + std::vector> points; + if (!SampleStPoints(&points)) { + AERROR << "Fail to sample st points."; return false; } - std::vector min_cost_speed_profile; - if (!GenerateMinCostSpeedProfile(obstacles, &min_cost_speed_profile)) { + PolyStGraphNode min_cost_node; + if (!GenerateMinCostSpeedProfile(points, obstacles, &min_cost_node)) { AERROR << "Fail to search min cost speed profile."; return false; } constexpr double delta_t = 0.1; // output resolution, in seconds - const auto curve = min_cost_speed_profile.back().speed_profile; + const auto curve = min_cost_node.speed_profile; for (double t = 0.0; t < planning_time_; t += delta_t) { const double s = curve.Evaluate(0, t); const double v = curve.Evaluate(1, t); @@ -79,19 +80,13 @@ bool PolyStGraph::FindStTunnel( } bool PolyStGraph::GenerateMinCostSpeedProfile( + const std::vector> &points, const std::vector &obstacles, - std::vector *min_cost_speed_profile) { - CHECK(min_cost_speed_profile != nullptr); - std::vector> points; - if (!SampleStPoints(&points)) { - AERROR << "Fail to sample st points."; - return false; - } + PolyStGraphNode *const min_cost_node) { + CHECK_NOTNULL(min_cost_node); PolyStGraphNode start_node = {STPoint(0.0, 0.0), init_point_.v(), init_point_.a()}; - min_cost_speed_profile->resize(2); - min_cost_speed_profile->front() = start_node; - SpeedProfileCost cost(config_, obstacles); + SpeedProfileCost cost(config_, obstacles, speed_limit_); double min_cost = std::numeric_limits::max(); for (const auto &level : points) { for (const auto &st_point : level) { @@ -103,9 +98,9 @@ bool PolyStGraph::GenerateMinCostSpeedProfile( node.speed_profile = QuinticPolynomialCurve1d( 0.0, start_node.speed, start_node.accel, node.st_point.s(), node.speed, node.accel, node.st_point.t()); - const double c = cost.Calculate(node.speed_profile); + const double c = cost.Calculate(node.speed_profile, st_point.t()); if (c < min_cost) { - min_cost_speed_profile->back() = node; + *min_cost_node = node; min_cost = c; } } diff --git a/modules/planning/tasks/poly_st_speed/poly_st_graph.h b/modules/planning/tasks/poly_st_speed/poly_st_graph.h index 7a9a3348e9..49469ba308 100644 --- a/modules/planning/tasks/poly_st_speed/poly_st_graph.h +++ b/modules/planning/tasks/poly_st_speed/poly_st_graph.h @@ -71,8 +71,9 @@ class PolyStGraph { }; bool GenerateMinCostSpeedProfile( + const std::vector> &points, const std::vector &obstacles, - std::vector *min_cost_path); + PolyStGraphNode *const min_cost_node); bool SampleStPoints(std::vector> *const points); diff --git a/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc b/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc index fc43db2a1f..3d2cf850c4 100644 --- a/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc +++ b/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc @@ -20,19 +20,59 @@ #include "modules/planning/tasks/poly_st_speed/speed_profile_cost.h" +#include + namespace apollo { namespace planning { +namespace { +constexpr auto kInfCost = std::numeric_limits::infinity(); +} using apollo::common::TrajectoryPoint; SpeedProfileCost::SpeedProfileCost( const PolyStSpeedConfig &config, - const std::vector &obstacles) - : config_(config), obstacles_(obstacles) {} + const std::vector &obstacles, + const SpeedLimit &speed_limit) + : config_(config), obstacles_(obstacles), speed_limit_(speed_limit) {} + +double SpeedProfileCost::Calculate(const QuinticPolynomialCurve1d &curve, + const double end_time) const { + double cost = 0.0; + constexpr double kDeltaT = 0.5; + for (double t = kDeltaT; t <= end_time; + t = std::fmin(end_time, t + end_time)) { + cost += CalculatePointCost(curve, t); + } + return cost; +} + +double SpeedProfileCost::CalculatePointCost( + const QuinticPolynomialCurve1d &curve, const double t) const { + const double s = curve.Evaluate(0, t); + const double v = curve.Evaluate(1, t); + const double a = curve.Evaluate(2, t); + const double da = curve.Evaluate(3, t); -double SpeedProfileCost::Calculate( - const QuinticPolynomialCurve1d &curve) const { - return 0.0; + const double speed_limit = speed_limit_.GetSpeedLimitByS(s); + if (v > speed_limit * 1.05) { + return kInfCost; + } + if (a > 1.5 || a < -4.5) { + return kInfCost; + } + for (const auto *obstacle : obstacles_) { + auto boundary = obstacle->st_boundary(); + if (boundary.IsPointInBoundary(STPoint(s, t))) { + return kInfCost; + } + } + double cost = 0.0; + constexpr double kSpeedCost = 1.0; + cost += kSpeedCost * std::pow((v - speed_limit), 2); + constexpr double kJerkCost = 1.0; + cost += kJerkCost * std::pow(da, 2); + return cost; } } // namespace planning diff --git a/modules/planning/tasks/poly_st_speed/speed_profile_cost.h b/modules/planning/tasks/poly_st_speed/speed_profile_cost.h index 4ed1f6dea2..7a38258eec 100644 --- a/modules/planning/tasks/poly_st_speed/speed_profile_cost.h +++ b/modules/planning/tasks/poly_st_speed/speed_profile_cost.h @@ -27,6 +27,7 @@ #include "modules/planning/common/obstacle.h" #include "modules/planning/common/path_decision.h" +#include "modules/planning/common/speed_limit.h" #include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h" namespace apollo { @@ -35,13 +36,19 @@ namespace planning { class SpeedProfileCost { public: explicit SpeedProfileCost(const PolyStSpeedConfig &config, - const std::vector &obstacles); + const std::vector &obstacles, + const SpeedLimit &speed_limit); - double Calculate(const QuinticPolynomialCurve1d &curve) const; + double Calculate(const QuinticPolynomialCurve1d &curve, + const double end_time) const; private: + double CalculatePointCost(const QuinticPolynomialCurve1d &curve, + const double t) const; + const PolyStSpeedConfig config_; const std::vector &obstacles_; + const SpeedLimit &speed_limit_; }; } // namespace planning -- GitLab