From d900e1baf938800f6ab81229cc677cc2c0c0f465 Mon Sep 17 00:00:00 2001 From: Aaron Xiao Date: Wed, 19 Jul 2017 15:42:51 -0700 Subject: [PATCH] Migrate DiscretizedTrajectory to Apollo 1.5. --- modules/planning/common/path/BUILD | 1 + .../planning/common/path/path_point_util.cc | 124 +++++++++++++-- .../planning/common/path/path_point_util.h | 20 ++- modules/planning/common/trajectory/BUILD | 22 ++- .../trajectory/discretized_trajectory.cc | 142 ++++++++++++++++++ .../trajectory/discretized_trajectory.h | 71 +++++++++ .../planning/common/trajectory/trajectory.h | 9 +- modules/planning/math/BUILD | 2 + 8 files changed, 363 insertions(+), 28 deletions(-) create mode 100644 modules/planning/common/trajectory/discretized_trajectory.cc create mode 100644 modules/planning/common/trajectory/discretized_trajectory.h diff --git a/modules/planning/common/path/BUILD b/modules/planning/common/path/BUILD index 4d08effa03..95a954149b 100644 --- a/modules/planning/common/path/BUILD +++ b/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", ], ) diff --git a/modules/planning/common/path/path_point_util.cc b/modules/planning/common/path/path_point_util.cc index e5730b6cc7..31ec98a3ab 100644 --- a/modules/planning/common/path/path_point_util.cc +++ b/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 dx0 { { tp0.v(), tp0.a() } }; + std::array dx1 { { tp1.v(), tp1.a() } }; + HermiteSpline 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 gx0 { { pp0.theta(), pp0.kappa() } }; + std::array gx1 { { pp1.theta(), pp1.kappa() } }; + HermiteSpline 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 diff --git a/modules/planning/common/path/path_point_util.h b/modules/planning/common/path/path_point_util.h index 39a241d4d6..297982cc4d 100644 --- a/modules/planning/common/path/path_point_util.h +++ b/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 diff --git a/modules/planning/common/trajectory/BUILD b/modules/planning/common/trajectory/BUILD index fb26a683ef..c802ed7171 100644 --- a/modules/planning/common/trajectory/BUILD +++ b/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", + ], +) diff --git a/modules/planning/common/trajectory/discretized_trajectory.cc b/modules/planning/common/trajectory/discretized_trajectory.cc new file mode 100644 index 0000000000..d0160f2116 --- /dev/null +++ b/modules/planning/common/trajectory/discretized_trajectory.cc @@ -0,0 +1,142 @@ +/****************************************************************************** + * 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 + +#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 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::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 diff --git a/modules/planning/common/trajectory/discretized_trajectory.h b/modules/planning/common/trajectory/discretized_trajectory.h new file mode 100644 index 0000000000..e9e4ac9691 --- /dev/null +++ b/modules/planning/common/trajectory/discretized_trajectory.h @@ -0,0 +1,71 @@ +/****************************************************************************** + * 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 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 _trajectory_points; +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_COMMON_TRAJECTORY_DISCRETIZED_TRAJECTORY_H diff --git a/modules/planning/common/trajectory/trajectory.h b/modules/planning/common/trajectory/trajectory.h index 4164742748..bfabd7e809 100644 --- a/modules/planning/common/trajectory/trajectory.h +++ b/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 - #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 diff --git a/modules/planning/math/BUILD b/modules/planning/math/BUILD index 79a4a06020..138319cfba 100644 --- a/modules/planning/math/BUILD +++ b/modules/planning/math/BUILD @@ -73,6 +73,8 @@ cc_library( "interpolation.h", ]), deps = [ + ":hermite_spline", + "//modules/common/math:math_utils", "@glog//:glog", ], ) -- GitLab