From 3682f6d85e71d0b9efcd1b33c76c09225c6b335f Mon Sep 17 00:00:00 2001
From: Zhang Liangliang <zhangliangliang@baidu.com>
Date: Tue, 1 Aug 2017 20:06:08 -0700
Subject: [PATCH] restructured qp_spline_st_graph.

---
 .../qp_spline_st_speed/qp_spline_st_graph.cc  | 43 ++++++++++++++-----
 .../qp_spline_st_speed/qp_spline_st_graph.h   | 12 +++++-
 2 files changed, 44 insertions(+), 11 deletions(-)

diff --git a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
index 517ee012c6..be9953bf5a 100644
--- a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
+++ b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
@@ -25,7 +25,7 @@
 #include <utility>
 
 #include "modules/common/log.h"
-#include "modules/planning/common/planning_util.h"
+
 namespace apollo {
 namespace planning {
 
@@ -41,6 +41,17 @@ QpSplineStGraph::QpSplineStGraph(
           _qp_spline_st_speed_config.total_time() /
           _qp_spline_st_speed_config.number_of_discrete_graph_t()) {}
 
+void QpSplineStGraph::Init() {
+  double curr_t = 0.0;
+  for (uint32_t i = 0;
+       i <= _qp_spline_st_speed_config.number_of_discrete_graph_t(); ++i) {
+    t_knots_.push_back(curr_t);
+    curr_t += time_resolution_;
+  }
+  _spline_generator.reset(new Spline1dGenerator(
+      t_knots_, _qp_spline_st_speed_config.spline_order()));
+}
+
 Status QpSplineStGraph::search(const StGraphData& st_graph_data,
                                const PathData& path_data,
                                SpeedData* const speed_data) {
@@ -53,16 +64,9 @@ Status QpSplineStGraph::search(const StGraphData& st_graph_data,
 
   // TODO(all): update speed limit here
   // TODO(all): update config through veh physical limit here generate knots
-  std::vector<double> t_knots;
-  double curr_knot = 0.0;
-  for (uint32_t i = 0;
-       i <= _qp_spline_st_speed_config.number_of_discrete_graph_t(); ++i) {
-    t_knots.push_back(curr_knot);
-    curr_knot += time_resolution_;
-  }
 
-  _spline_generator.reset(new Spline1dGenerator(
-      t_knots, _qp_spline_st_speed_config.spline_order()));
+  // initialize time resolution and
+  Init();
 
   if (!apply_constraint(st_graph_data.obs_boundary()).ok()) {
     const std::string msg = "Apply constraint failed!";
@@ -222,6 +226,25 @@ Status QpSplineStGraph::solve() {
              : Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::solve");
 }
 
+Status QpSplineStGraph::AddFollowReferenceLineKernel(
+    const StGraphBoundary& follow_boundary) {
+  if (follow_boundary.boundary_type() !=
+      StGraphBoundary::BoundaryType::FOLLOW) {
+    std::string msg = common::util::StrCat(
+        "Fail to add follow reference line kernel because boundary type is ",
+        "incorrect. type: ", static_cast<int>(follow_boundary.boundary_type()),
+        ".");
+    return Status(ErrorCode::PLANNING_ERROR, msg);
+  }
+
+  // TODO: (Liangliang) implement this dummy function.
+  auto* spline_kernel = _spline_generator->mutable_spline_kernel();
+  const std::vector<double> t_coord;
+  const std::vector<double> ref_s;
+  spline_kernel->add_reference_line_kernel_matrix(t_coord, ref_s, 1.0);
+  return Status::OK();
+}
+
 Status QpSplineStGraph::get_s_constraints_by_time(
     const std::vector<StGraphBoundary>& boundaries, const double time,
     const double total_path_s, double* const s_upper_bound,
diff --git a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h
index 7da59d9c2c..1a013d9ae5 100644
--- a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h
+++ b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h
@@ -26,10 +26,12 @@
 
 #include "modules/common/configs/proto/vehicle_config.pb.h"
 #include "modules/common/proto/pnc_point.pb.h"
-#include "modules/common/status/status.h"
 #include "modules/planning/proto/qp_spline_st_speed_config.pb.h"
 
+#include "modules/common/status/status.h"
+#include "modules/common/util/string_util.h"
 #include "modules/planning/common/path/path_data.h"
+#include "modules/planning/common/planning_util.h"
 #include "modules/planning/common/speed/speed_data.h"
 #include "modules/planning/math/smoothing_spline/spline_1d_generator.h"
 #include "modules/planning/optimizer/st_graph/st_graph_boundary.h"
@@ -47,6 +49,8 @@ class QpSplineStGraph {
                         const PathData& path_data, SpeedData* const speed_data);
 
  private:
+  void Init();
+
   // apply st graph constraint
   common::Status apply_constraint(
       const std::vector<StGraphBoundary>& boundaries);
@@ -66,6 +70,9 @@ class QpSplineStGraph {
   // generate reference speed profile
   common::Status apply_reference_speed_profile();
 
+  common::Status AddFollowReferenceLineKernel(
+      const StGraphBoundary& follow_boundary);
+
  private:
   // qp st configuration
   QpSplineStSpeedConfig _qp_spline_st_speed_config;
@@ -78,6 +85,9 @@ class QpSplineStGraph {
 
   // time resolution
   double time_resolution_ = 0.0;
+
+  // t_knots
+  std::vector<double> t_knots_;
 };
 
 }  // namespace planning
-- 
GitLab