added get_speed_limit_by_s and by_t functions.

modules/planning/common/speed_limit.cc

@@ -18,9 +18,9 @@
  * @file speed_limit.cc
  **/
 
-#include "modules/planning/common/speed_limit.h"
 #include <algorithm>
 #include "modules/planning/common/planning_util.h"
+#include "modules/planning/common/speed_limit.h"
 
 namespace apollo {
 namespace planning {
@@ -46,7 +46,7 @@ const std::vector<SpeedPoint>& SpeedLimit::speed_limits() const {
   return _speed_point;
 }
 
-double SpeedLimit::get_speed_limit(const double s) const {
+double SpeedLimit::get_speed_limit_by_s(const double s) const {
   double ref_s = s;
   if (_speed_point.size() == 0) {
     return 0.0;
@@ -79,5 +79,40 @@ double SpeedLimit::get_speed_limit(const double s) const {
   }
   return util::interpolate(*(it_lower - 1), *it_lower, weight).v();
 }
+
+double SpeedLimit::get_speed_limit_by_t(const double t) const {
+  double ref_t = t;
+  if (_speed_point.size() == 0) {
+    return 0.0;
+  }
+
+  if (_speed_point.size() == 1) {
+    return _speed_point.front().v();
+  }
+
+  if (ref_t > _speed_point.back().t()) {
+    return _speed_point.back().v();
+  }
+
+  if (ref_t < _speed_point.front().t()) {
+    return _speed_point.front().v();
+  }
+
+  auto func = [](const SpeedPoint& sp, const double t) { return sp.t() < t; };
+
+  auto it_lower =
+      std::lower_bound(_speed_point.begin(), _speed_point.end(), t, func);
+  if (it_lower == _speed_point.begin()) {
+    return _speed_point.front().v();
+  }
+
+  double weight = 0.0;
+  double range = (*it_lower).t() - (*(it_lower - 1)).t();
+  if (range > 0) {
+    weight = (t - (*(it_lower - 1)).t()) / range;
+  }
+  return util::interpolate(*(it_lower - 1), *it_lower, weight).v();
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/speed_limit.h

@@ -35,7 +35,8 @@ class SpeedLimit {
   void add_speed_limit(const double s, const double t, const double v,
                        const double a, const double da);
   const std::vector<common::SpeedPoint>& speed_limits() const;
-  double get_speed_limit(const double s) const;
+  double get_speed_limit_by_s(const double s) const;
+  double get_speed_limit_by_t(const double t) const;
 
  private:
   std::vector<common::SpeedPoint> _speed_point;

modules/planning/optimizer/dp_st_speed/dp_st_graph.cc

@@ -150,7 +150,8 @@ void DpStGraph::CalculateCostAt(const StGraphData& st_graph_data,
   }
 
   // TODO(all): get speed limit from mapper
-  double speed_limit = st_graph_data.speed_limit().get_speed_limit(unit_s_ * r);
+  double speed_limit =
+      st_graph_data.speed_limit().get_speed_limit_by_s(unit_s_ * r);
   if (c == 1) {
     cost_table_[c][r].set_total_cost(
         cost_table_[c][r].obstacle_cost() + cost_table_[0][0].total_cost() +

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -88,9 +88,9 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
   SpeedLimit speed_limit;
   if (!boundary_mapper_
            .get_speed_limits(common::VehicleState::instance()->pose(),
-                             Frame::PncMap(), path_data, planning_distance,
-                             dp_st_speed_config_.matrix_dimension_s(),
-                             dp_st_speed_config_.max_speed(), &speed_limit)
+                                  Frame::PncMap(), path_data, planning_distance,
+                                  dp_st_speed_config_.matrix_dimension_s(),
+                                  dp_st_speed_config_.max_speed(), &speed_limit)
            .ok()) {
     const std::string msg =
         "Getting speed limits for dp st speed optimizer failed!";

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -74,7 +74,8 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (!ApplyKernel(st_graph_data.speed_limit()).ok()) {
+  if (!ApplyKernel(st_graph_data.obs_boundary(), st_graph_data.speed_limit())
+           .ok()) {
     const std::string msg = "Apply kernel failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
@@ -211,7 +212,7 @@ Status QpSplineStGraph::ApplyKernel(
   for (uint32_t i = 0;
        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);
+    dist_ref += time_resolution_ * speed_limit.get_speed_limit_by_s(dist_ref);
   }
   // TODO: change reference line kernel to configurable version
   spline_kernel->add_reference_line_kernel_matrix(t_knots_, s_vec, 1);
@@ -277,5 +278,14 @@ Status QpSplineStGraph::GetSConstraintByTime(
 
   return Status::OK();
 }
+
+Status QpSplineStGraph::EstimateSpeedConstraint(
+    const StGraphData& st_graph_data, const PathData& path_data,
+    const SpeedLimit& speed_limit,
+    std::vector<double>* speed_constraint) const {
+  // TODO: (Liangliang) implement this function.
+  return Status::OK();
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h

@@ -56,7 +56,8 @@ class QpSplineStGraph {
       const std::vector<StGraphBoundary>& boundaries);
 
   // apply objective function
-  common::Status ApplyKernel(const SpeedLimit& speed_limit);
+  common::Status ApplyKernel(const std::vector<StGraphBoundary>& boundaries,
+                             const SpeedLimit& speed_limit);
 
   // solve
   common::Status Solve();
@@ -73,6 +74,11 @@ class QpSplineStGraph {
   common::Status AddFollowReferenceLineKernel(
       const StGraphBoundary& follow_boundary);
 
+  common::Status EstimateSpeedConstraint(
+      const StGraphData& st_graph_data, const PathData& path_data,
+      const SpeedLimit& speed_limit,
+      std::vector<double>* speed_constraint) const;
+
  private:
   // qp st configuration
   QpSplineStSpeedConfig qp_spline_st_speed_config_;