diff --git a/modules/common/path/BUILD b/modules/map/pnc_map/BUILD similarity index 100% rename from modules/common/path/BUILD rename to modules/map/pnc_map/BUILD diff --git a/modules/common/path/path.cc b/modules/map/pnc_map/path.cc similarity index 99% rename from modules/common/path/path.cc rename to modules/map/pnc_map/path.cc index c0a95d71f623a1a626ed71076ecc2f03747d4308..6c2ea33259c54d479699740f92ced653f852cc3f 100644 --- a/modules/common/path/path.cc +++ b/modules/map/pnc_map/path.cc @@ -14,7 +14,7 @@ * limitations under the License. *****************************************************************************/ -#include "modules/common/path/path.h" +#include "modules/map/pnc_map/path.h" #include #include @@ -27,7 +27,7 @@ #include "modules/common/math/vec2d.h" namespace apollo { -namespace common { +namespace hdmap { using apollo::common::math::LineSegment2d; using apollo::common::math::Polygon2d; @@ -934,5 +934,5 @@ bool PathApproximation::overlap_with(const Path& path, const Box2d& box, return false; } -} // namespace common +} // namespace hdmap } // namespace apollo diff --git a/modules/common/path/path.h b/modules/map/pnc_map/path.h similarity index 99% rename from modules/common/path/path.h rename to modules/map/pnc_map/path.h index f6fc5e2ea0fa79235951564fb5e91cf91cef462e..1c6cc0d7bde726c0c38c75416ccb273596c64cc2 100644 --- a/modules/common/path/path.h +++ b/modules/map/pnc_map/path.h @@ -38,7 +38,7 @@ #include "modules/map/hdmap/hdmap_common.h" namespace apollo { -namespace common { +namespace hdmap { // class LaneInfo; // class OverlapInfo; @@ -322,7 +322,7 @@ class Path { std::vector _speed_bump_overlaps; }; -} // namespace common +} // namespace hdmap } // namespace apollo #endif // MODULES_MAP_PNC_MAP_TRAJECTORY_H_ diff --git a/modules/planning/reference_line/.BUILD.swp b/modules/planning/reference_line/.BUILD.swp new file mode 100644 index 0000000000000000000000000000000000000000..37faae0e8b4f79413b657265748dd2cbff7299fd Binary files /dev/null and b/modules/planning/reference_line/.BUILD.swp differ diff --git a/modules/planning/reference_line/BUILD b/modules/planning/reference_line/BUILD index 6153cc2502adb049a318624bc1735247db4e3310..264bb755a2fdfb071510fd9b968ee1d5ae644094 100644 --- a/modules/planning/reference_line/BUILD +++ b/modules/planning/reference_line/BUILD @@ -4,12 +4,18 @@ cc_library( name = "lib_reference_line", srcs = [ "reference_point.cc", + "reference_line.cc", ], hdrs = [ "reference_point.h", + "reference_line.h", ], deps = [ - "//modules/common/path:pnc_path", + "//modules/common:log", + "//modules/common/proto:path_point_proto", + "//modules/map/pnc_map:pnc_path", "//modules/common/math:math", + "//modules/planning/math:double", + "//modules/planning/math:interpolation", ], ) diff --git a/modules/planning/reference_line/reference_line.cc b/modules/planning/reference_line/reference_line.cc new file mode 100644 index 0000000000000000000000000000000000000000..f29dd980acf5d29f1c53a8def78d4eba3f066af1 --- /dev/null +++ b/modules/planning/reference_line/reference_line.cc @@ -0,0 +1,226 @@ +/****************************************************************************** + * 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 reference_line.cc + **/ + +#include "modules/planning/reference_line/reference_line.h" + +#include "boost/math/tools/minima.hpp" + +#include "modules/common/log.h" +#include "modules/common/math/angle.h" +#include "modules/planning/math/double.h" +#include "modules/planning/math/interpolation.h" + +namespace apollo { +namespace planning { + +using ReferenceMapLine = ::apollo::hdmap::Path; + +ReferenceLine::ReferenceLine( + const std::vector& reference_points) + : _reference_points(reference_points) { + std::vector<::apollo::hdmap::PathPoint> points(reference_points.begin(), + reference_points.end()); + _reference_map_line = std::move(ReferenceMapLine(std::move(points))); +} + +ReferenceLine::ReferenceLine( + const std::vector& reference_points, + const std::vector<::apollo::hdmap::LaneSegment>& lane_segments, + const double max_approximation_error) + : _reference_points(reference_points) { + std::vector<::apollo::hdmap::PathPoint> points(reference_points.begin(), + reference_points.end()); + _reference_map_line = std::move(ReferenceMapLine( + std::move(points), lane_segments, max_approximation_error)); +} + +void ReferenceLine::move(const ReferenceLine& reference_line) { + _reference_map_line = + ReferenceMapLine(reference_line.reference_map_line().path_points()); + _reference_points = reference_line.reference_points(); +} + +void ReferenceLine::move(const ReferenceLine&& reference_line) { + _reference_map_line = + ReferenceMapLine(reference_line.reference_map_line().path_points()); + _reference_points = reference_line.reference_points(); +} + +ReferencePoint ReferenceLine::get_reference_point(const double s) const { + const auto& accumulated_s = _reference_map_line.accumulated_s(); + if (s < accumulated_s.front()) { + AWARN << "The requested s is nearer than the start point of the reference " + "line; reference line starts at " + << accumulated_s.back() << ", requested " << s << "."; + return _reference_points.front(); + } + if (s > accumulated_s.back()) { + AWARN + << "The requested s exceeds the reference line; reference line ends at " + << accumulated_s.back() << "requested " << s << " ."; + return _reference_points.back(); + } + + auto it_lower = + std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s); + if (it_lower == accumulated_s.begin()) { + return _reference_points.front(); + } else { + std::size_t index = (std::size_t)(it_lower - accumulated_s.begin()); + auto p0 = _reference_points[index - 1]; + auto p1 = _reference_points[index]; + + auto s0 = accumulated_s[index - 1]; + auto s1 = accumulated_s[index]; + + return ReferenceLine::interpolate(p0, s0, p1, s1, s); + } +} + +double ReferenceLine::find_min_distance_point(const ReferencePoint& p0, + const double s0, + const ReferencePoint& p1, + const double s1, const double x, + const double y) { + auto func_dist_square = [&p0, &p1, &s0, &s1, &x, &y](const double s) { + auto p = ReferenceLine::interpolate(p0, s0, p1, s1, s); + double dx = p.x() - x; + double dy = p.y() - y; + return dx * dx + dy * dy; + }; + + return ::boost::math::tools::brent_find_minima(func_dist_square, s0, s1, 8) + .first; +} + +ReferencePoint ReferenceLine::get_reference_point(const double x, + const double y) const { + CHECK(_reference_points.size() > 0); + + auto func_distance_square = [](const ReferencePoint& point, const double x, + const double y) { + double dx = point.x() - x; + double dy = point.y() - y; + return dx * dx + dy * dy; + }; + + double d_min = func_distance_square(_reference_points.front(), x, y); + double index_min = 0; + + for (size_t i = 1; i < _reference_points.size(); ++i) { + double d_temp = func_distance_square(_reference_points[i], x, y); + if (d_temp < d_min) { + d_min = d_temp; + index_min = i; + } + } + + size_t index_start = index_min == 0 ? index_min : index_min - 1; + size_t index_end = + index_min + 1 == _reference_points.size() ? index_min : index_min + 1; + + if (index_start == index_end) return _reference_points[index_start]; + + double s0 = _reference_map_line.accumulated_s()[index_start]; + double s1 = _reference_map_line.accumulated_s()[index_end]; + + double s = ReferenceLine::find_min_distance_point( + _reference_points[index_start], s0, _reference_points[index_end], s1, x, + y); + + return ReferenceLine::interpolate(_reference_points[index_start], s0, + _reference_points[index_end], s1, s); +} + +bool ReferenceLine::get_point_in_Cartesian_frame( + const apollo::common::SLPoint& sl_point, + Eigen::Vector2d* const xy_point) const { + CHECK_NOTNULL(xy_point); + if (_reference_map_line.num_points() < 2) { + AERROR << "The reference line has too few points."; + return false; + } + + const auto matched_point = get_reference_point(sl_point.s()); + const auto angle = + ::apollo::common::math::Angle16::from_rad(matched_point.heading()); + (*xy_point)[0] = + (matched_point.x() - ::apollo::common::math::sin(angle) * sl_point.l()); + (*xy_point)[1] = + (matched_point.y() + ::apollo::common::math::cos(angle) * sl_point.l()); + return true; +} + +bool ReferenceLine::get_point_in_Frenet_frame( + const Eigen::Vector2d& xy_point, + apollo::common::SLPoint* const sl_point) const { + CHECK_NOTNULL(sl_point); + double s = 0; + double l = 0; + if (!_reference_map_line.get_nearest_point({xy_point.x(), xy_point.y()}, &s, + &l)) { + AERROR << "Can't get nearest point from path."; + return false; + } + + if (s > _reference_map_line.accumulated_s().back()) { + AERROR << "The s of point is bigger than the length of current path."; + return false; + } + sl_point->set_s(s); + sl_point->set_l(l); + return true; +} + +ReferencePoint ReferenceLine::interpolate(const ReferencePoint& p0, + const double s0, + const ReferencePoint& p1, + const double s1, const double s) { + ReferencePoint p = p1; + p.set_x(Interpolation::lerp(p0.x(), s0, p1.x(), s1, s)); + p.set_y(Interpolation::lerp(p0.y(), s0, p1.y(), s1, s)); + p.set_heading(Interpolation::slerp(p0.heading(), s0, p1.heading(), s1, s)); + p.set_kappa(Interpolation::lerp(p0.kappa(), s0, p1.kappa(), s1, s)); + p.set_dkappa(Interpolation::lerp(p0.dkappa(), s0, p1.dkappa(), s1, s)); + + // lane boundary info, lane info will be the same as the p1. + return p; +} + +const std::vector& ReferenceLine::reference_points() const { + return _reference_points; +} + +const ReferenceMapLine& ReferenceLine::reference_map_line() const { + return _reference_map_line; +} + +double ReferenceLine::get_lane_width(const double s) const { + // TODO : need implement; + return 4.0; +} + +bool ReferenceLine::is_on_road(const apollo::common::SLPoint& sl_point) const { + // TODO : need implement; + return true; +} + +} // namespace planning +} // namespace apollo diff --git a/modules/planning/reference_line/reference_line.h b/modules/planning/reference_line/reference_line.h new file mode 100644 index 0000000000000000000000000000000000000000..cba0cbdc3b8c9922ecacd4221eaf6cdffba082ac --- /dev/null +++ b/modules/planning/reference_line/reference_line.h @@ -0,0 +1,75 @@ +/****************************************************************************** + * 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 reference_line.h + **/ + +#ifndef MODULES_PLANNING_MATH_REFERENCE_LINE_H_ +#define MODULES_PLANNING_MATH_REFERENCE_LINE_H_ + +#include "Eigen/Dense" + +#include "modules/common/proto/path_point.pb.h" + +#include "modules/map/pnc_map/path.h" +#include "modules/planning/reference_line/reference_point.h" + +namespace apollo { +namespace planning { + +class ReferenceLine { + public: + ReferenceLine() = default; + ReferenceLine(const std::vector& reference_points); + ReferenceLine(const std::vector& reference_points, + const std::vector<::apollo::hdmap::LaneSegment>& lane_segments, + const double max_approximation_error); + void move(const ReferenceLine& reference_line); + void move(const ReferenceLine&& reference_line); + const ::apollo::hdmap::Path& reference_map_line() const; + const std::vector& reference_points() const; + + ReferencePoint get_reference_point(const double s) const; + ReferencePoint get_reference_point(const double x, const double y) const; + + bool get_point_in_Cartesian_frame(const apollo::common::SLPoint& sl_point, + Eigen::Vector2d* const xy_point) const; + bool get_point_in_Frenet_frame(const Eigen::Vector2d& xy_point, + apollo::common::SLPoint* const sl_point) const; + + double get_lane_width(const double s) const; + bool is_on_road(const apollo::common::SLPoint& sl_point) const; + + private: + static ReferencePoint interpolate(const ReferencePoint& p0, const double s0, + const ReferencePoint& p1, const double s1, + const double s); + + static double find_min_distance_point(const ReferencePoint& p0, + const double s0, + const ReferencePoint& p1, + const double s1, const double x, + const double y); + + std::vector _reference_points; + ::apollo::hdmap::Path _reference_map_line; +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_MATH_REFERENCE_LINE_H_ diff --git a/modules/planning/reference_line/reference_point.cc b/modules/planning/reference_line/reference_point.cc index 422a78eecaa9127d6a18de2d8b190beed0fe96a1..711b606a3f8a2df60c2f6d1e008468eef5e1e536 100644 --- a/modules/planning/reference_line/reference_point.cc +++ b/modules/planning/reference_line/reference_point.cc @@ -27,7 +27,7 @@ ReferencePoint::ReferencePoint(const ::apollo::common::math::Vec2d& point, const double heading, const double kappa, const double dkappa, const double lower_bound, const double upper_bound) - : ::apollo::common::PathPoint(point, heading), + : ::apollo::hdmap::PathPoint(point, heading), _kappa(kappa), _dkappa(dkappa), _lower_bound(lower_bound), @@ -35,14 +35,14 @@ ReferencePoint::ReferencePoint(const ::apollo::common::math::Vec2d& point, ReferencePoint::ReferencePoint( const apollo::common::math::Vec2d& point, const double heading, - const ::apollo::common::LaneWaypoint lane_waypoint) - : ::apollo::common::PathPoint(point, heading, lane_waypoint) {} + const ::apollo::hdmap::LaneWaypoint lane_waypoint) + : ::apollo::hdmap::PathPoint(point, heading, lane_waypoint) {} ReferencePoint::ReferencePoint( const apollo::common::math::Vec2d& point, const double heading, const double kappa, const double dkappa, - const ::apollo::common::LaneWaypoint lane_waypoint) - : ::apollo::common::PathPoint(point, heading, lane_waypoint), + const ::apollo::hdmap::LaneWaypoint lane_waypoint) + : ::apollo::hdmap::PathPoint(point, heading, lane_waypoint), _kappa(kappa), _dkappa(dkappa) {} diff --git a/modules/planning/reference_line/reference_point.h b/modules/planning/reference_line/reference_point.h index 8f92fe7ebd52a619faf3ca408720a87c47133384..afb74c1179ab90b391afa96503b863a1961fa594 100644 --- a/modules/planning/reference_line/reference_point.h +++ b/modules/planning/reference_line/reference_point.h @@ -21,12 +21,12 @@ #ifndef MODULES_PLANNING_REFERENCE_LINE_REFERENCE_POINT_H_ #define MODULES_PLANNING_REFERENCE_LINE_REFERENCE_POINT_H_ -#include "modules/common/path/path.h" +#include "modules/map/pnc_map/path.h" namespace apollo { namespace planning { -class ReferencePoint : public ::apollo::common::PathPoint { +class ReferencePoint : public ::apollo::hdmap::PathPoint { public: ReferencePoint() = default; @@ -35,11 +35,11 @@ class ReferencePoint : public ::apollo::common::PathPoint { const double lower_bound, const double upper_bound); ReferencePoint(const apollo::common::math::Vec2d& point, const double heading, - const ::apollo::common::LaneWaypoint lane_waypoint); + const ::apollo::hdmap::LaneWaypoint lane_waypoint); ReferencePoint(const apollo::common::math::Vec2d& point, const double heading, const double kappa, const double dkappa, - const ::apollo::common::LaneWaypoint lane_waypoint); + const ::apollo::hdmap::LaneWaypoint lane_waypoint); void set_kappa(const double kappa); void set_dkappa(const double dkappa);