Planning: implemented poly_st_graph. Added init files for speed_profile_cost.h/cc

modules/planning/tasks/poly_st_speed/BUILD

@@ -2,6 +2,22 @@ load("//tools:cpplint.bzl", "cpplint")
 
 package(default_visibility = ["//visibility:public"])
 
+cc_library(
+    name = "speed_profile_cost",
+    srcs = [
+        "speed_profile_cost.cc",
+    ],
+    hdrs = [
+        "speed_profile_cost.h",
+    ],
+    deps = [
+        "//modules/planning/proto:planning_config_proto",
+        "//modules/planning/proto:poly_st_speed_config_proto",
+        "//modules/planning/tasks:speed_optimizer",
+        "//modules/planning/tasks/st_graph:st_boundary_mapper",
+    ],
+)
+
 cc_library(
     name = "poly_st_graph",
     srcs = [
@@ -11,6 +27,7 @@ cc_library(
         "poly_st_graph.h",
     ],
     deps = [
+        ":speed_profile_cost",
         "//modules/common:log",
         "//modules/common/configs:vehicle_config_helper",
         "//modules/common/configs/proto:vehicle_config_proto",

modules/planning/tasks/poly_st_speed/poly_st_graph.cc

@@ -32,6 +32,7 @@
 #include "modules/common/util/util.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
+#include "modules/planning/tasks/poly_st_speed/speed_profile_cost.h"
 
 namespace apollo {
 namespace planning {
@@ -40,10 +41,12 @@ using apollo::common::ErrorCode;
 using apollo::common::Status;
 
 PolyStGraph::PolyStGraph(const PolyStSpeedConfig &config,
-                         const ReferenceLineInfo *reference_line_info)
+                         const ReferenceLineInfo *reference_line_info,
+                         const SpeedLimit &speed_limit)
     : config_(config),
       reference_line_info_(reference_line_info),
-      reference_line_(reference_line_info->reference_line()) {}
+      reference_line_(reference_line_info->reference_line()),
+      speed_limit_(speed_limit) {}
 
 bool PolyStGraph::FindStTunnel(
     const common::TrajectoryPoint &init_point,
@@ -57,15 +60,57 @@ bool PolyStGraph::FindStTunnel(
     AERROR << "Fail to generate graph!";
     return false;
   }
-  // TODO(All): implement this function
+
+  std::vector<PolyStGraphNode> min_cost_speed_profile;
+  if (!GenerateMinCostSpeedProfile(obstacles, &min_cost_speed_profile)) {
+    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;
+  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);
+    const double a = curve.Evaluate(2, t);
+    const double da = curve.Evaluate(3, t);
+    speed_data->AppendSpeedPoint(s, t, v, a, da);
+  }
   return true;
 }
 
 bool PolyStGraph::GenerateMinCostSpeedProfile(
     const std::vector<const PathObstacle *> &obstacles,
-    std::vector<PolyStGraphNode> *min_cost_path) {
-  CHECK(min_cost_path != nullptr);
-  // TODO(All): implement this function
+    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 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);
+  double min_cost = std::numeric_limits<double>::max();
+  for (const auto &level : points) {
+    for (const auto &st_point : level) {
+      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)) {
+        PolyStGraphNode node = {st_point, v, 0.0};
+        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);
+        if (c < min_cost) {
+          min_cost_speed_profile->back() = node;
+          min_cost = c;
+        }
+      }
+    }
+  }
   return true;
 }
 
@@ -77,7 +122,7 @@ bool PolyStGraph::SampleStPoints(
   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_) {
-      level_points.emplace_back(t, s);
+      level_points.emplace_back(s, t);
     }
     points->push_back(std::move(level_points));
   }

modules/planning/tasks/poly_st_speed/poly_st_graph.h

@@ -35,6 +35,7 @@
 #include "modules/planning/common/path_obstacle.h"
 #include "modules/planning/common/reference_line_info.h"
 #include "modules/planning/common/speed/speed_data.h"
+#include "modules/planning/common/speed_limit.h"
 #include "modules/planning/common/trajectory/discretized_trajectory.h"
 #include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
 #include "modules/planning/reference_line/reference_point.h"
@@ -45,7 +46,8 @@ namespace planning {
 class PolyStGraph {
  public:
   explicit PolyStGraph(const PolyStSpeedConfig &config,
-                       const ReferenceLineInfo *reference_line_info);
+                       const ReferenceLineInfo *reference_line_info,
+                       const SpeedLimit &speed_limit);
 
   ~PolyStGraph() = default;
 
@@ -54,9 +56,6 @@ class PolyStGraph {
                     SpeedData *const speed_data);
 
  private:
-  /**
-   * an private inner struct for the poly st graph
-   */
   struct PolyStGraphNode {
    public:
     PolyStGraphNode() = default;
@@ -82,7 +81,7 @@ class PolyStGraph {
   common::TrajectoryPoint init_point_;
   const ReferenceLineInfo *reference_line_info_ = nullptr;
   const ReferenceLine &reference_line_;
-  SpeedData speed_data_;
+  const SpeedLimit &speed_limit_;
 
   double unit_t_ = 1.0;
   double unit_s_ = 5.0;

modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc

@@ -110,7 +110,8 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
 
   // step 2 perform graph search
   // make a poly_st_graph and perform search here.
-  PolyStGraph poly_st_graph(poly_st_speed_config_, reference_line_info_);
+  PolyStGraph poly_st_graph(poly_st_speed_config_, reference_line_info_,
+                            speed_limits);
   auto ret = poly_st_graph.FindStTunnel(
       init_point,
       reference_line_info_->path_decision()->path_obstacles().Items(),

modules/planning/tasks/poly_st_speed/speed_profile_cost.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#include "modules/planning/tasks/poly_st_speed/speed_profile_cost.h"
+
+namespace apollo {
+namespace planning {
+
+using apollo::common::TrajectoryPoint;
+
+SpeedProfileCost::SpeedProfileCost(
+    const PolyStSpeedConfig &config,
+    const std::vector<const PathObstacle *> &obstacles)
+    : config_(config), obstacles_(obstacles) {}
+
+double SpeedProfileCost::Calculate(
+    const QuinticPolynomialCurve1d &curve) const {
+  return 0.0;
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/tasks/poly_st_speed/speed_profile_cost.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file trajectory_cost.h
+ **/
+
+#ifndef MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_
+#define MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_
+
+#include <vector>
+
+#include "modules/planning/proto/poly_st_speed_config.pb.h"
+
+#include "modules/planning/common/obstacle.h"
+#include "modules/planning/common/path_decision.h"
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
+
+namespace apollo {
+namespace planning {
+
+class SpeedProfileCost {
+ public:
+  explicit SpeedProfileCost(const PolyStSpeedConfig &config,
+                            const std::vector<const PathObstacle *> &obstacles);
+
+  double Calculate(const QuinticPolynomialCurve1d &curve) const;
+
+ private:
+  const PolyStSpeedConfig config_;
+  const std::vector<const PathObstacle *> &obstacles_;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_