From b9b28bf1019e3813ac260ae1a9fa9f2e666f28ec Mon Sep 17 00:00:00 2001
From: Zhang Liangliang <zhangliangliang@baidu.com>
Date: Wed, 2 Aug 2017 10:21:49 -0700
Subject: [PATCH] used different evaluation and konts number for
 qp_spline_st_speed

---
 .../qp_spline_st_speed_config_file.pb.txt     |  4 ++--
 .../qp_spline_st_speed/qp_spline_st_graph.cc  | 24 +++++++++++++++----
 .../qp_spline_st_speed/qp_spline_st_graph.h   |  6 ++++-
 3 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/modules/planning/conf/qp_spline_st_speed_config_file.pb.txt b/modules/planning/conf/qp_spline_st_speed_config_file.pb.txt
index f76e995c04..31ab886edd 100644
--- a/modules/planning/conf/qp_spline_st_speed_config_file.pb.txt
+++ b/modules/planning/conf/qp_spline_st_speed_config_file.pb.txt
@@ -2,8 +2,8 @@ total_path_length: 80.0
 total_time: 8.0
 output_time_resolution: 0.05
 number_of_discrete_graph_s: 80
-number_of_discrete_graph_t: 20
-number_of_evaluated_graph_t: 10
+number_of_discrete_graph_t: 10
+number_of_evaluated_graph_t: 15
 spline_order: 6
 speed_kernel_weight: 0.0
 accel_kernel_weight: 0.0
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 c94ead94d1..a907377278 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
@@ -37,17 +37,32 @@ QpSplineStGraph::QpSplineStGraph(
     const QpSplineStSpeedConfig& qp_spline_st_speed_config,
     const apollo::common::VehicleParam& veh_param)
     : qp_spline_st_speed_config_(qp_spline_st_speed_config),
-      time_resolution_(
+      time_resolution_(qp_spline_st_speed_config_.total_time() /
+                       qp_spline_st_speed_config_.number_of_discrete_graph_t()),
+      evaluation_time_resolution_(
           qp_spline_st_speed_config_.total_time() /
-          qp_spline_st_speed_config_.number_of_discrete_graph_t()) {}
+          qp_spline_st_speed_config_.number_of_evaluated_graph_t())
+
+{}
 
 void QpSplineStGraph::Init() {
+  // init knots
   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_;
   }
+
+  // init evaluated t positions
+  curr_t = 0.0;
+  for (uint32_t i = 0;
+       i <= qp_spline_st_speed_config_.number_of_evaluated_graph_t(); ++i) {
+    t_evaluated_.push_back(curr_t);
+    curr_t += evaluation_time_resolution_;
+  }
+
+  // init spline generator
   spline_generator_.reset(new Spline1dGenerator(
       t_knots_, qp_spline_st_speed_config_.spline_order()));
 }
@@ -156,7 +171,7 @@ Status QpSplineStGraph::ApplyConstraint(
   std::vector<double> s_upper_bound;
   std::vector<double> s_lower_bound;
 
-  for (const double curr_t : t_knots_) {
+  for (const double curr_t : t_evaluated_) {
     double lower_s = 0.0;
     double upper_s = 0.0;
     GetSConstraintByTime(boundaries, curr_t,
@@ -165,7 +180,8 @@ Status QpSplineStGraph::ApplyConstraint(
     s_upper_bound.push_back(upper_s);
     s_lower_bound.push_back(lower_s);
   }
-  if (!constraint->add_fx_boundary(t_knots_, s_lower_bound, s_upper_bound)) {
+  if (!constraint->add_fx_boundary(t_evaluated_, s_lower_bound,
+                                   s_upper_bound)) {
     const std::string msg = "Fail to apply obstacle constraint";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
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 5e85ccee80..7220306438 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
@@ -92,8 +92,12 @@ class QpSplineStGraph {
   // time resolution
   double time_resolution_ = 0.0;
 
-  // t_knots
+  // knots
   std::vector<double> t_knots_;
+
+  double evaluation_time_resolution_ = 0.0;
+  // evaluated points
+  std::vector<double> t_evaluated_;
 };
 
 }  // namespace planning
-- 
GitLab