Planning: refactored speed_limit, removed t info.

modules/planning/common/speed_limit.cc

@@ -32,42 +32,29 @@ namespace planning {
 
 using common::SpeedPoint;
 
-void SpeedLimit::AddSpeedLimit(const SpeedPoint& speed_point) {
-  if (!speed_points_.empty()) {
-    DCHECK_GE(speed_point.s(), speed_points_.back().s());
-    DCHECK_GE(speed_point.t(), speed_points_.back().t());
+void SpeedLimit::AddSpeedLimit(const double s, const double v) {
+  if (!speed_limit_points_.empty()) {
+    DCHECK_GE(s, speed_limit_points_.back().first);
   }
-  speed_points_.push_back(speed_point);
+  speed_limit_points_.emplace_back(s, v);
 }
 
-const std::vector<SpeedPoint>& SpeedLimit::speed_points() const {
-  return speed_points_;
+const std::vector<std::pair<double, double>>& SpeedLimit::speed_limit_points()
+    const {
+  return speed_limit_points_;
 }
 
 double SpeedLimit::GetSpeedLimitByS(const double s) const {
-  DCHECK_GE(speed_points_.size(), 2);
-  DCHECK_GE(s, speed_points_.front().s());
+  DCHECK_GE(speed_limit_points_.size(), 2);
+  DCHECK_GE(s, speed_limit_points_.front().first);
 
-  auto compare_s = [](const SpeedPoint& sp, const double s) {
-    return sp.s() < s;
+  auto compare_s = [](const std::pair<double, double> point, const double s) {
+    return point.first < s;
   };
 
-  auto it_lower = std::lower_bound(speed_points_.begin(), speed_points_.end(),
-                                   s, compare_s);
-  return it_lower->v();
-}
-
-double SpeedLimit::GetSpeedLimitByT(const double t) const {
-  DCHECK_GE(speed_points_.size(), 2);
-  DCHECK_GE(t, speed_points_.front().t());
-
-  auto compare_t = [](const SpeedPoint& sp, const double t) {
-    return sp.t() < t;
-  };
-
-  auto it_lower = std::lower_bound(speed_points_.begin(), speed_points_.end(),
-                                   t, compare_t);
-  return it_lower->v();
+  auto it_lower = std::lower_bound(speed_limit_points_.begin(),
+                                   speed_limit_points_.end(), s, compare_s);
+  return it_lower->second;
 }
 
 }  // namespace planning

modules/planning/common/speed_limit.h

@@ -31,16 +31,18 @@ namespace planning {
 class SpeedLimit {
  public:
   SpeedLimit() = default;
-  void AddSpeedLimit(const common::SpeedPoint& speed_point);
+  void AddSpeedLimit(const double s, const double v);
 
-  const std::vector<common::SpeedPoint>& speed_points() const;
+  const std::vector<std::pair<double, double>>& speed_limit_points() const;
   double GetSpeedLimitByS(const double s) const;
-  double GetSpeedLimitByT(const double t) const;
 
-  void Clear() { speed_points_.clear(); }
+  void Clear() { speed_limit_points_.clear(); }
 
  private:
-  std::vector<common::SpeedPoint> speed_points_;
+  // use a vector to represent speed limit
+  // the first number is s, the second number is v
+  // It means at distance s from the start point, the speed limit is v.
+  std::vector<std::pair<double, double>> speed_limit_points_;
 };
 
 }  // namespace planning

modules/planning/common/speed_limit_test.cc

@@ -32,11 +32,10 @@ class SpeedLimitTest : public ::testing::Test {
   virtual void SetUp() {
     speed_limit_.Clear();
     for (int i = 0; i < 100; ++i) {
-      SpeedPoint sp;
-      sp.set_s(i * 1.0);
-      sp.set_t(i * 0.5);
-      sp.set_v((i % 2 == 0) ? 5.0 : 10.0);
-      speed_limit_.AddSpeedLimit(std::move(sp));
+      std::pair<double, double> sp;
+      sp.first = i * 1.0;
+      sp.second = (i % 2 == 0) ? 5.0 : 10.0;
+      speed_limit_.AddSpeedLimit(sp.first, sp.second);
     }
   }
 
@@ -46,45 +45,28 @@ class SpeedLimitTest : public ::testing::Test {
 
 TEST_F(SpeedLimitTest, SimpleSpeedLimitCreation) {
   SpeedLimit simple_speed_limit;
-  EXPECT_TRUE(simple_speed_limit.speed_points().empty());
-  EXPECT_EQ(speed_limit_.speed_points().size(), 100);
+  EXPECT_TRUE(simple_speed_limit.speed_limit_points().empty());
+  EXPECT_EQ(speed_limit_.speed_limit_points().size(), 100);
 }
 
 TEST_F(SpeedLimitTest, GetSpeedLimitByS) {
-  EXPECT_EQ(speed_limit_.speed_points().size(), 100);
+  EXPECT_EQ(speed_limit_.speed_limit_points().size(), 100);
   double s = 0.0;
   const double ds = 0.01;
   while (s < 99.0) {
     double v_limit = speed_limit_.GetSpeedLimitByS(s);
 
     auto it_lower = std::lower_bound(
-        speed_limit_.speed_points().begin(), speed_limit_.speed_points().end(),
-        s, [](const SpeedPoint& point, const double curr_s) {
-          return point.s() < curr_s;
+        speed_limit_.speed_limit_points().begin(),
+        speed_limit_.speed_limit_points().end(), s,
+        [](const std::pair<double, double>& point, const double curr_s) {
+          return point.first < curr_s;
         });
 
-    EXPECT_DOUBLE_EQ(v_limit, it_lower->v());
+    EXPECT_DOUBLE_EQ(v_limit, it_lower->second);
     s += ds;
   }
 }
 
-TEST_F(SpeedLimitTest, GetSpeedLimitByT) {
-  EXPECT_EQ(speed_limit_.speed_points().size(), 100);
-  double t = 0.0;
-  const double dt = 0.01;
-  while (t < 49.0) {
-    double v_limit = speed_limit_.GetSpeedLimitByT(t);
-
-    auto it_lower = std::lower_bound(
-        speed_limit_.speed_points().begin(), speed_limit_.speed_points().end(),
-        t, [](const SpeedPoint& point, const double curr_t) {
-          return point.t() < curr_t;
-        });
-
-    EXPECT_DOUBLE_EQ(v_limit, it_lower->v());
-    t += dt;
-  }
-}
-
 }  // namespace planning
 }  // namespace apollo

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -21,9 +21,9 @@
 #include "modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h"
 
 #include <algorithm>
+#include <limits>
 #include <string>
 #include <utility>
-#include <limits>
 
 #include "modules/common/log.h"
 
@@ -254,7 +254,7 @@ Status QpSplineStGraph::AddCruiseReferenceLineKernel(
     const std::vector<double>& evaluate_t, const SpeedLimit& speed_limit) {
   auto* spline_kernel = spline_generator_->mutable_spline_kernel();
   std::vector<double> s_vec;
-  if (speed_limit.speed_points().size() == 0) {
+  if (speed_limit.speed_limit_points().size() == 0) {
     std::string msg = "Fail to apply_kernel due to empty speed limits.";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
@@ -344,17 +344,18 @@ Status QpSplineStGraph::EstimateSpeedUpperBound(
   uint32_t j = 0;
   double distance = 0.0;
   const double kDistanceEpsilon = 1e-6;
-  while (i < t_evaluated_.size() && j + 1 < speed_limit.speed_points().size()) {
+  while (i < t_evaluated_.size() &&
+         j + 1 < speed_limit.speed_limit_points().size()) {
     distance = v * t_evaluated_[i];
-    if (fabs(distance - speed_limit.speed_points()[j].s()) < kDistanceEpsilon) {
-      speed_upper_bound->push_back(speed_limit.speed_points()[j].v());
+    if (fabs(distance - speed_limit.speed_limit_points()[j].first) <
+        kDistanceEpsilon) {
+      speed_upper_bound->push_back(speed_limit.speed_limit_points()[j].second);
       ++i;
       ADEBUG << "speed upper bound:" << speed_upper_bound->back();
-  } else if (distance < speed_limit.speed_points()[j].s()) {
+    } else if (distance < speed_limit.speed_limit_points()[j].first) {
       ++i;
-    } else if (distance <= speed_limit.speed_points()[j + 1].s()) {
-      speed_upper_bound->push_back(
-          speed_limit.GetSpeedLimitByT(t_evaluated_[i]));
+    } else if (distance <= speed_limit.speed_limit_points()[j + 1].first) {
+      speed_upper_bound->push_back(speed_limit.GetSpeedLimitByS(distance));
       ADEBUG << "speed upper bound:" << speed_upper_bound->back();
       ++i;
     } else {

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -23,6 +23,8 @@
 #include <algorithm>
 #include <vector>
 
+#include "modules/common/proto/pnc_point.pb.h"
+
 #include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/util/file.h"
 #include "modules/common/vehicle_state/vehicle_state.h"
@@ -80,8 +82,12 @@ void QpSplineStSpeedOptimizer::RecordSTGraphDebug(
     }
   }
 
-  st_graph_debug->mutable_speed_limit()->CopyFrom(
-      {speed_limits.speed_points().begin(), speed_limits.speed_points().end()});
+  for (const auto& point : speed_limits.speed_limit_points()) {
+    common::SpeedPoint speed_point;
+    speed_point.set_s(point.first);
+    speed_point.set_v(point.second);
+    st_graph_debug->add_speed_limit()->CopyFrom(speed_point);
+  }
 
   st_graph_debug->mutable_speed_profile()->CopyFrom(
       {speed_data.speed_vector().begin(), speed_data.speed_vector().end()});

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -467,7 +467,6 @@ Status StBoundaryMapper::GetSpeedLimits(
   CHECK_NOTNULL(speed_limit_data);
 
   std::vector<double> speed_limits;
-  double curr_t = 0.0;
   for (const auto& path_point : path_data_.discretized_path().path_points()) {
     if (Double::Compare(path_point.s(), reference_line_.length()) > 0) {
       std::string msg = common::util::StrCat(
@@ -491,19 +490,7 @@ Status StBoundaryMapper::GetSpeedLimits(
         st_boundary_config_.lowest_speed(),
         std::fmin(speed_limit_on_path, speed_limit_on_reference_line));
 
-    common::SpeedPoint speed_point;
-
-    double delta_t = 0.0;
-    if (!speed_limit_data->speed_points().empty()) {
-      delta_t = (path_point.s() - speed_limit_data->speed_points().back().s()) /
-                curr_speed_limit;
-    }
-    speed_point.set_s(path_point.s());
-    speed_point.set_v(curr_speed_limit);
-    speed_point.set_t(curr_t);
-    speed_limit_data->AddSpeedLimit(speed_point);
-
-    curr_t += delta_t;
+    speed_limit_data->AddSpeedLimit(path_point.s(), curr_speed_limit);
   }
   return Status::OK();
 }