Planning: added cost calculation for poly st graph. (#2064)

modules/planning/tasks/path_optimizer.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file path_optimizer.cpp
+ * @file
  **/
 
 #include "modules/planning/tasks/path_optimizer.h"

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 <algorithm>
-#include <unordered_map>
 
-#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<PolyStGraphNode> min_cost_path;
-  if (!GenerateMinCostSpeedProfile(obstacles, &min_cost_path)) {
-    AERROR << "Fail to generate graph!";
+
+  // sample end points
+  std::vector<std::vector<STPoint>> points;
+  if (!SampleStPoints(&points)) {
+    AERROR << "Fail to sample st points.";
     return false;
   }
 
-  std::vector<PolyStGraphNode> 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<std::vector<STPoint>> &points,
     const std::vector<const PathObstacle *> &obstacles,
-    std::vector<PolyStGraphNode> *min_cost_speed_profile) {
-  CHECK(min_cost_speed_profile != nullptr);
-  std::vector<std::vector<STPoint>> 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<double>::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;
         }
       }

modules/planning/tasks/poly_st_speed/poly_st_graph.h

@@ -71,8 +71,9 @@ class PolyStGraph {
   };
 
   bool GenerateMinCostSpeedProfile(
+      const std::vector<std::vector<STPoint>> &points,
       const std::vector<const PathObstacle *> &obstacles,
-      std::vector<PolyStGraphNode> *min_cost_path);
+      PolyStGraphNode *const min_cost_node);
 
   bool SampleStPoints(std::vector<std::vector<STPoint>> *const points);
 

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 <limits>
+
 namespace apollo {
 namespace planning {
+namespace {
+constexpr auto kInfCost = std::numeric_limits<double>::infinity();
+}
 
 using apollo::common::TrajectoryPoint;
 
 SpeedProfileCost::SpeedProfileCost(
     const PolyStSpeedConfig &config,
-    const std::vector<const PathObstacle *> &obstacles)
-    : config_(config), obstacles_(obstacles) {}
+    const std::vector<const PathObstacle *> &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

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<const PathObstacle *> &obstacles);
+                            const std::vector<const PathObstacle *> &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<const PathObstacle *> &obstacles_;
+  const SpeedLimit &speed_limit_;
 };
 
 }  // namespace planning