Planning: change params in speed profile cost to configurable params.

modules/planning/conf/planning_config.pb.txt

@@ -147,6 +147,16 @@ em_planner_config {
     poly_st_speed_config {
         total_path_length: 250.0
         total_time: 8.0
+
+        preferred_accel: 1.5
+        preferred_decel: -2.2
+        max_accel: 2.5
+        min_decel: -4.5
+
+        speed_limit_buffer: 0.05
+        speed_weight: 1.0
+        jerk_weight: 10.0
+
         st_boundary_config {
             boundary_buffer: 0.1
             high_speed_centric_acceleration_limit: 1.2

modules/planning/planner/em/BUILD

@@ -32,6 +32,7 @@ cc_library(
         "//modules/planning/tasks:task",
         "//modules/planning/tasks/dp_poly_path:dp_poly_path_optimizer",
         "//modules/planning/tasks/dp_st_speed:dp_st_speed_optimizer",
+        "//modules/planning/tasks/poly_st_speed:poly_st_speed_optimizer",
         "//modules/planning/tasks/path_decider",
         "//modules/planning/tasks/qp_spline_path",
         "//modules/planning/tasks/qp_spline_st_speed:qp_spline_st_speed_optimizer",

modules/planning/planner/em/em_planner.cc

@@ -36,6 +36,7 @@
 #include "modules/planning/tasks/dp_poly_path/dp_poly_path_optimizer.h"
 #include "modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.h"
 #include "modules/planning/tasks/path_decider/path_decider.h"
+#include "modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.h"
 #include "modules/planning/tasks/qp_spline_path/qp_spline_path_optimizer.h"
 #include "modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.h"
 #include "modules/planning/tasks/speed_decider/speed_decider.h"
@@ -64,11 +65,11 @@ void EMPlanner::RegisterTasks() {
                          []() -> Task* { return new DpStSpeedOptimizer(); });
   task_factory_.Register(SPEED_DECIDER,
                          []() -> Task* { return new SpeedDecider(); });
-  task_factory_.Register(QP_SPLINE_PATH_OPTIMIZER,
-                         []() -> Task* { return new QpSplinePathOptimizer(); });
   task_factory_.Register(QP_SPLINE_ST_SPEED_OPTIMIZER, []() -> Task* {
     return new QpSplineStSpeedOptimizer();
   });
+  task_factory_.Register(POLY_ST_SPEED_OPTIMIZER,
+                         []() -> Task* { return new PolyStSpeedOptimizer(); });
 }
 
 Status EMPlanner::Init(const PlanningConfig& config) {

modules/planning/proto/planning_config.proto

@@ -17,6 +17,7 @@ enum TaskType {
   TRAFFIC_DECIDER = 4;
   PATH_DECIDER = 5;
   SPEED_DECIDER = 6;
+  POLY_ST_SPEED_OPTIMIZER = 7;
 };
 
 message RuleConfig {

modules/planning/proto/poly_st_speed_config.proto

@@ -8,5 +8,12 @@ import "modules/planning/proto/st_boundary_config.proto";
 message PolyStSpeedConfig {
   optional double total_path_length = 1;
   optional double total_time = 2;
-  optional apollo.planning.StBoundaryConfig st_boundary_config = 3;
+  optional double preferred_accel = 3;
+  optional double preferred_decel = 4;
+  optional double max_accel = 5;
+  optional double min_decel = 6;
+  optional double speed_limit_buffer = 7;
+  optional double speed_weight = 8;
+  optional double jerk_weight = 9;
+  optional apollo.planning.StBoundaryConfig st_boundary_config = 10;
 }

modules/planning/tasks/poly_st_speed/poly_st_graph.cc

@@ -34,6 +34,9 @@
 
 namespace apollo {
 namespace planning {
+namespace {
+constexpr double kEpsilon = 1e-6;
+}
 
 using apollo::common::ErrorCode;
 using apollo::common::Status;
@@ -67,6 +70,7 @@ bool PolyStGraph::FindStTunnel(
     AERROR << "Fail to search min cost speed profile.";
     return false;
   }
+  speed_data->Clear();
   constexpr double delta_t = 0.1;  // output resolution, in seconds
   const auto curve = min_cost_node.speed_profile;
   for (double t = 0.0; t < planning_time_; t += delta_t) {
@@ -90,10 +94,12 @@ bool PolyStGraph::GenerateMinCostSpeedProfile(
   double min_cost = std::numeric_limits<double>::max();
   for (const auto &level : points) {
     for (const auto &st_point : level) {
+      AERROR << st_point.DebugString();
       const double speed_limit = speed_limit_.GetSpeedLimitByS(st_point.s());
       constexpr int num_speed = 5;
-      for (double v = 0; v < speed_limit;
-           v = std::fmin(speed_limit, v + speed_limit / num_speed)) {
+      for (double v = 0; v < speed_limit + kEpsilon;
+           v += speed_limit / num_speed) {
+        AERROR << "v = " << v;
         PolyStGraphNode node = {st_point, v, 0.0};
         node.speed_profile = QuinticPolynomialCurve1d(
             0.0, start_node.speed, start_node.accel, node.st_point.s(),
@@ -116,7 +122,7 @@ bool PolyStGraph::SampleStPoints(
   constexpr double start_s = 0.0;
   for (double t = start_t; t < planning_time_; t += unit_t_) {
     std::vector<STPoint> level_points;
-    for (double s = start_s; s < planning_distance_; s += unit_s_) {
+    for (double s = start_s; s < planning_distance_ + kEpsilon; s += unit_s_) {
       level_points.emplace_back(s, t);
     }
     points->push_back(std::move(level_points));

modules/planning/tasks/poly_st_speed/speed_profile_cost.cc

@@ -26,6 +26,7 @@ namespace apollo {
 namespace planning {
 namespace {
 constexpr auto kInfCost = std::numeric_limits<double>::infinity();
+constexpr double kEpsilon = 1e-6;
 }
 
 using apollo::common::TrajectoryPoint;
@@ -40,8 +41,7 @@ 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)) {
+  for (double t = kDeltaT; t < end_time + kEpsilon; t += kDeltaT) {
     cost += CalculatePointCost(curve, t);
   }
   return cost;
@@ -54,11 +54,15 @@ double SpeedProfileCost::CalculatePointCost(
   const double a = curve.Evaluate(2, t);
   const double da = curve.Evaluate(3, t);
 
+  if (s < 0.0) {
+    return kInfCost;
+  }
+
   const double speed_limit = speed_limit_.GetSpeedLimitByS(s);
-  if (v > speed_limit * 1.05) {
+  if (v < 0.0 || v > speed_limit * (1.0 + config_.speed_limit_buffer())) {
     return kInfCost;
   }
-  if (a > 1.5 || a < -4.5) {
+  if (a > config_.preferred_accel() || a < config_.preferred_decel()) {
     return kInfCost;
   }
   for (const auto *obstacle : obstacles_) {
@@ -68,10 +72,8 @@ double SpeedProfileCost::CalculatePointCost(
     }
   }
   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);
+  cost += config_.speed_weight() * std::pow((v - speed_limit), 2);
+  cost += config_.jerk_weight() * std::pow(da, 2);
   return cost;
 }