Migrate DiscretizedTrajectory to Apollo 1.5.

modules/planning/common/path/BUILD

@@ -16,6 +16,7 @@ cc_library(
         "//modules/common/proto:path_point_proto",
         "//modules/planning/math:hermite_spline",
         "//modules/planning/math:integration",
+        "//modules/planning/math:interpolation",
         "@eigen//:eigen",
     ],
 )

modules/planning/common/path/path_point_util.cc

@@ -24,13 +24,17 @@
 
 #include "modules/planning/math/hermite_spline.h"
 #include "modules/planning/math/integration.h"
+#include "modules/planning/math/interpolation.h"
 
 namespace apollo {
 namespace planning {
 namespace util {
 
-common::PathPoint interpolate(const common::PathPoint& p0,
-                              const common::PathPoint& p1, const double s) {
+using apollo::common::PathPoint;
+using apollo::common::TrajectoryPoint;
+
+PathPoint interpolate(const PathPoint& p0, const PathPoint& p1,
+                      const double s) {
   double s0 = p0.s();
   double s1 = p1.s();
   CHECK(s0 <= s && s <= s1);
@@ -54,7 +58,7 @@ common::PathPoint interpolate(const common::PathPoint& p0,
   double dkappa = geometry_spline.evaluate(2, s);
   double d2kappa = geometry_spline.evaluate(3, s);
 
-  common::PathPoint p;
+  PathPoint p;
   p.set_x(x);
   p.set_y(y);
   p.set_theta(theta);
@@ -65,25 +69,25 @@ common::PathPoint interpolate(const common::PathPoint& p0,
   return std::move(p);
 }
 
-common::PathPoint interpolate_linear_approximation(
-    const common::PathPoint& left, const common::PathPoint& right,
-    const double s) {
-  double s0 = left.s();
-  double s1 = right.s();
+PathPoint interpolate_linear_approximation(const PathPoint& p0,
+                                           const PathPoint& p1,
+                                           const double s) {
+  double s0 = p0.s();
+  double s1 = p1.s();
   CHECK(s0 < s1);
 
-  common::PathPoint path_point;
+  PathPoint path_point;
   double weight = (s - s0) / (s1 - s0);
-  double x = (1 - weight) * left.x() + weight * right.x();
-  double y = (1 - weight) * left.y() + weight * right.y();
+  double x = (1 - weight) * p0.x() + weight * p1.x();
+  double y = (1 - weight) * p0.y() + weight * p1.y();
   double cos_heading =
-      (1 - weight) * std::cos(left.theta()) + weight * std::cos(right.theta());
+      (1 - weight) * std::cos(p0.theta()) + weight * std::cos(p1.theta());
   double sin_heading =
-      (1 - weight) * std::sin(left.theta()) + weight * std::sin(right.theta());
+      (1 - weight) * std::sin(p0.theta()) + weight * std::sin(p1.theta());
   double theta = std::atan2(sin_heading, cos_heading);
-  double kappa = (1 - weight) * left.kappa() + weight * right.kappa();
-  double dkappa = (1 - weight) * left.dkappa() + weight * right.dkappa();
-  double ddkappa = (1 - weight) * left.ddkappa() + weight * right.ddkappa();
+  double kappa = (1 - weight) * p0.kappa() + weight * p1.kappa();
+  double dkappa = (1 - weight) * p0.dkappa() + weight * p1.dkappa();
+  double ddkappa = (1 - weight) * p0.ddkappa() + weight * p1.ddkappa();
   path_point.set_x(x);
   path_point.set_y(y);
   path_point.set_theta(theta);
@@ -93,6 +97,94 @@ common::PathPoint interpolate_linear_approximation(
   return path_point;
 }
 
+TrajectoryPoint interpolate(const TrajectoryPoint& tp0,
+                            const TrajectoryPoint& tp1,
+                            const double t) {
+  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 = Integration::gauss_legendre(func_v, t0, t1);
+  double s = Integration::gauss_legendre(func_v, t0, t);
+
+  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() + Integration::gauss_legendre(func_cos_theta, s0, s);
+  double y = pp0.y() + Integration::gauss_legendre(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);
+
+  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;
+}
+
+TrajectoryPoint interpolate_linear_approximation(const TrajectoryPoint& tp0,
+                                                 const TrajectoryPoint& tp1,
+                                                 const double t) {
+  const PathPoint& pp0 = tp0.path_point();
+  const PathPoint& pp1 = tp1.path_point();
+  double t0 = tp0.relative_time();
+  double t1 = tp1.relative_time();
+
+  TrajectoryPoint tp;
+  tp.set_v(Interpolation::lerp(tp0.v(), t0, tp1.v(), t1, t));
+  tp.set_a(Interpolation::lerp(tp0.a(), t0, tp1.a(), t1, t));
+  tp.set_relative_time(t);
+  tp.set_dkappa(Interpolation::lerp(tp0.dkappa(), t0, tp1.dkappa(), t1, t));
+
+  PathPoint* path_point = tp.mutable_path_point();
+  path_point->set_x(Interpolation::lerp(pp0.x(), t0, pp1.x(), t1, t));
+  path_point->set_y(Interpolation::lerp(pp0.y(), t0, pp1.y(), t1, t));
+  path_point->set_theta(
+      Interpolation::lerp(pp0.theta(), t0, pp1.theta(), t1, t));
+  path_point->set_kappa(
+      Interpolation::lerp(pp0.kappa(), t0, pp1.kappa(), t1, t));
+  path_point->set_dkappa(
+      Interpolation::lerp(pp0.dkappa(), t0, pp1.dkappa(), t1, t));
+  path_point->set_ddkappa(
+      Interpolation::lerp(pp0.ddkappa(), t0, pp1.ddkappa(), t1, t));
+  path_point->set_s(Interpolation::lerp(pp0.s(), t0, pp1.s(), t1, t));
+
+  return tp;
+}
+
 }  // namespace util
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/path/path_point_util.h

@@ -29,14 +29,26 @@ namespace apollo {
 namespace planning {
 namespace util {
 
-common::PathPoint interpolate(const common::PathPoint& p0,
-                              const common::PathPoint& p1, const double s);
+apollo::common::PathPoint interpolate(
+    const apollo::common::PathPoint& p0,
+    const apollo::common::PathPoint& p1, const double s);
 
 // @ weight shall between 1 and 0
-common::PathPoint interpolate_linear_approximation(
-    const common::PathPoint& left, const common::PathPoint& right,
+apollo::common::PathPoint interpolate_linear_approximation(
+    const apollo::common::PathPoint& p0,
+    const apollo::common::PathPoint& p1,
     const double s);
 
+apollo::common::TrajectoryPoint interpolate(
+    const apollo::common::TrajectoryPoint& tp0,
+    const apollo::common::TrajectoryPoint& tp1,
+    const double t);
+
+apollo::common::TrajectoryPoint interpolate_linear_approximation(
+    const apollo::common::TrajectoryPoint& tp0,
+    const apollo::common::TrajectoryPoint& tp1,
+    const double t);
+
 }  // namespace util
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/trajectory/BUILD

@@ -2,12 +2,28 @@ package(default_visibility = ["//visibility:public"])
 
 cc_library(
     name = "trajectory",
-    hdrs = glob([
-        "*.h",
-    ]),
+    hdrs = [
+        "trajectory.h",
+    ],
     deps = [
         "//external:gflags",
         "//modules/common/math",
         "//modules/common/proto:path_point_proto",
     ],
 )
+
+cc_library(
+    name = "discretized_trajectory",
+    srcs = [
+        "discretized_trajectory.cc",
+    ],
+    hdrs = [
+        "discretized_trajectory.h",
+    ],
+    deps = [
+        ":trajectory",
+        "//modules/common/proto:path_point_proto",
+        "//modules/planning/common/path:path_point_util",
+        "@eigen//:eigen",
+    ],
+)

modules/planning/common/trajectory/discretized_trajectory.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file discretized_trajectory.cpp
+ **/
+
+#include "modules/planning/common/trajectory/discretized_trajectory.h"
+
+#include <climits>
+
+#include "glog/logging.h"
+#include "modules/planning/common/path/path_point_util.h"
+
+namespace apollo {
+namespace planning {
+
+using apollo::common::TrajectoryPoint;
+
+DiscretizedTrajectory::DiscretizedTrajectory(
+    std::vector<TrajectoryPoint> trajectory_points) {
+  _trajectory_points = std::move(trajectory_points);
+}
+
+double DiscretizedTrajectory::time_length() const {
+  if (_trajectory_points.empty()) {
+    return 0.0;
+  }
+  return _trajectory_points.back().relative_time() -
+         _trajectory_points.front().relative_time();
+}
+
+TrajectoryPoint DiscretizedTrajectory::evaluate(
+    const double relative_time) const {
+  CHECK(!_trajectory_points.empty());
+  CHECK(_trajectory_points.front().relative_time() <= relative_time &&
+        _trajectory_points.back().relative_time() <= relative_time)
+        << "Invalid relative time input!";
+
+  auto comp = [](const TrajectoryPoint& p, const double relative_time) {
+    return p.relative_time() < relative_time;
+  };
+
+  auto it_lower = std::lower_bound(_trajectory_points.begin(),
+                                   _trajectory_points.end(),
+                                   relative_time, comp);
+
+  if (it_lower == _trajectory_points.begin()) {
+    return _trajectory_points.front();
+  }
+  return util::interpolate(*(it_lower - 1), *it_lower, relative_time);
+}
+
+TrajectoryPoint DiscretizedTrajectory::evaluate_linear_approximation(
+    const double relative_time) const {
+  auto comp = [](const TrajectoryPoint& p, const double relative_time) {
+    return p.relative_time() < relative_time;
+  };
+
+  auto it_lower = std::lower_bound(_trajectory_points.begin(),
+                                   _trajectory_points.end(),
+                                   relative_time, comp);
+
+  if (it_lower == _trajectory_points.begin()) {
+    return _trajectory_points.front();
+  }
+  return util::interpolate_linear_approximation(*(it_lower - 1), *it_lower,
+                                                relative_time);
+}
+
+std::size_t DiscretizedTrajectory::query_nearest_point(
+    const double relative_time) const {
+  auto func = [](const TrajectoryPoint& tp, const double relative_time) {
+    return tp.relative_time() < relative_time;
+  };
+  auto it_lower = std::lower_bound(_trajectory_points.begin(),
+                                   _trajectory_points.end(),
+                                   relative_time, func);
+  return (std::size_t)(it_lower - _trajectory_points.begin());
+}
+
+std::size_t DiscretizedTrajectory::query_nearest_point(
+    const Eigen::Vector2d& position) const {
+  double dist_min = std::numeric_limits<double>::max();
+  std::size_t index_min = 0;
+  for (std::size_t i = 0; i < _trajectory_points.size(); ++i) {
+    const Eigen::Vector2d coordinate(_trajectory_points[i].path_point().x(),
+                                     _trajectory_points[i].path_point().y());
+    Eigen::Vector2d dist_vec = coordinate - position;
+    double dist = dist_vec.dot(dist_vec);
+    if (dist < dist_min) {
+      dist_min = dist;
+      index_min = i;
+    }
+  }
+  return index_min;
+}
+
+void DiscretizedTrajectory::add_trajectory_point(
+    const TrajectoryPoint& trajectory_point) {
+  if (!_trajectory_points.empty()) {
+    CHECK_GT(trajectory_point.relative_time(),
+             _trajectory_points.back().relative_time());
+  }
+  _trajectory_points.push_back(trajectory_point);
+}
+
+const TrajectoryPoint& DiscretizedTrajectory::trajectory_point_at(
+    const std::size_t index) const {
+  CHECK(index < num_of_points());
+  return _trajectory_points[index];
+}
+
+TrajectoryPoint DiscretizedTrajectory::start_point() const {
+  CHECK(!_trajectory_points.empty());
+  return _trajectory_points.front();
+}
+
+TrajectoryPoint DiscretizedTrajectory::end_point() const {
+  CHECK(!_trajectory_points.empty());
+  return _trajectory_points.back();
+}
+
+std::size_t DiscretizedTrajectory::num_of_points() const {
+  return _trajectory_points.size();
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/trajectory/discretized_trajectory.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file discretized_trajectory.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_TRAJECTORY_DISCRETIZED_TRAJECTORY_H
+#define MODULES_PLANNING_COMMON_TRAJECTORY_DISCRETIZED_TRAJECTORY_H
+
+#include "Eigen/Core"
+#include "modules/planning/common/trajectory/trajectory.h"
+
+namespace apollo {
+namespace planning {
+
+class DiscretizedTrajectory : public Trajectory {
+public:
+  DiscretizedTrajectory() = default;
+
+  DiscretizedTrajectory(
+      std::vector<apollo::common::TrajectoryPoint> trajectory_points);
+
+  virtual ~DiscretizedTrajectory() = default;
+
+  virtual double time_length() const override;
+
+  virtual apollo::common::TrajectoryPoint evaluate(
+      const double relative_time) const override;
+
+  virtual apollo::common::TrajectoryPoint start_point() const override;
+
+  virtual apollo::common::TrajectoryPoint end_point() const override;
+
+  virtual apollo::common::TrajectoryPoint evaluate_linear_approximation(
+      const double relative_time) const;
+
+  virtual std::size_t query_nearest_point(const double relative_time) const;
+
+  virtual std::size_t query_nearest_point(
+      const Eigen::Vector2d& position) const;
+
+  virtual void add_trajectory_point(
+      const apollo::common::TrajectoryPoint& trajectory_point);
+
+  const apollo::common::TrajectoryPoint& trajectory_point_at(
+      const std::size_t index) const;
+
+  std::size_t num_of_points() const;
+
+protected:
+  std::vector<apollo::common::TrajectoryPoint> _trajectory_points;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_COMMON_TRAJECTORY_DISCRETIZED_TRAJECTORY_H

modules/planning/common/trajectory/trajectory.h

@@ -21,8 +21,6 @@
 #ifndef MODULES_PLANNING_COMMON_TRAJECTORY_TRAJECTORY_H_
 #define MODULES_PLANNING_COMMON_TRAJECTORY_TRAJECTORY_H_
 
-#include <cstdlib>
-
 #include "modules/common/proto/path_point.pb.h"
 
 namespace apollo {
@@ -36,11 +34,12 @@ class Trajectory {
 
   virtual double time_length() const = 0;
 
-  virtual TrajectoryPoint evaluate(const double relative_time) const = 0;
+  virtual apollo::common::TrajectoryPoint evaluate(
+      const double relative_time) const = 0;
 
-  virtual TrajectoryPoint start_point() const = 0;
+  virtual apollo::common::TrajectoryPoint start_point() const = 0;
 
-  virtual TrajectoryPoint end_point() const = 0;
+  virtual apollo::common::TrajectoryPoint end_point() const = 0;
 };
 
 }  // namespace planning

modules/planning/math/BUILD

@@ -73,6 +73,8 @@ cc_library(
         "interpolation.h",
     ]),
     deps = [
+        ":hermite_spline",
+        "//modules/common/math:math_utils",
         "@glog//:glog",
     ],
 )