use t_knots_ in qp_spline_st_graph

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -36,29 +36,29 @@ using apollo::common::VehicleParam;
 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),
+    : qp_spline_st_speed_config_(qp_spline_st_speed_config),
       time_resolution_(
-          _qp_spline_st_speed_config.total_time() /
-          _qp_spline_st_speed_config.number_of_discrete_graph_t()) {}
+          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) {
+       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()));
+  spline_generator_.reset(new Spline1dGenerator(
+      t_knots_, qp_spline_st_speed_config_.spline_order()));
 }
 
-Status QpSplineStGraph::search(const StGraphData& st_graph_data,
+Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
                                const PathData& path_data,
                                SpeedData* const speed_data) {
-  _init_point = st_graph_data.init_point();
+  init_point_ = st_graph_data.init_point();
   if (st_graph_data.path_data_length() <
-      _qp_spline_st_speed_config.total_path_length()) {
-    _qp_spline_st_speed_config.set_total_path_length(
+      qp_spline_st_speed_config_.total_path_length()) {
+    qp_spline_st_speed_config_.set_total_path_length(
         st_graph_data.path_data_length());
   }
 
@@ -68,19 +68,19 @@ Status QpSplineStGraph::search(const StGraphData& st_graph_data,
   // initialize time resolution and
   Init();
 
-  if (!apply_constraint(st_graph_data.obs_boundary()).ok()) {
+  if (!ApplyConstraint(st_graph_data.obs_boundary()).ok()) {
     const std::string msg = "Apply constraint failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (!apply_kernel(st_graph_data.speed_limit()).ok()) {
+  if (!ApplyKernel(st_graph_data.speed_limit()).ok()) {
     const std::string msg = "Apply kernel failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (!solve().ok()) {
+  if (!Solve().ok()) {
     const std::string msg = "Solve qp problem failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
@@ -88,11 +88,11 @@ Status QpSplineStGraph::search(const StGraphData& st_graph_data,
 
   // extract output
   speed_data->Clear();
-  const Spline1d& spline = _spline_generator->spline();
+  const Spline1d& spline = spline_generator_->spline();
 
-  double time_resolution = _qp_spline_st_speed_config.output_time_resolution();
+  double time_resolution = qp_spline_st_speed_config_.output_time_resolution();
   double time = 0.0;
-  while (time < _qp_spline_st_speed_config.total_time() + time_resolution) {
+  while (time < qp_spline_st_speed_config_.total_time() + time_resolution) {
     double s = spline(time);
     double v = spline.derivative(time);
     double a = spline.second_order_derivative(time);
@@ -104,10 +104,10 @@ Status QpSplineStGraph::search(const StGraphData& st_graph_data,
   return Status::OK();
 }
 
-Status QpSplineStGraph::apply_constraint(
+Status QpSplineStGraph::ApplyConstraint(
     const std::vector<StGraphBoundary>& boundaries) {
   Spline1dConstraint* constraint =
-      _spline_generator->mutable_spline_constraint();
+      spline_generator_->mutable_spline_constraint();
   // position, velocity, acceleration
 
   if (!constraint->add_point_fx_constraint(0.0, 0.0)) {
@@ -116,14 +116,14 @@ Status QpSplineStGraph::apply_constraint(
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (!constraint->add_point_derivative_constraint(0.0, _init_point.v())) {
+  if (!constraint->add_point_derivative_constraint(0.0, init_point_.v())) {
     const std::string msg = "add st start point velocity constraint failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
   if (!constraint->add_point_second_derivative_constraint(0.0,
-                                                          _init_point.a())) {
+                                                          init_point_.a())) {
     const std::string msg =
         "add st start point acceleration constraint failed!";
     AERROR << msg;
@@ -131,7 +131,7 @@ Status QpSplineStGraph::apply_constraint(
   }
 
   if (!constraint->add_point_second_derivative_constraint(
-          _spline_generator->spline().x_knots().back(), 0.0)) {
+          spline_generator_->spline().x_knots().back(), 0.0)) {
     const std::string msg = "add st end point acceleration constraint failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
@@ -152,26 +152,19 @@ Status QpSplineStGraph::apply_constraint(
   }
 
   // boundary constraint
-  double evaluated_resolution =
-      _qp_spline_st_speed_config.total_time() /
-      _qp_spline_st_speed_config.number_of_evaluated_graph_t();
-
-  std::vector<double> evaluated_knots;
   std::vector<double> s_upper_bound;
   std::vector<double> s_lower_bound;
-  for (uint32_t i = 1;
-       i <= _qp_spline_st_speed_config.number_of_evaluated_graph_t(); ++i) {
-    evaluated_knots.push_back(i * evaluated_resolution);
+
+  for (const double curr_t : t_knots_) {
     double lower_s = 0.0;
     double upper_s = 0.0;
-    get_s_constraints_by_time(boundaries, evaluated_knots.back(),
-                              _qp_spline_st_speed_config.total_path_length(),
-                              &upper_s, &lower_s);
+    GetSConstraintByTime(boundaries, curr_t,
+                         qp_spline_st_speed_config_.total_path_length(),
+                         &upper_s, &lower_s);
     s_upper_bound.push_back(upper_s);
     s_lower_bound.push_back(lower_s);
   }
-  if (!constraint->add_fx_boundary(evaluated_knots, s_lower_bound,
-                                   s_upper_bound)) {
+  if (!constraint->add_fx_boundary(t_knots_, s_lower_bound, s_upper_bound)) {
     const std::string msg = "Fail to apply obstacle constraint";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
@@ -181,47 +174,43 @@ Status QpSplineStGraph::apply_constraint(
   return Status::OK();
 }
 
-Status QpSplineStGraph::apply_kernel(const SpeedLimit& speed_limit) {
-  Spline1dKernel* spline_kernel = _spline_generator->mutable_spline_kernel();
+Status QpSplineStGraph::ApplyKernel(const SpeedLimit& speed_limit) {
+  Spline1dKernel* spline_kernel = spline_generator_->mutable_spline_kernel();
 
-  if (_qp_spline_st_speed_config.speed_kernel_weight() > 0) {
+  if (qp_spline_st_speed_config_.speed_kernel_weight() > 0) {
     spline_kernel->add_derivative_kernel_matrix(
-        _qp_spline_st_speed_config.speed_kernel_weight());
+        qp_spline_st_speed_config_.speed_kernel_weight());
   }
 
-  if (_qp_spline_st_speed_config.accel_kernel_weight() > 0) {
+  if (qp_spline_st_speed_config_.accel_kernel_weight() > 0) {
     spline_kernel->add_second_order_derivative_matrix(
-        _qp_spline_st_speed_config.accel_kernel_weight());
+        qp_spline_st_speed_config_.accel_kernel_weight());
   }
 
-  if (_qp_spline_st_speed_config.jerk_kernel_weight() > 0) {
+  if (qp_spline_st_speed_config_.jerk_kernel_weight() > 0) {
     spline_kernel->add_third_order_derivative_matrix(
-        _qp_spline_st_speed_config.jerk_kernel_weight());
+        qp_spline_st_speed_config_.jerk_kernel_weight());
   }
 
   // TODO(all): add reference speed profile for different main decision
-  std::vector<double> t_knots;
   std::vector<double> s_vec;
   if (speed_limit.speed_limits().size() == 0) {
-    return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::apply_kernel");
+    return Status(ErrorCode::PLANNING_ERROR,
+                  "Fail to apply_kernel due to empty speed limits.");
   }
-
   double dist_ref = 0.0;
-  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);
+       i <= qp_spline_st_speed_config_.number_of_discrete_graph_t(); ++i) {
     s_vec.push_back(dist_ref);
     dist_ref += time_resolution_ * speed_limit.get_speed_limit(dist_ref);
-    curr_knot += time_resolution_;
   }
   // TODO: change reference line kernel to configurable version
-  spline_kernel->add_reference_line_kernel_matrix(t_knots, s_vec, 1);
+  spline_kernel->add_reference_line_kernel_matrix(t_knots_, s_vec, 1);
   return Status::OK();
 }
 
-Status QpSplineStGraph::solve() {
-  return _spline_generator->solve()
+Status QpSplineStGraph::Solve() {
+  return spline_generator_->solve()
              ? Status::OK()
              : Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::solve");
 }
@@ -237,20 +226,27 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
     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);
+  auto* spline_kernel = spline_generator_->mutable_spline_kernel();
+  std::vector<double> ref_s;
+  for (const double curr_t : t_knots_) {
+    double s_upper = 0.0;
+    double s_lower = 0.0;
+    if (follow_boundary.get_s_boundary_position(curr_t, &s_upper, &s_lower)) {
+      ref_s.push_back(s_upper - follow_boundary.characteristic_length());
+    }
+    ref_s.push_back(s_upper);
+  }
+
+  spline_kernel->add_reference_line_kernel_matrix(t_knots_, ref_s, 1.0);
   return Status::OK();
 }
 
-Status QpSplineStGraph::get_s_constraints_by_time(
+Status QpSplineStGraph::GetSConstraintByTime(
     const std::vector<StGraphBoundary>& boundaries, const double time,
     const double total_path_s, double* const s_upper_bound,
     double* const s_lower_bound) const {
   *s_upper_bound =
-      std::min(total_path_s, time * _qp_spline_st_speed_config.max_speed());
+      std::min(total_path_s, time * qp_spline_st_speed_config_.max_speed());
 
   for (const StGraphBoundary& boundary : boundaries) {
     double s_upper = 0.0;

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h

@@ -45,43 +45,43 @@ class QpSplineStGraph {
   QpSplineStGraph(const QpSplineStSpeedConfig& qp_config,
                   const apollo::common::VehicleParam& veh_param);
 
-  common::Status search(const StGraphData& st_graph_data,
+  common::Status Search(const StGraphData& st_graph_data,
                         const PathData& path_data, SpeedData* const speed_data);
 
  private:
   void Init();
 
   // apply st graph constraint
-  common::Status apply_constraint(
+  common::Status ApplyConstraint(
       const std::vector<StGraphBoundary>& boundaries);
 
   // apply objective function
-  common::Status apply_kernel(const SpeedLimit& speed_limit);
+  common::Status ApplyKernel(const SpeedLimit& speed_limit);
 
   // solve
-  common::Status solve();
+  common::Status Solve();
 
   // extract upper lower bound for constraint;
-  common::Status get_s_constraints_by_time(
+  common::Status GetSConstraintByTime(
       const std::vector<StGraphBoundary>& boundaries, const double time,
       const double total_path_s, double* const s_upper_bound,
       double* const s_lower_bound) const;
 
   // generate reference speed profile
-  common::Status apply_reference_speed_profile();
+  // common::Status ApplyReferenceSpeedProfile();
 
   common::Status AddFollowReferenceLineKernel(
       const StGraphBoundary& follow_boundary);
 
  private:
   // qp st configuration
-  QpSplineStSpeedConfig _qp_spline_st_speed_config;
+  QpSplineStSpeedConfig qp_spline_st_speed_config_;
 
   // initial status
-  common::TrajectoryPoint _init_point;
+  common::TrajectoryPoint init_point_;
 
   // solver
-  std::unique_ptr<Spline1dGenerator> _spline_generator = nullptr;
+  std::unique_ptr<Spline1dGenerator> spline_generator_ = nullptr;
 
   // time resolution
   double time_resolution_ = 0.0;

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -102,7 +102,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
 
   StGraphData st_graph_data(boundaries, init_point, speed_limits,
                             path_data.discretized_path().length());
-  if (st_graph.search(st_graph_data, path_data, speed_data) != Status::OK()) {
+  if (st_graph.Search(st_graph_data, path_data, speed_data) != Status::OK()) {
     return Status(ErrorCode::PLANNING_ERROR,
                   "Failed to search graph with dynamic programming!");
   }