Planning: use linear interpolation as default method.

modules/common/math/linear_interpolation.cc

@@ -25,9 +25,6 @@ namespace apollo {
 namespace common {
 namespace math {
 
-using apollo::common::PathPoint;
-using apollo::common::TrajectoryPoint;
-
 double slerp(const double a0, const double t0, const double a1, const double t1,
              const double t) {
   if (std::abs(t1 - t0) <= kMathEpsilon) {
@@ -48,12 +45,22 @@ double slerp(const double a0, const double t0, const double a1, const double t1,
   return NormalizeAngle(a);
 }
 
+SLPoint InterpolateUsingLinearApproximation(const SLPoint &p0,
+                                            const SLPoint &p1, const double w) {
+  CHECK_GE(w, 0.0);
+
+  SLPoint p;
+  p.set_s((1 - w) * p0.s() + w * p1.s());
+  p.set_l((1 - w) * p0.l() + w * p1.l());
+  return p;
+}
+
 PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0,
                                               const PathPoint &p1,
                                               const double s) {
   double s0 = p0.s();
   double s1 = p1.s();
-  CHECK(s0 < s1);
+  CHECK_LE(s0, s1);
 
   PathPoint path_point;
   double weight = (s - s0) / (s1 - s0);
@@ -91,22 +98,18 @@ TrajectoryPoint InterpolateUsingLinearApproximation(const TrajectoryPoint &tp0,
   double t1 = tp1.relative_time();
 
   TrajectoryPoint tp;
-  tp.set_v(common::math::lerp(tp0.v(), t0, tp1.v(), t1, t));
-  tp.set_a(common::math::lerp(tp0.a(), t0, tp1.a(), t1, t));
+  tp.set_v(lerp(tp0.v(), t0, tp1.v(), t1, t));
+  tp.set_a(lerp(tp0.a(), t0, tp1.a(), t1, t));
   tp.set_relative_time(t);
 
   PathPoint *path_point = tp.mutable_path_point();
-  path_point->set_x(common::math::lerp(pp0.x(), t0, pp1.x(), t1, t));
-  path_point->set_y(common::math::lerp(pp0.y(), t0, pp1.y(), t1, t));
-  path_point->set_theta(
-      common::math::lerp(pp0.theta(), t0, pp1.theta(), t1, t));
-  path_point->set_kappa(
-      common::math::lerp(pp0.kappa(), t0, pp1.kappa(), t1, t));
-  path_point->set_dkappa(
-      common::math::lerp(pp0.dkappa(), t0, pp1.dkappa(), t1, t));
-  path_point->set_ddkappa(
-      common::math::lerp(pp0.ddkappa(), t0, pp1.ddkappa(), t1, t));
-  path_point->set_s(common::math::lerp(pp0.s(), t0, pp1.s(), t1, t));
+  path_point->set_x(lerp(pp0.x(), t0, pp1.x(), t1, t));
+  path_point->set_y(lerp(pp0.y(), t0, pp1.y(), t1, t));
+  path_point->set_theta(lerp(pp0.theta(), t0, pp1.theta(), t1, t));
+  path_point->set_kappa(lerp(pp0.kappa(), t0, pp1.kappa(), t1, t));
+  path_point->set_dkappa(lerp(pp0.dkappa(), t0, pp1.dkappa(), t1, t));
+  path_point->set_ddkappa(lerp(pp0.ddkappa(), t0, pp1.ddkappa(), t1, t));
+  path_point->set_s(lerp(pp0.s(), t0, pp1.s(), t1, t));
 
   return tp;
 }

modules/common/math/linear_interpolation.h

@@ -46,7 +46,7 @@ namespace math {
  * @return Interpolated point.
  */
 template <typename T>
-T lerp(const T& x0, const double t0, const T& x1, const double t1,
+T lerp(const T &x0, const double t0, const T &x1, const double t1,
        const double t) {
   if (std::abs(t1 - t0) <= 1.0e-6) {
     AERROR << "input time difference is too small";
@@ -71,12 +71,16 @@ T lerp(const T& x0, const double t0, const T& x1, const double t1,
 double slerp(const double a0, const double t0, const double a1, const double t1,
              const double t);
 
-apollo::common::PathPoint InterpolateUsingLinearApproximation(
-    const common::PathPoint &p0, const common::PathPoint &p1, const double s);
+SLPoint InterpolateUsingLinearApproximation(const SLPoint &p0,
+                                            const SLPoint &p1, const double w);
 
-apollo::common::TrajectoryPoint InterpolateUsingLinearApproximation(
-    const common::TrajectoryPoint &tp0, const common::TrajectoryPoint &tp1,
-    const double t);
+PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0,
+                                              const PathPoint &p1,
+                                              const double s);
+
+TrajectoryPoint InterpolateUsingLinearApproximation(const TrajectoryPoint &tp0,
+                                                    const TrajectoryPoint &tp1,
+                                                    const double t);
 
 }  // namespace math
 }  // namespace common

modules/planning/common/path/discretized_path.cc

@@ -47,8 +47,7 @@ double DiscretizedPath::Length() const {
   return path_points_.back().s() - path_points_.front().s();
 }
 
-common::PathPoint DiscretizedPath::EvaluateUsingLinearApproximation(
-    const double path_s) const {
+common::PathPoint DiscretizedPath::Evaluate(const double path_s) const {
   CHECK(!path_points_.empty());
   auto it_lower = QueryLowerBound(path_s);
   if (it_lower == path_points_.begin()) {

modules/planning/common/path/discretized_path.h

@@ -44,7 +44,7 @@ class DiscretizedPath {
 
   const common::PathPoint& EndPoint() const;
 
-  common::PathPoint EvaluateUsingLinearApproximation(const double path_s) const;
+  common::PathPoint Evaluate(const double path_s) const;
 
   const std::vector<common::PathPoint>& path_points() const;
 

modules/planning/common/path/discretized_path_test.cc

@@ -46,23 +46,22 @@ TEST(DiscretizedPathTest, basic_test) {
 
   EXPECT_DOUBLE_EQ(discretized_path.Length(), std::sqrt(1.0 + 1.0) * 3.0);
 
-  auto eval_p1 = discretized_path.EvaluateUsingLinearApproximation(0.0);
+  auto eval_p1 = discretized_path.Evaluate(0.0);
   EXPECT_DOUBLE_EQ(eval_p1.s(), 0.0);
   EXPECT_DOUBLE_EQ(eval_p1.x(), 0.0);
   EXPECT_DOUBLE_EQ(eval_p1.y(), 0.0);
 
-  auto eval_p2 =
-      discretized_path.EvaluateUsingLinearApproximation(0.3 * std::sqrt(2.0));
+  auto eval_p2 = discretized_path.Evaluate(0.3 * std::sqrt(2.0));
   EXPECT_DOUBLE_EQ(eval_p2.s(), 0.3 * std::sqrt(2.0));
   EXPECT_DOUBLE_EQ(eval_p2.x(), 0.3);
   EXPECT_DOUBLE_EQ(eval_p2.y(), 0.3);
 
-  auto eval_p3 = discretized_path.EvaluateUsingLinearApproximation(1.8);
+  auto eval_p3 = discretized_path.Evaluate(1.8);
   EXPECT_DOUBLE_EQ(eval_p3.s(), 1.8);
   EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
   EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));
 
-  auto eval_p4 = discretized_path.EvaluateUsingLinearApproximation(2.5);
+  auto eval_p4 = discretized_path.Evaluate(2.5);
   EXPECT_DOUBLE_EQ(eval_p4.s(), 2.5);
   EXPECT_DOUBLE_EQ(eval_p4.x(), (2.0 + 0.5) / std::sqrt(2));
   EXPECT_DOUBLE_EQ(eval_p4.y(), (2.0 + 0.5) / std::sqrt(2));

modules/planning/common/path/path_data.cc

@@ -94,7 +94,7 @@ void PathData::SetReferenceLine(const ReferenceLine *reference_line) {
 
 bool PathData::GetPathPointWithPathS(
     const double s, common::PathPoint *const path_point) const {
-  *path_point = discretized_path_.EvaluateUsingLinearApproximation(s);
+  *path_point = discretized_path_.Evaluate(s);
   return true;
 }
 
@@ -135,8 +135,7 @@ bool PathData::GetPathPointWithRefS(const double ref_s,
       discretized_path_.path_points().at(index).s() +
       r * (discretized_path_.path_points().at(index + 1).s() -
            discretized_path_.path_points().at(index).s());
-  path_point->CopyFrom(
-      discretized_path_.EvaluateUsingLinearApproximation(discretized_path_s));
+  path_point->CopyFrom(discretized_path_.Evaluate(discretized_path_s));
 
   return true;
 }

modules/planning/common/planning_util.cc

@@ -37,126 +37,6 @@ using common::SpeedPoint;
 using common::TrajectoryPoint;
 using common::adapter::AdapterManager;
 
-PathPoint Interpolate(const PathPoint &p0, const PathPoint &p1,
-                      const double s) {
-  double s0 = p0.s();
-  double s1 = p1.s();
-  CHECK(s0 <= s && s <= s1);
-
-  double theta_diff = common::math::NormalizeAngle(p1.theta() - p0.theta());
-
-  std::array<double, 3> gx0{{0.0, p0.kappa(), p0.dkappa()}};
-  std::array<double, 3> gx1{{theta_diff, p1.kappa(), p1.dkappa()}};
-
-  HermiteSpline<double, 5> geometry_spline(gx0, gx1, s0, s1);
-  auto func_cos_theta = [&geometry_spline, &p0](const double s) {
-    auto theta = geometry_spline.Evaluate(0, s) + p0.theta();
-    return std::cos(theta);
-  };
-  auto func_sin_theta = [&geometry_spline, &p0](const double s) {
-    auto theta = geometry_spline.Evaluate(0, s) + p0.theta();
-    return std::sin(theta);
-  };
-
-  double x =
-      p0.x() + common::math::IntegrateByGaussLegendre<5>(func_cos_theta, s0, s);
-  double y =
-      p0.y() + common::math::IntegrateByGaussLegendre<5>(func_sin_theta, s0, s);
-  double theta =
-      common::math::NormalizeAngle(geometry_spline.Evaluate(0, s) + p0.theta());
-  double kappa = geometry_spline.Evaluate(1, s);
-  double dkappa = geometry_spline.Evaluate(2, s);
-  double d2kappa = geometry_spline.Evaluate(3, s);
-
-  PathPoint p;
-  p.set_x(x);
-  p.set_y(y);
-  p.set_theta(theta);
-  p.set_kappa(kappa);
-  p.set_dkappa(dkappa);
-  p.set_ddkappa(d2kappa);
-  p.set_s(s);
-  return p;
-}
-
-TrajectoryPoint Interpolate(const TrajectoryPoint &tp0,
-                            const TrajectoryPoint &tp1, const double t) {
-  if (std::fabs(tp1.path_point().s() - tp0.path_point().s()) < 1.0e-4) {
-    return tp1;
-  }
-
-  const PathPoint &pp0 = tp0.path_point();
-  const PathPoint &pp1 = tp1.path_point();
-  double t0 = tp0.relative_time();
-  double t1 = tp1.relative_time();
-
-  std::array<double, 2> dx0{{tp0.v(), tp0.a()}};
-  std::array<double, 2> dx1{{tp1.v(), tp1.a()}};
-  HermiteSpline<double, 3> dynamic_spline(dx0, dx1, t0, t1);
-
-  double s0 = 0.0;
-  auto func_v = [&dynamic_spline](const double t) {
-    return dynamic_spline.Evaluate(0, t);
-  };
-  double s1 = common::math::IntegrateByGaussLegendre<5>(func_v, t0, t1);
-  double s = common::math::IntegrateByGaussLegendre<5>(func_v, t0, t);
-
-  if (std::fabs(tp0.path_point().s() - s1) < 1.0e-4) {
-    return tp1;
-  }
-
-  double v = dynamic_spline.Evaluate(0, t);
-  double a = dynamic_spline.Evaluate(1, t);
-
-  std::array<double, 2> gx0{{pp0.theta(), pp0.kappa()}};
-  std::array<double, 2> gx1{{pp1.theta(), pp1.kappa()}};
-  HermiteSpline<double, 3> geometry_spline(gx0, gx1, s0, s1);
-  auto func_cos_theta = [&geometry_spline](const double s) {
-    auto theta = geometry_spline.Evaluate(0, s);
-    return std::cos(theta);
-  };
-  auto func_sin_theta = [&geometry_spline](const double s) {
-    auto theta = geometry_spline.Evaluate(0, s);
-    return std::sin(theta);
-  };
-
-  double x = pp0.x() +
-             common::math::IntegrateByGaussLegendre<5>(func_cos_theta, s0, s);
-  double y = pp0.y() +
-             common::math::IntegrateByGaussLegendre<5>(func_sin_theta, s0, s);
-  double theta = geometry_spline.Evaluate(0, s);
-  double kappa = geometry_spline.Evaluate(1, s);
-  double dkappa = geometry_spline.Evaluate(2, s);
-  double d2kappa = geometry_spline.Evaluate(3, s);
-
-  TrajectoryPoint tp;
-  tp.set_v(v);
-  tp.set_a(a);
-  tp.set_relative_time(t);
-
-  PathPoint *path_point = tp.mutable_path_point();
-  path_point->set_x(x);
-  path_point->set_y(y);
-  path_point->set_theta(theta);
-  path_point->set_kappa(kappa);
-  path_point->set_dkappa(dkappa);
-  path_point->set_ddkappa(d2kappa);
-  path_point->set_s(s);
-
-  // check the diff of computed s1 and p1.s()?
-  return tp;
-}
-
-common::SLPoint Interpolate(const common::SLPoint &start,
-                            const common::SLPoint &end, const double weight) {
-  common::SLPoint point;
-  double s = start.s() * (1 - weight) + end.s() * weight;
-  double l = start.l() * (1 - weight) + end.l() * weight;
-  point.set_s(s);
-  point.set_l(l);
-  return point;
-}
-
 PlanningStatus *GetPlanningStatus() {
   static PlanningStatus status;
   return &status;

modules/planning/common/planning_util.h

@@ -35,15 +35,6 @@ namespace apollo {
 namespace planning {
 namespace util {
 
-common::SLPoint Interpolate(const common::SLPoint &start,
-                            const common::SLPoint &end, const double weight);
-
-common::PathPoint Interpolate(const common::PathPoint &p0,
-                              const common::PathPoint &p1, const double s);
-
-common::TrajectoryPoint Interpolate(const common::TrajectoryPoint &tp0,
-                                    const common::TrajectoryPoint &tp1,
-                                    const double t);
 /**
  * This function returns the run-time state of the planning module.
  * @Warnning: this function is not thread safe.

modules/planning/common/trajectory/discretized_trajectory.cc

@@ -45,7 +45,7 @@ DiscretizedTrajectory::DiscretizedTrajectory(const ADCTrajectory& trajectory) {
                             trajectory.trajectory_point().end());
 }
 
-TrajectoryPoint DiscretizedTrajectory::EvaluateUsingLinearApproximation(
+TrajectoryPoint DiscretizedTrajectory::Evaluate(
     const double relative_time) const {
   auto comp = [](const TrajectoryPoint& p, const double relative_time) {
     return p.relative_time() < relative_time;

modules/planning/common/trajectory/discretized_trajectory.h

@@ -54,8 +54,7 @@ class DiscretizedTrajectory : public Trajectory {
 
   double GetSpatialLength() const override;
 
-  virtual common::TrajectoryPoint EvaluateUsingLinearApproximation(
-      const double relative_time) const;
+  common::TrajectoryPoint Evaluate(const double relative_time) const override;
 
   virtual uint32_t QueryNearestPoint(const double relative_time) const;
 

modules/planning/common/trajectory/trajectory.h

@@ -32,6 +32,9 @@ class Trajectory {
 
   virtual ~Trajectory() = default;
 
+  virtual common::TrajectoryPoint Evaluate(
+      const double relative_time) const = 0;
+
   virtual common::TrajectoryPoint StartPoint() const = 0;
 
   virtual double GetTemporalLength() const = 0;

modules/planning/common/trajectory/trajectory_stitcher.cc

@@ -63,8 +63,7 @@ void TrajectoryStitcher::TransformLastPublishedTrajectory(
     return;
   }
   const double time_diff = current_time - prev_trajectory->header_time();
-  auto matched_point =
-      prev_trajectory->EvaluateUsingLinearApproximation(time_diff);
+  auto matched_point = prev_trajectory->Evaluate(time_diff);
   if (!matched_point.has_path_point()) {
     return;
   }
@@ -163,8 +162,7 @@ std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
     return ComputeReinitStitchingTrajectory(vehicle_state);
   }
 
-  auto matched_point =
-      prev_trajectory->EvaluateUsingLinearApproximation(veh_rel_time);
+  auto matched_point = prev_trajectory->Evaluate(veh_rel_time);
 
   if (!matched_point.has_path_point()) {
     return ComputeReinitStitchingTrajectory(vehicle_state);

modules/planning/tasks/qp_spline_path/qp_frenet_frame.cc

@@ -28,6 +28,7 @@
 
 #include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/macro.h"
+#include "modules/common/math/linear_interpolation.h"
 #include "modules/common/util/util.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/common/planning_util.h"
@@ -339,8 +340,10 @@ std::pair<double, double> QpFrenetFrame::MapLateralConstraint(
     weight_front = (s_front - start.s()) / (end.s() - start.s());
   }
 
-  common::SLPoint front = util::Interpolate(start, end, weight_front);
-  common::SLPoint back = util::Interpolate(start, end, weight_back);
+  common::SLPoint front = common::math::InterpolateUsingLinearApproximation(
+      start, end, weight_front);
+  common::SLPoint back = common::math::InterpolateUsingLinearApproximation(
+      start, end, weight_back);
 
   if (nudge_type == ObjectNudge::RIGHT_NUDGE) {
     result.second = std::min(front.l(), back.l());

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -397,9 +397,8 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
       const double step_length = vehicle_param_.front_edge_to_center();
       for (double path_s = 0.0; path_s < discretized_path.Length();
            path_s += step_length) {
-        const auto curr_adc_path_point =
-            discretized_path.EvaluateUsingLinearApproximation(
-                path_s + discretized_path.StartPoint().s());
+        const auto curr_adc_path_point = discretized_path.Evaluate(
+            path_s + discretized_path.StartPoint().s());
         if (CheckOverlap(curr_adc_path_point, obs_box,
                          st_boundary_config_.boundary_buffer())) {
           // found overlap, start searching with higher resolution
@@ -422,9 +421,8 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
               break;
             }
             if (!find_low) {
-              const auto& point_low =
-                  discretized_path.EvaluateUsingLinearApproximation(
-                      low_s + discretized_path.StartPoint().s());
+              const auto& point_low = discretized_path.Evaluate(
+                  low_s + discretized_path.StartPoint().s());
               if (!CheckOverlap(point_low, obs_box,
                                 st_boundary_config_.boundary_buffer())) {
                 low_s += fine_tuning_step_length;
@@ -433,9 +431,8 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
               }
             }
             if (!find_high) {
-              const auto& point_high =
-                  discretized_path.EvaluateUsingLinearApproximation(
-                      high_s + discretized_path.StartPoint().s());
+              const auto& point_high = discretized_path.Evaluate(
+                  high_s + discretized_path.StartPoint().s());
               if (!CheckOverlap(point_high, obs_box,
                                 st_boundary_config_.boundary_buffer())) {
                 high_s -= fine_tuning_step_length;