Planning: added initial implementation of poly_st_graph.

modules/planning/tasks/poly_st_speed/BUILD

@@ -2,6 +2,28 @@ load("//tools:cpplint.bzl", "cpplint")
 
 package(default_visibility = ["//visibility:public"])
 
+cc_library(
+    name = "poly_st_graph",
+    srcs = [
+        "poly_st_graph.cc",
+    ],
+    hdrs = [
+        "poly_st_graph.h",
+    ],
+    deps = [
+        "//modules/common:log",
+        "//modules/common/configs:vehicle_config_helper",
+        "//modules/common/configs/proto:vehicle_config_proto",
+        "//modules/common/proto:pnc_point_proto",
+        "//modules/localization/proto:pose_proto",
+        "//modules/map/hdmap",
+        "//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_speed_optimizer",
     srcs = [
@@ -11,6 +33,7 @@ cc_library(
         "poly_st_speed_optimizer.h",
     ],
     deps = [
+        ":poly_st_graph",
         "//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

+/******************************************************************************
+ * 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/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"
+
+#include "modules/common/configs/vehicle_config_helper.h"
+#include "modules/common/log.h"
+#include "modules/common/util/util.h"
+#include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
+
+namespace apollo {
+namespace planning {
+
+using apollo::common::ErrorCode;
+using apollo::common::Status;
+
+PolyStGraph::PolyStGraph(const PolyStSpeedConfig &config,
+                         const ReferenceLineInfo *reference_line_info)
+    : config_(config),
+      reference_line_info_(reference_line_info),
+      reference_line_(reference_line_info->reference_line()) {}
+
+bool PolyStGraph::FindStTunnel(
+    const common::TrajectoryPoint &init_point,
+    const std::vector<const PathObstacle *> &obstacles,
+    SpeedData *const speed_data) {
+  CHECK_NOTNULL(speed_data);
+
+  init_point_ = init_point;
+  std::vector<PolyStGraphNode> min_cost_path;
+  if (!GenerateMinCostSpeedProfile(obstacles, &min_cost_path)) {
+    AERROR << "Fail to generate graph!";
+    return false;
+  }
+  // TODO(All): implement this function
+  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
+  return true;
+}
+
+bool PolyStGraph::SampleStPoints(
+    std::vector<std::vector<STPoint>> *const points) {
+  CHECK_NOTNULL(points);
+  constexpr double start_t = 5.0;
+  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_) {
+      level_points.emplace_back(t, s);
+    }
+    points->push_back(std::move(level_points));
+  }
+  return true;
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/tasks/poly_st_speed/poly_st_graph.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
+ **/
+
+#ifndef MODULES_PLANNING_TASKS_POLY_ST_SPEED_POLY_ST_GRAPH_H_
+#define MODULES_PLANNING_TASKS_POLY_ST_SPEED_POLY_ST_GRAPH_H_
+
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "modules/common/proto/pnc_point.pb.h"
+#include "modules/planning/proto/poly_st_speed_config.pb.h"
+
+#include "modules/common/status/status.h"
+#include "modules/planning/common/path/path_data.h"
+#include "modules/planning/common/path_decision.h"
+#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/trajectory/discretized_trajectory.h"
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
+#include "modules/planning/reference_line/reference_point.h"
+
+namespace apollo {
+namespace planning {
+
+class PolyStGraph {
+ public:
+  explicit PolyStGraph(const PolyStSpeedConfig &config,
+                       const ReferenceLineInfo *reference_line_info);
+
+  ~PolyStGraph() = default;
+
+  bool FindStTunnel(const common::TrajectoryPoint &init_point,
+                    const std::vector<const PathObstacle *> &obstacles,
+                    SpeedData *const speed_data);
+
+ private:
+  /**
+   * an private inner struct for the poly st graph
+   */
+  struct PolyStGraphNode {
+   public:
+    PolyStGraphNode() = default;
+
+    PolyStGraphNode(const STPoint point_st, const double speed,
+                    const double accel)
+        : st_point(point_st), speed(speed), accel(accel) {}
+
+    STPoint st_point;
+    double speed = 0.0;
+    double accel = 0.0;
+    QuinticPolynomialCurve1d speed_profile;
+  };
+
+  bool GenerateMinCostSpeedProfile(
+      const std::vector<const PathObstacle *> &obstacles,
+      std::vector<PolyStGraphNode> *min_cost_path);
+
+  bool SampleStPoints(std::vector<std::vector<STPoint>> *const points);
+
+ private:
+  PolyStSpeedConfig config_;
+  common::TrajectoryPoint init_point_;
+  const ReferenceLineInfo *reference_line_info_ = nullptr;
+  const ReferenceLine &reference_line_;
+  SpeedData speed_data_;
+
+  double unit_t_ = 1.0;
+  double unit_s_ = 5.0;
+  double planning_distance_ = 100.0;
+  double planning_time_ = 6.0;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_TASKS_POLY_ST_SPEED_POLY_ST_GRAPH_H_

modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc

@@ -31,6 +31,7 @@
 #include "modules/common/util/file.h"
 #include "modules/common/vehicle_state/vehicle_state_provider.h"
 #include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/tasks/poly_st_speed/poly_st_graph.h"
 #include "modules/planning/tasks/st_graph/st_graph_data.h"
 
 namespace apollo {
@@ -109,7 +110,15 @@ 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_);
+  auto ret = poly_st_graph.FindStTunnel(
+      init_point,
+      reference_line_info_->path_decision()->path_obstacles().Items(),
+      speed_data);
+  if (!ret) {
+    return Status(ErrorCode::PLANNING_ERROR,
+                  "Fail to find st tunnel in PolyStGraph.");
+  }
   return Status::OK();
 }