From 997dd4d53f8d6f8bec09fe217b10ca5a5b06cae0 Mon Sep 17 00:00:00 2001 From: Dong Li Date: Tue, 18 Jul 2017 16:01:06 -0700 Subject: [PATCH] added planning/common/path --- modules/common/proto/path_point.proto | 12 + modules/map/hdmap/hdmap_common.cc | 286 ++++---- modules/map/hdmap/hdmap_common_test.cc | 648 +++++++++--------- modules/planning/common/BUILD | 4 + modules/planning/common/logic_point.cc | 38 +- modules/planning/common/logic_point.h | 14 +- modules/planning/common/path/BUILD | 75 ++ .../planning/common/path/discretized_path.cc | 120 ++++ .../planning/common/path/discretized_path.h | 70 ++ .../planning/common/path/frenet_frame_path.cc | 62 ++ .../planning/common/path/frenet_frame_path.h | 58 ++ modules/planning/common/path/path.h | 47 ++ modules/planning/common/path/path_data.cc | 107 +++ modules/planning/common/path/path_data.h | 64 ++ .../planning/common/path/path_point_util.cc | 98 +++ .../planning/common/path/path_point_util.h | 44 ++ modules/planning/common/path/sl_point_util.cc | 39 ++ modules/planning/common/path/sl_point_util.h | 38 + modules/planning/common/speed/speed_data.cc | 97 +-- modules/planning/common/speed/speed_data.h | 34 +- modules/planning/common/speed/speed_point.cc | 96 ++- modules/planning/common/speed/speed_point.h | 52 +- modules/planning/common/speed/st_point.cc | 34 +- modules/planning/common/speed/st_point.h | 23 +- .../math/spiral_curve/quintic_spiral_curve.cc | 13 +- .../math/spiral_curve/spiral_curve.cc | 8 +- modules/planning/planner/em_planner.cc | 9 +- modules/planning/planner/em_planner.h | 5 +- modules/planning/planning.cc | 1 + modules/planning/planning.h | 4 +- modules/planning/planning_test.cc | 2 +- 31 files changed, 1495 insertions(+), 707 deletions(-) create mode 100644 modules/planning/common/path/BUILD create mode 100644 modules/planning/common/path/discretized_path.cc create mode 100644 modules/planning/common/path/discretized_path.h create mode 100644 modules/planning/common/path/frenet_frame_path.cc create mode 100644 modules/planning/common/path/frenet_frame_path.h create mode 100644 modules/planning/common/path/path.h create mode 100644 modules/planning/common/path/path_data.cc create mode 100644 modules/planning/common/path/path_data.h create mode 100644 modules/planning/common/path/path_point_util.cc create mode 100644 modules/planning/common/path/path_point_util.h create mode 100644 modules/planning/common/path/sl_point_util.cc create mode 100644 modules/planning/common/path/sl_point_util.h diff --git a/modules/common/proto/path_point.proto b/modules/common/proto/path_point.proto index 0ad9976792..028a734364 100644 --- a/modules/common/proto/path_point.proto +++ b/modules/common/proto/path_point.proto @@ -2,6 +2,18 @@ syntax = "proto2"; package apollo.common; +message SLPoint { + optional double s = 1; + optional double l = 2; +} + +message FrenetFramePoint { + optional double s = 1; + optional double l = 2; + optional double dl = 3; + optional double ddl = 4; +} + message PathPoint { // coordinates optional double x = 1; diff --git a/modules/map/hdmap/hdmap_common.cc b/modules/map/hdmap/hdmap_common.cc index 5ff5661e35..7d9100af33 100644 --- a/modules/map/hdmap/hdmap_common.cc +++ b/modules/map/hdmap/hdmap_common.cc @@ -13,11 +13,11 @@ See the License for the specific language governing permissions and limitations under the License. =========================================================================*/ -#include #include +#include -#include "modules/map/hdmap/hdmap_common.h" #include "glog/logging.h" +#include "modules/map/hdmap/hdmap_common.h" namespace { @@ -28,57 +28,58 @@ const double kSegmentationEpsilon = 0.2; const double kDuplicatedPointsEpsilon = 1e-7; void remove_duplicates(std::vector *points) { - CHECK_NOTNULL(points); - - int count = 0; - const double limit = kDuplicatedPointsEpsilon * kDuplicatedPointsEpsilon; - for (size_t i = 0; i < points->size(); ++i) { - if (count == 0 || - (*points)[i].DistanceSquareTo((*points)[count - 1]) > limit) { - (*points)[count++] = (*points)[i]; - } + CHECK_NOTNULL(points); + + int count = 0; + const double limit = kDuplicatedPointsEpsilon * kDuplicatedPointsEpsilon; + for (size_t i = 0; i < points->size(); ++i) { + if (count == 0 || + (*points)[i].DistanceSquareTo((*points)[count - 1]) > limit) { + (*points)[count++] = (*points)[i]; } - points->resize(count); + } + points->resize(count); } void points_from_curve(const apollo::hdmap::Curve &input_curve, - std::vector *points) { - CHECK_NOTNULL(points)->clear(); + std::vector *points) { + CHECK_NOTNULL(points)->clear(); - for (const auto &curve : input_curve.segment()) { + for (const auto &curve : input_curve.segment()) { if (curve.has_line_segment()) { - for (const auto &point : curve.line_segment().point()) { - points->emplace_back(point.x(), point.y()); - } + for (const auto &point : curve.line_segment().point()) { + points->emplace_back(point.x(), point.y()); + } } else { - LOG(FATAL) << "Can not handle curve type."; - } + LOG(FATAL) << "Can not handle curve type."; } - remove_duplicates(points); + } + remove_duplicates(points); } apollo::common::math::Polygon2d convert_to_polygon2d( - const apollo::hdmap::Polygon &polygon) { - std::vector points; - points.reserve(polygon.point_size()); - for (const auto &point : polygon.point()) { - points.emplace_back(point.x(), point.y()); - } - remove_duplicates(&points); - while (points.size() >= 2 && - points[0].DistanceTo(points.back()) - <= apollo::common::math::kMathEpsilon) { - points.pop_back(); - } - return apollo::common::math::Polygon2d(points); + const apollo::hdmap::Polygon &polygon) { + std::vector points; + points.reserve(polygon.point_size()); + for (const auto &point : polygon.point()) { + points.emplace_back(point.x(), point.y()); + } + remove_duplicates(&points); + while (points.size() >= 2 && + points[0].DistanceTo(points.back()) <= + apollo::common::math::kMathEpsilon) { + points.pop_back(); + } + return apollo::common::math::Polygon2d(points); } -void segments_from_curve(const apollo::hdmap::Curve &curve, - std::vector *segments) { - std::vector points; - points_from_curve(curve, &points); - for (size_t i = 0; i + 1 < points.size(); ++i) { - segments->emplace_back(points[i], points[i + 1]); +void segments_from_curve( + const apollo::hdmap::Curve &curve, + std::vector *segments) { + std::vector points; + points_from_curve(curve, &points); + for (size_t i = 0; i + 1 < points.size(); ++i) { + segments->emplace_back(points[i], points[i + 1]); } } } // namespace @@ -89,165 +90,164 @@ namespace hdmap { LaneInfo::LaneInfo(const apollo::hdmap::Lane &lane) : _lane(lane) { init(); } void LaneInfo::init() { - points_from_curve(_lane.central_curve(), &_points); - CHECK_GE(_points.size(), 2); - _segments.clear(); - _accumulated_s.clear(); - _unit_directions.clear(); - _headings.clear(); - - double s = 0; - for (size_t i = 0; i + 1 < _points.size(); ++i) { - _segments.emplace_back(_points[i], _points[i + 1]); - _accumulated_s.push_back(s); - _unit_directions.push_back(_segments.back().unit_direction()); - s += _segments.back().length(); - } + points_from_curve(_lane.central_curve(), &_points); + CHECK_GE(_points.size(), 2); + _segments.clear(); + _accumulated_s.clear(); + _unit_directions.clear(); + _headings.clear(); + + double s = 0; + for (size_t i = 0; i + 1 < _points.size(); ++i) { + _segments.emplace_back(_points[i], _points[i + 1]); _accumulated_s.push_back(s); - _total_length = s; - CHECK(!_unit_directions.empty()); - _unit_directions.push_back(_unit_directions.back()); - for (const auto &direction : _unit_directions) { - _headings.push_back(direction.Angle()); - } - CHECK(!_segments.empty()); + _unit_directions.push_back(_segments.back().unit_direction()); + s += _segments.back().length(); + } + _accumulated_s.push_back(s); + _total_length = s; + CHECK(!_unit_directions.empty()); + _unit_directions.push_back(_unit_directions.back()); + for (const auto &direction : _unit_directions) { + _headings.push_back(direction.Angle()); + } + CHECK(!_segments.empty()); - _sampled_left_width.clear(); - _sampled_right_width.clear(); - for (const auto &sample : _lane.left_sample()) { - _sampled_left_width.emplace_back(sample.s(), sample.width()); - } - for (const auto &sample : _lane.right_sample()) { - _sampled_right_width.emplace_back(sample.s(), sample.width()); - } + _sampled_left_width.clear(); + _sampled_right_width.clear(); + for (const auto &sample : _lane.left_sample()) { + _sampled_left_width.emplace_back(sample.s(), sample.width()); + } + for (const auto &sample : _lane.right_sample()) { + _sampled_right_width.emplace_back(sample.s(), sample.width()); + } } void LaneInfo::get_width(const double s, double *left_width, double *right_width) const { - if (left_width != nullptr) { - *left_width = get_width_from_sample(_sampled_left_width, s); - } - if (right_width != nullptr) { - *right_width = get_width_from_sample(_sampled_right_width, s); - } + if (left_width != nullptr) { + *left_width = get_width_from_sample(_sampled_left_width, s); + } + if (right_width != nullptr) { + *right_width = get_width_from_sample(_sampled_right_width, s); + } } double LaneInfo::get_width(const double s) const { - double left_width = 0.0; - double right_width = 0.0; - get_width(s, &left_width, &right_width); - return left_width + right_width; + double left_width = 0.0; + double right_width = 0.0; + get_width(s, &left_width, &right_width); + return left_width + right_width; } double LaneInfo::get_effective_width(const double s) const { - double left_width = 0.0; - double right_width = 0.0; - get_width(s, &left_width, &right_width); - return 2 * std::min(left_width, right_width); + double left_width = 0.0; + double right_width = 0.0; + get_width(s, &left_width, &right_width); + return 2 * std::min(left_width, right_width); } double LaneInfo::get_width_from_sample( - const std::vector& samples, - const double s) const { - if (samples.empty()) { - return 0.0; - } - if (s <= samples[0].first) { - return samples[0].second; - } - if (s >= samples.back().first) { - return samples.back().second; - } - int low = 0; - int high = static_cast(samples.size()); - while (low + 1 < high) { - const int mid = (low + high) / 2; - if (samples[mid].first <= s) { - low = mid; - } else { - high = mid; - } + const std::vector &samples, const double s) const { + if (samples.empty()) { + return 0.0; + } + if (s <= samples[0].first) { + return samples[0].second; + } + if (s >= samples.back().first) { + return samples.back().second; + } + int low = 0; + int high = static_cast(samples.size()); + while (low + 1 < high) { + const int mid = (low + high) / 2; + if (samples[mid].first <= s) { + low = mid; + } else { + high = mid; } - const LaneInfo::SampledWidth& sample1 = samples[low]; - const LaneInfo::SampledWidth& sample2 = samples[high]; - const double ratio = (sample2.first - s) / (sample2.first - sample1.first); - return sample1.second * ratio + sample2.second * (1.0 - ratio); + } + const LaneInfo::SampledWidth &sample1 = samples[low]; + const LaneInfo::SampledWidth &sample2 = samples[high]; + const double ratio = (sample2.first - s) / (sample2.first - sample1.first); + return sample1.second * ratio + sample2.second * (1.0 - ratio); } JunctionInfo::JunctionInfo(const apollo::hdmap::Junction &junction) : _junction(junction) { - init(); + init(); } void JunctionInfo::init() { - _polygon = convert_to_polygon2d(_junction.polygon()); - CHECK_GT(_polygon.num_points(), 2); + _polygon = convert_to_polygon2d(_junction.polygon()); + CHECK_GT(_polygon.num_points(), 2); } SignalInfo::SignalInfo(const apollo::hdmap::Signal &signal) : _signal(signal) { - init(); + init(); } void SignalInfo::init() { - for (const auto &stop_line : _signal.stop_line()) { - segments_from_curve(stop_line, &_segments); - } - CHECK(!_segments.empty()); - std::vector points; - for (const auto& segment : _segments) { - points.emplace_back(segment.start()); - points.emplace_back(segment.end()); - } - CHECK_GT(points.size(), 0); + for (const auto &stop_line : _signal.stop_line()) { + segments_from_curve(stop_line, &_segments); + } + CHECK(!_segments.empty()); + std::vector points; + for (const auto &segment : _segments) { + points.emplace_back(segment.start()); + points.emplace_back(segment.end()); + } + CHECK_GT(points.size(), 0); } CrosswalkInfo::CrosswalkInfo(const apollo::hdmap::Crosswalk &crosswalk) : _crosswalk(crosswalk) { - init(); + init(); } void CrosswalkInfo::init() { - _polygon = convert_to_polygon2d(_crosswalk.polygon()); - CHECK_GT(_polygon.num_points(), 2); + _polygon = convert_to_polygon2d(_crosswalk.polygon()); + CHECK_GT(_polygon.num_points(), 2); } StopSignInfo::StopSignInfo(const apollo::hdmap::StopSign &stop_sign) : _stop_sign(stop_sign) { - init(); + init(); } void StopSignInfo::init() { - // for (const auto &stop_line : _stop_sign.stop_line()) { - // segments_from_curve(stop_line, &_segments); - // } - segments_from_curve(_stop_sign.stop_line(), &_segments); - CHECK(!_segments.empty()); + // for (const auto &stop_line : _stop_sign.stop_line()) { + // segments_from_curve(stop_line, &_segments); + // } + segments_from_curve(_stop_sign.stop_line(), &_segments); + CHECK(!_segments.empty()); } YieldSignInfo::YieldSignInfo(const apollo::hdmap::YieldSign &yield_sign) : _yield_sign(yield_sign) { - init(); + init(); } void YieldSignInfo::init() { - // for (const auto &stop_line : _yield_sign.stop_line()) { - // segments_from_curve(stop_line, &_segments); - // } - segments_from_curve(_yield_sign.stop_line(), &_segments); - CHECK(!_segments.empty()); + // for (const auto &stop_line : _yield_sign.stop_line()) { + // segments_from_curve(stop_line, &_segments); + // } + segments_from_curve(_yield_sign.stop_line(), &_segments); + CHECK(!_segments.empty()); } OverlapInfo::OverlapInfo(const apollo::hdmap::Overlap &overlap) : _overlap(overlap) {} -const ObjectOverlapInfo * OverlapInfo::get_object_overlap_info( - const apollo::hdmap::Id &id) const { - for (const auto &object : _overlap.object()) { - if (object.id().id() == id.id()) { - return &object; - } +const ObjectOverlapInfo *OverlapInfo::get_object_overlap_info( + const apollo::hdmap::Id &id) const { + for (const auto &object : _overlap.object()) { + if (object.id().id() == id.id()) { + return &object; } - return nullptr; + } + return nullptr; } } // namespace hdmap diff --git a/modules/map/hdmap/hdmap_common_test.cc b/modules/map/hdmap/hdmap_common_test.cc index 099e26adca..9f33fe1005 100644 --- a/modules/map/hdmap/hdmap_common_test.cc +++ b/modules/map/hdmap/hdmap_common_test.cc @@ -17,8 +17,8 @@ limitations under the License. #include "modules/map/hdmap/hdmap_impl.h" int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } namespace apollo { @@ -26,377 +26,373 @@ namespace hdmap { class HDMapCommonTestSuite : public ::testing::Test { protected: - HDMapCommonTestSuite() {} - virtual ~HDMapCommonTestSuite() {} - virtual void SetUp() { - } - virtual void TearDown() { - } - void init_lane_obj(apollo::hdmap::Lane* lane); - void init_junction_obj(apollo::hdmap::Junction* junction); - void init_signal_obj(apollo::hdmap::Signal* signal); - void init_crosswalk_obj(apollo::hdmap::Crosswalk* crosswalk); - void init_stop_sign_obj(apollo::hdmap::StopSign* stop_sign); - void init_yield_sign_obj(apollo::hdmap::YieldSign* yield_sign); + HDMapCommonTestSuite() {} + virtual ~HDMapCommonTestSuite() {} + virtual void SetUp() {} + virtual void TearDown() {} + void init_lane_obj(apollo::hdmap::Lane* lane); + void init_junction_obj(apollo::hdmap::Junction* junction); + void init_signal_obj(apollo::hdmap::Signal* signal); + void init_crosswalk_obj(apollo::hdmap::Crosswalk* crosswalk); + void init_stop_sign_obj(apollo::hdmap::StopSign* stop_sign); + void init_yield_sign_obj(apollo::hdmap::YieldSign* yield_sign); }; void HDMapCommonTestSuite::init_lane_obj(apollo::hdmap::Lane* lane) { - lane->mutable_id()->set_id("lane_1"); - apollo::hdmap::CurveSegment* curve_segment = - lane->mutable_central_curve()->add_segment(); - apollo::hdmap::LineSegment* line_segment = - curve_segment->mutable_line_segment(); - apollo::hdmap::Point* pt = line_segment->add_point(); - pt->set_x(1.0); - pt->set_y(1.0); - pt = line_segment->add_point(); - pt->set_x(2.0); - pt->set_y(1.0); - pt = line_segment->add_point(); - pt->set_x(3.0); - pt->set_y(1.0); - pt = line_segment->add_point(); - pt->set_x(4.0); - pt->set_y(1.0); - pt = line_segment->add_point(); - pt->set_x(5.0); - pt->set_y(1.0); - apollo::hdmap::LaneSampleAssociation* lane_sample = lane->add_left_sample(); - lane_sample->set_s(0.0); - lane_sample->set_width(1.0); - lane_sample = lane->add_left_sample(); - lane_sample->set_s(1.0); - lane_sample->set_width(1.0); - lane_sample = lane->add_left_sample(); - lane_sample->set_s(2.0); - lane_sample->set_width(1.0); - lane_sample = lane->add_left_sample(); - lane_sample->set_s(3.0); - lane_sample->set_width(0.8); - lane_sample = lane->add_left_sample(); - lane_sample->set_s(4.0); - lane_sample->set_width(0.5); + lane->mutable_id()->set_id("lane_1"); + apollo::hdmap::CurveSegment* curve_segment = + lane->mutable_central_curve()->add_segment(); + apollo::hdmap::LineSegment* line_segment = + curve_segment->mutable_line_segment(); + apollo::hdmap::Point* pt = line_segment->add_point(); + pt->set_x(1.0); + pt->set_y(1.0); + pt = line_segment->add_point(); + pt->set_x(2.0); + pt->set_y(1.0); + pt = line_segment->add_point(); + pt->set_x(3.0); + pt->set_y(1.0); + pt = line_segment->add_point(); + pt->set_x(4.0); + pt->set_y(1.0); + pt = line_segment->add_point(); + pt->set_x(5.0); + pt->set_y(1.0); + apollo::hdmap::LaneSampleAssociation* lane_sample = lane->add_left_sample(); + lane_sample->set_s(0.0); + lane_sample->set_width(1.0); + lane_sample = lane->add_left_sample(); + lane_sample->set_s(1.0); + lane_sample->set_width(1.0); + lane_sample = lane->add_left_sample(); + lane_sample->set_s(2.0); + lane_sample->set_width(1.0); + lane_sample = lane->add_left_sample(); + lane_sample->set_s(3.0); + lane_sample->set_width(0.8); + lane_sample = lane->add_left_sample(); + lane_sample->set_s(4.0); + lane_sample->set_width(0.5); - lane_sample = lane->add_right_sample(); - lane_sample->set_s(0.0); - lane_sample->set_width(1.0); - lane_sample = lane->add_right_sample(); - lane_sample->set_s(1.0); - lane_sample->set_width(1.0); - lane_sample = lane->add_right_sample(); - lane_sample->set_s(2.0); - lane_sample->set_width(1.0); - lane_sample = lane->add_right_sample(); - lane_sample->set_s(3.0); - lane_sample->set_width(1.0); - lane_sample = lane->add_right_sample(); - lane_sample->set_s(4.0); - lane_sample->set_width(1.0); + lane_sample = lane->add_right_sample(); + lane_sample->set_s(0.0); + lane_sample->set_width(1.0); + lane_sample = lane->add_right_sample(); + lane_sample->set_s(1.0); + lane_sample->set_width(1.0); + lane_sample = lane->add_right_sample(); + lane_sample->set_s(2.0); + lane_sample->set_width(1.0); + lane_sample = lane->add_right_sample(); + lane_sample->set_s(3.0); + lane_sample->set_width(1.0); + lane_sample = lane->add_right_sample(); + lane_sample->set_s(4.0); + lane_sample->set_width(1.0); } void HDMapCommonTestSuite::init_junction_obj( - apollo::hdmap::Junction* junction) { - junction->mutable_id()->set_id("junction_1"); - apollo::hdmap::Polygon* polygon = junction->mutable_polygon(); - apollo::hdmap::Point* pt = polygon->add_point(); - pt->set_x(1.0); - pt->set_y(1.0); - pt = polygon->add_point(); - pt->set_x(2.0); - pt->set_y(1.0); - pt = polygon->add_point(); - pt->set_x(3.0); - pt->set_y(1.0); - pt = polygon->add_point(); - pt->set_x(4.0); - pt->set_y(1.0); - pt = polygon->add_point(); - pt->set_x(5.0); - pt->set_y(1.0); - pt = polygon->add_point(); - pt->set_x(5.0); - pt->set_y(1.0); - pt = polygon->add_point(); - pt->set_x(5.0); - pt->set_y(2.0); - pt = polygon->add_point(); - pt->set_x(1.0); - pt->set_y(2.0); - pt = polygon->add_point(); - pt->set_x(1.0); - pt->set_y(1.0); + apollo::hdmap::Junction* junction) { + junction->mutable_id()->set_id("junction_1"); + apollo::hdmap::Polygon* polygon = junction->mutable_polygon(); + apollo::hdmap::Point* pt = polygon->add_point(); + pt->set_x(1.0); + pt->set_y(1.0); + pt = polygon->add_point(); + pt->set_x(2.0); + pt->set_y(1.0); + pt = polygon->add_point(); + pt->set_x(3.0); + pt->set_y(1.0); + pt = polygon->add_point(); + pt->set_x(4.0); + pt->set_y(1.0); + pt = polygon->add_point(); + pt->set_x(5.0); + pt->set_y(1.0); + pt = polygon->add_point(); + pt->set_x(5.0); + pt->set_y(1.0); + pt = polygon->add_point(); + pt->set_x(5.0); + pt->set_y(2.0); + pt = polygon->add_point(); + pt->set_x(1.0); + pt->set_y(2.0); + pt = polygon->add_point(); + pt->set_x(1.0); + pt->set_y(1.0); } void HDMapCommonTestSuite::init_signal_obj(apollo::hdmap::Signal* signal) { - signal->mutable_id()->set_id("signal_1"); - apollo::hdmap::Polygon* polygon = signal->mutable_boundary(); - apollo::hdmap::Point* pt = polygon->add_point(); - pt->set_x(1.0); - pt->set_y(1.0); - pt->set_z(1.0); - pt = polygon->add_point(); - pt->set_x(1.0); - pt->set_y(1.0); - pt->set_z(5.0); - pt = polygon->add_point(); - pt->set_x(3.0); - pt->set_y(1.0); - pt->set_z(5.0); - pt = polygon->add_point(); - pt->set_x(3.0); - pt->set_y(1.0); - pt->set_z(1.0); + signal->mutable_id()->set_id("signal_1"); + apollo::hdmap::Polygon* polygon = signal->mutable_boundary(); + apollo::hdmap::Point* pt = polygon->add_point(); + pt->set_x(1.0); + pt->set_y(1.0); + pt->set_z(1.0); + pt = polygon->add_point(); + pt->set_x(1.0); + pt->set_y(1.0); + pt->set_z(5.0); + pt = polygon->add_point(); + pt->set_x(3.0); + pt->set_y(1.0); + pt->set_z(5.0); + pt = polygon->add_point(); + pt->set_x(3.0); + pt->set_y(1.0); + pt->set_z(1.0); - apollo::hdmap::Subsignal* sub_signal = signal->add_subsignal(); - sub_signal->mutable_id()->set_id("sub_signal_1"); - pt = sub_signal->mutable_location(); - pt->set_x(2.0); - pt->set_y(1.0); - pt->set_z(4.0); - sub_signal = signal->add_subsignal(); - sub_signal->mutable_id()->set_id("sub_signal_2"); - pt = sub_signal->mutable_location(); - pt->set_x(2.0); - pt->set_y(1.0); - pt->set_z(3.0); - sub_signal = signal->add_subsignal(); - sub_signal->mutable_id()->set_id("sub_signal_3"); - pt = sub_signal->mutable_location(); - pt->set_x(2.0); - pt->set_y(1.0); - pt->set_z(2.0); + apollo::hdmap::Subsignal* sub_signal = signal->add_subsignal(); + sub_signal->mutable_id()->set_id("sub_signal_1"); + pt = sub_signal->mutable_location(); + pt->set_x(2.0); + pt->set_y(1.0); + pt->set_z(4.0); + sub_signal = signal->add_subsignal(); + sub_signal->mutable_id()->set_id("sub_signal_2"); + pt = sub_signal->mutable_location(); + pt->set_x(2.0); + pt->set_y(1.0); + pt->set_z(3.0); + sub_signal = signal->add_subsignal(); + sub_signal->mutable_id()->set_id("sub_signal_3"); + pt = sub_signal->mutable_location(); + pt->set_x(2.0); + pt->set_y(1.0); + pt->set_z(2.0); - apollo::hdmap::CurveSegment* curve_segment = - signal->add_stop_line()->add_segment(); - apollo::hdmap::LineSegment* line_segment = - curve_segment->mutable_line_segment(); - pt = line_segment->add_point(); - pt->set_x(0.0); - pt->set_y(4.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(1.0); - pt->set_y(4.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(2.0); - pt->set_y(4.0); - pt->set_z(0.0); + apollo::hdmap::CurveSegment* curve_segment = + signal->add_stop_line()->add_segment(); + apollo::hdmap::LineSegment* line_segment = + curve_segment->mutable_line_segment(); + pt = line_segment->add_point(); + pt->set_x(0.0); + pt->set_y(4.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(1.0); + pt->set_y(4.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(2.0); + pt->set_y(4.0); + pt->set_z(0.0); - curve_segment = signal->add_stop_line()->add_segment(); - line_segment = curve_segment->mutable_line_segment(); - pt = line_segment->add_point(); - pt->set_x(2.0); - pt->set_y(4.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(3.0); - pt->set_y(4.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(4.0); - pt->set_y(4.0); - pt->set_z(0.0); + curve_segment = signal->add_stop_line()->add_segment(); + line_segment = curve_segment->mutable_line_segment(); + pt = line_segment->add_point(); + pt->set_x(2.0); + pt->set_y(4.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(3.0); + pt->set_y(4.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(4.0); + pt->set_y(4.0); + pt->set_z(0.0); } void HDMapCommonTestSuite::init_crosswalk_obj( - apollo::hdmap::Crosswalk* crosswalk) { - crosswalk->mutable_id()->set_id("crosswalk_1"); - apollo::hdmap::Polygon* polygon = crosswalk->mutable_polygon(); - apollo::hdmap::Point* pt = polygon->add_point(); - pt->set_x(0.0); - pt->set_y(0.0); - pt->set_z(0.0); - pt = polygon->add_point(); - pt->set_x(3.0); - pt->set_y(0.0); - pt->set_z(0.0); - pt = polygon->add_point(); - pt->set_x(3.0); - pt->set_y(3.0); - pt->set_z(0.0); - pt = polygon->add_point(); - pt->set_x(0.0); - pt->set_y(3.0); - pt->set_z(0.0); + apollo::hdmap::Crosswalk* crosswalk) { + crosswalk->mutable_id()->set_id("crosswalk_1"); + apollo::hdmap::Polygon* polygon = crosswalk->mutable_polygon(); + apollo::hdmap::Point* pt = polygon->add_point(); + pt->set_x(0.0); + pt->set_y(0.0); + pt->set_z(0.0); + pt = polygon->add_point(); + pt->set_x(3.0); + pt->set_y(0.0); + pt->set_z(0.0); + pt = polygon->add_point(); + pt->set_x(3.0); + pt->set_y(3.0); + pt->set_z(0.0); + pt = polygon->add_point(); + pt->set_x(0.0); + pt->set_y(3.0); + pt->set_z(0.0); } void HDMapCommonTestSuite::init_stop_sign_obj( - apollo::hdmap::StopSign* stop_sign) { - stop_sign->mutable_id()->set_id("stop_sign_1"); - apollo::hdmap::CurveSegment* curve_segment = - stop_sign->mutable_stop_line()->add_segment(); - apollo::hdmap::LineSegment* line_segment = - curve_segment->mutable_line_segment(); - apollo::hdmap::Point* pt = line_segment->add_point(); - pt->set_x(0.0); - pt->set_y(0.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(1.0); - pt->set_y(0.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(2.0); - pt->set_y(0.0); - pt->set_z(0.0); + apollo::hdmap::StopSign* stop_sign) { + stop_sign->mutable_id()->set_id("stop_sign_1"); + apollo::hdmap::CurveSegment* curve_segment = + stop_sign->mutable_stop_line()->add_segment(); + apollo::hdmap::LineSegment* line_segment = + curve_segment->mutable_line_segment(); + apollo::hdmap::Point* pt = line_segment->add_point(); + pt->set_x(0.0); + pt->set_y(0.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(1.0); + pt->set_y(0.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(2.0); + pt->set_y(0.0); + pt->set_z(0.0); } void HDMapCommonTestSuite::init_yield_sign_obj( - apollo::hdmap::YieldSign* yield_sign) { - yield_sign->mutable_id()->set_id("yield_sign_1"); - apollo::hdmap::CurveSegment* curve_segment = - yield_sign->mutable_stop_line()->add_segment(); - apollo::hdmap::LineSegment* line_segment = - curve_segment->mutable_line_segment(); - apollo::hdmap::Point* pt = line_segment->add_point(); - pt->set_x(0.0); - pt->set_y(0.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(1.0); - pt->set_y(0.0); - pt->set_z(0.0); - pt = line_segment->add_point(); - pt->set_x(2.0); - pt->set_y(0.0); - pt->set_z(0.0); + apollo::hdmap::YieldSign* yield_sign) { + yield_sign->mutable_id()->set_id("yield_sign_1"); + apollo::hdmap::CurveSegment* curve_segment = + yield_sign->mutable_stop_line()->add_segment(); + apollo::hdmap::LineSegment* line_segment = + curve_segment->mutable_line_segment(); + apollo::hdmap::Point* pt = line_segment->add_point(); + pt->set_x(0.0); + pt->set_y(0.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(1.0); + pt->set_y(0.0); + pt->set_z(0.0); + pt = line_segment->add_point(); + pt->set_x(2.0); + pt->set_y(0.0); + pt->set_z(0.0); } TEST_F(HDMapCommonTestSuite, lane_info) { - apollo::hdmap::Lane lane; - init_lane_obj(&lane); - LaneInfo lane_info(lane); - ASSERT_STREQ(lane.id().id().c_str(), lane_info.id().id().c_str()); - ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size(), - lane_info.points().size()); - for (std::size_t i = 0; i < lane_info.points().size(); ++i) { - ASSERT_NEAR(lane.central_curve().segment(0).line_segment().point(i).x(), - lane_info.points()[i].x(), 1E-5); - ASSERT_NEAR(lane.central_curve().segment(0).line_segment().point(i).y(), - lane_info.points()[i].y(), 1E-5); - } - ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size() - 1, - lane_info.segments().size()); - for (std::size_t i = 0; i < lane_info.segments().size(); ++i) { - ASSERT_NEAR(1.0, lane_info.segments()[i].length(), 1E-4); - } - ASSERT_EQ(lane_info.unit_directions().size(), + apollo::hdmap::Lane lane; + init_lane_obj(&lane); + LaneInfo lane_info(lane); + ASSERT_STREQ(lane.id().id().c_str(), lane_info.id().id().c_str()); + ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size(), + lane_info.points().size()); + for (std::size_t i = 0; i < lane_info.points().size(); ++i) { + ASSERT_NEAR(lane.central_curve().segment(0).line_segment().point(i).x(), + lane_info.points()[i].x(), 1E-5); + ASSERT_NEAR(lane.central_curve().segment(0).line_segment().point(i).y(), + lane_info.points()[i].y(), 1E-5); + } + ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size() - 1, + lane_info.segments().size()); + for (std::size_t i = 0; i < lane_info.segments().size(); ++i) { + ASSERT_NEAR(1.0, lane_info.segments()[i].length(), 1E-4); + } + ASSERT_EQ(lane_info.unit_directions().size(), lane_info.segments().size() + 1); - for (std::size_t i = 0; i < lane_info.segments().size(); ++i) { - ASSERT_EQ(lane_info.segments()[i].unit_direction(), - lane_info.unit_directions()[i]); - } - ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size(), - lane_info.accumulate_s().size()); - for (std::size_t i = 0; i < lane_info.accumulate_s().size(); ++i) { - ASSERT_NEAR(i * 1.0, lane_info.accumulate_s()[i], 1E-4); - } - ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size(), - lane_info.headings().size()); - for (std::size_t i = 0; i < lane_info.headings().size(); ++i) { - ASSERT_NEAR(lane_info.unit_directions()[i].Angle(), - lane_info.headings()[i], 1E-3); - } - double left_width = 0.0; - double right_width = 0.0; - lane_info.get_width(2.0, &left_width, &right_width); - ASSERT_NEAR(1.0, left_width, 1E-3); - ASSERT_NEAR(1.0, right_width, 1E-3); - lane_info.get_width(3.5, &left_width, &right_width); - ASSERT_NEAR(0.65, left_width, 1E-3); - ASSERT_NEAR(1.0, right_width, 1E-3); - ASSERT_NEAR(4.0, lane_info.total_length(), 1E-3); + for (std::size_t i = 0; i < lane_info.segments().size(); ++i) { + ASSERT_EQ(lane_info.segments()[i].unit_direction(), + lane_info.unit_directions()[i]); + } + ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size(), + lane_info.accumulate_s().size()); + for (std::size_t i = 0; i < lane_info.accumulate_s().size(); ++i) { + ASSERT_NEAR(i * 1.0, lane_info.accumulate_s()[i], 1E-4); + } + ASSERT_EQ(lane.central_curve().segment(0).line_segment().point_size(), + lane_info.headings().size()); + for (std::size_t i = 0; i < lane_info.headings().size(); ++i) { + ASSERT_NEAR(lane_info.unit_directions()[i].Angle(), lane_info.headings()[i], + 1E-3); + } + double left_width = 0.0; + double right_width = 0.0; + lane_info.get_width(2.0, &left_width, &right_width); + ASSERT_NEAR(1.0, left_width, 1E-3); + ASSERT_NEAR(1.0, right_width, 1E-3); + lane_info.get_width(3.5, &left_width, &right_width); + ASSERT_NEAR(0.65, left_width, 1E-3); + ASSERT_NEAR(1.0, right_width, 1E-3); + ASSERT_NEAR(4.0, lane_info.total_length(), 1E-3); } TEST_F(HDMapCommonTestSuite, lane_info_get_width) { - apollo::hdmap::Lane lane; - init_lane_obj(&lane); - LaneInfo lane_info(lane); - // double width = 0.0; - ASSERT_NEAR(2.0, lane_info.get_width(2.0), 1E-3); - ASSERT_NEAR(1.65, lane_info.get_width(3.5), 1E-3); + apollo::hdmap::Lane lane; + init_lane_obj(&lane); + LaneInfo lane_info(lane); + // double width = 0.0; + ASSERT_NEAR(2.0, lane_info.get_width(2.0), 1E-3); + ASSERT_NEAR(1.65, lane_info.get_width(3.5), 1E-3); } TEST_F(HDMapCommonTestSuite, lane_info_get_effective_width) { - apollo::hdmap::Lane lane; - init_lane_obj(&lane); - LaneInfo lane_info(lane); - // double width = 0.0; - ASSERT_NEAR(2.0, lane_info.get_effective_width(2.0), 1E-3); - ASSERT_NEAR(1.3, lane_info.get_effective_width(3.5), 1E-3); + apollo::hdmap::Lane lane; + init_lane_obj(&lane); + LaneInfo lane_info(lane); + // double width = 0.0; + ASSERT_NEAR(2.0, lane_info.get_effective_width(2.0), 1E-3); + ASSERT_NEAR(1.3, lane_info.get_effective_width(3.5), 1E-3); } TEST_F(HDMapCommonTestSuite, junction_info) { - apollo::hdmap::Junction junction; - init_junction_obj(&junction); - JunctionInfo junction_info(junction); - ASSERT_STREQ(junction.id().id().c_str(), junction_info.id().id().c_str()); - ASSERT_EQ(7, junction_info.polygon().points().size()); - for (std::size_t i = 0; i < 5; ++i) { - ASSERT_NEAR((i + 1) * 1.0, - junction_info.polygon().points()[i].x(), 1E-3); - } - ASSERT_NEAR(5.0, junction_info.polygon().points()[5].x(), 1E-3); - ASSERT_NEAR(2.0, junction_info.polygon().points()[5].y(), 1E-3); - ASSERT_NEAR(1.0, junction_info.polygon().points()[6].x(), 1E-3); - ASSERT_NEAR(2.0, junction_info.polygon().points()[6].y(), 1E-3); + apollo::hdmap::Junction junction; + init_junction_obj(&junction); + JunctionInfo junction_info(junction); + ASSERT_STREQ(junction.id().id().c_str(), junction_info.id().id().c_str()); + ASSERT_EQ(7, junction_info.polygon().points().size()); + for (std::size_t i = 0; i < 5; ++i) { + ASSERT_NEAR((i + 1) * 1.0, junction_info.polygon().points()[i].x(), 1E-3); + } + ASSERT_NEAR(5.0, junction_info.polygon().points()[5].x(), 1E-3); + ASSERT_NEAR(2.0, junction_info.polygon().points()[5].y(), 1E-3); + ASSERT_NEAR(1.0, junction_info.polygon().points()[6].x(), 1E-3); + ASSERT_NEAR(2.0, junction_info.polygon().points()[6].y(), 1E-3); } TEST_F(HDMapCommonTestSuite, signal_info) { - apollo::hdmap::Signal signal; - init_signal_obj(&signal); - SignalInfo signal_info(signal); - ASSERT_STREQ(signal.id().id().c_str(), signal_info.id().id().c_str()); - ASSERT_EQ(4, signal_info.signal().boundary().point_size()); + apollo::hdmap::Signal signal; + init_signal_obj(&signal); + SignalInfo signal_info(signal); + ASSERT_STREQ(signal.id().id().c_str(), signal_info.id().id().c_str()); + ASSERT_EQ(4, signal_info.signal().boundary().point_size()); - int segment_size = 0; - for (int i = 0; i < signal.stop_line_size(); ++i) { - segment_size += - signal.stop_line(i).segment(0).line_segment().point_size() - 1; - } - ASSERT_EQ(segment_size, signal_info.segments().size()); - for (std::size_t i = 0; i < signal_info.segments().size(); ++i) { - ASSERT_NEAR(1.0, signal_info.segments()[i].length(), 1E-4); - } + int segment_size = 0; + for (int i = 0; i < signal.stop_line_size(); ++i) { + segment_size += + signal.stop_line(i).segment(0).line_segment().point_size() - 1; + } + ASSERT_EQ(segment_size, signal_info.segments().size()); + for (std::size_t i = 0; i < signal_info.segments().size(); ++i) { + ASSERT_NEAR(1.0, signal_info.segments()[i].length(), 1E-4); + } } TEST_F(HDMapCommonTestSuite, crosswalk_info) { - apollo::hdmap::Crosswalk crosswalk; - init_crosswalk_obj(&crosswalk); - CrosswalkInfo crosswalk_info(crosswalk); - ASSERT_STREQ(crosswalk.id().id().c_str(), crosswalk_info.id().id().c_str()); - ASSERT_EQ(4, crosswalk_info.crosswalk().polygon().point_size()); - ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[0].x(), 1E-3); - ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[0].y(), 1E-3); - ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[1].x(), 1E-3); - ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[1].y(), 1E-3); - ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[2].x(), 1E-3); - ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[2].y(), 1E-3); - ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[3].x(), 1E-3); - ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[3].y(), 1E-3); + apollo::hdmap::Crosswalk crosswalk; + init_crosswalk_obj(&crosswalk); + CrosswalkInfo crosswalk_info(crosswalk); + ASSERT_STREQ(crosswalk.id().id().c_str(), crosswalk_info.id().id().c_str()); + ASSERT_EQ(4, crosswalk_info.crosswalk().polygon().point_size()); + ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[0].x(), 1E-3); + ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[0].y(), 1E-3); + ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[1].x(), 1E-3); + ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[1].y(), 1E-3); + ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[2].x(), 1E-3); + ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[2].y(), 1E-3); + ASSERT_NEAR(0.0, crosswalk_info.polygon().points()[3].x(), 1E-3); + ASSERT_NEAR(3.0, crosswalk_info.polygon().points()[3].y(), 1E-3); } TEST_F(HDMapCommonTestSuite, stop_sign_info) { - apollo::hdmap::StopSign stop_sign; - init_stop_sign_obj(&stop_sign); - StopSignInfo stop_sign_info(stop_sign); - ASSERT_STREQ(stop_sign.id().id().c_str(), stop_sign_info.id().id().c_str()); - ASSERT_EQ(stop_sign.stop_line().segment(0).line_segment().point_size() - 1, + apollo::hdmap::StopSign stop_sign; + init_stop_sign_obj(&stop_sign); + StopSignInfo stop_sign_info(stop_sign); + ASSERT_STREQ(stop_sign.id().id().c_str(), stop_sign_info.id().id().c_str()); + ASSERT_EQ(stop_sign.stop_line().segment(0).line_segment().point_size() - 1, stop_sign_info.segments().size()); - for (std::size_t i = 0; i < stop_sign_info.segments().size(); ++i) { - ASSERT_NEAR(1.0, stop_sign_info.segments()[i].length(), 1E-4); - } + for (std::size_t i = 0; i < stop_sign_info.segments().size(); ++i) { + ASSERT_NEAR(1.0, stop_sign_info.segments()[i].length(), 1E-4); + } } TEST_F(HDMapCommonTestSuite, yield_sign_info) { - apollo::hdmap::YieldSign yield_sign; - init_yield_sign_obj(&yield_sign); - YieldSignInfo yield_sign_info(yield_sign); - ASSERT_STREQ(yield_sign.id().id().c_str(), - yield_sign_info.id().id().c_str()); - ASSERT_EQ(yield_sign.stop_line().segment(0).line_segment().point_size() - 1, + apollo::hdmap::YieldSign yield_sign; + init_yield_sign_obj(&yield_sign); + YieldSignInfo yield_sign_info(yield_sign); + ASSERT_STREQ(yield_sign.id().id().c_str(), yield_sign_info.id().id().c_str()); + ASSERT_EQ(yield_sign.stop_line().segment(0).line_segment().point_size() - 1, yield_sign_info.segments().size()); - for (std::size_t i = 0; i < yield_sign_info.segments().size(); ++i) { - ASSERT_NEAR(1.0, yield_sign_info.segments()[i].length(), 1E-4); - } + for (std::size_t i = 0; i < yield_sign_info.segments().size(); ++i) { + ASSERT_NEAR(1.0, yield_sign_info.segments()[i].length(), 1E-4); + } } } // namespace hdmap diff --git a/modules/planning/common/BUILD b/modules/planning/common/BUILD index cd1e95b529..894e0badfe 100644 --- a/modules/planning/common/BUILD +++ b/modules/planning/common/BUILD @@ -1,3 +1,5 @@ +load("//tools:cpplint.bzl", "cpplint") + package(default_visibility = ["//visibility:public"]) cc_library( @@ -36,3 +38,5 @@ cc_library( "//modules/localization/proto:localization_proto", ], ) + +cpplint() diff --git a/modules/planning/common/logic_point.cc b/modules/planning/common/logic_point.cc index 8d917660c6..ee74876a59 100644 --- a/modules/planning/common/logic_point.cc +++ b/modules/planning/common/logic_point.cc @@ -28,43 +28,27 @@ namespace apollo { namespace planning { -LogicPoint::LogicPoint(const double x, - const double y, - const double s, - const double heading, - const double kappa, +LogicPoint::LogicPoint(const double x, const double y, const double s, + const double heading, const double kappa, const double dkappa) : ::apollo::common::math::Vec2d(x, y), _s(s), _heading(heading), _kappa(kappa), _dkappa(dkappa), - _lane_id("") { -} + _lane_id("") {} -double LogicPoint::s() const { - return _s; -} +double LogicPoint::s() const { return _s; } -double LogicPoint::heading() const { - return _heading; -} +double LogicPoint::heading() const { return _heading; } -double LogicPoint::kappa() const { - return _kappa; -} +double LogicPoint::kappa() const { return _kappa; } -double LogicPoint::dkappa() const { - return _dkappa; -} +double LogicPoint::dkappa() const { return _dkappa; } -void LogicPoint::set_lane_id(const std::string &lane_id) { - _lane_id = lane_id; -} +void LogicPoint::set_lane_id(const std::string &lane_id) { _lane_id = lane_id; } -const std::string &LogicPoint::lane_id() const { - return _lane_id; -} +const std::string &LogicPoint::lane_id() const { return _lane_id; } -} // namespace planning -} // namespace apollo +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/logic_point.h b/modules/planning/common/logic_point.h index 928db265e1..91c7ae8b01 100644 --- a/modules/planning/common/logic_point.h +++ b/modules/planning/common/logic_point.h @@ -33,11 +33,8 @@ namespace planning { class LogicPoint : public ::apollo::common::math::Vec2d { public: - explicit LogicPoint(const double x, - const double y, - const double s, - const double heading, - const double kappa, + explicit LogicPoint(const double x, const double y, const double s, + const double heading, const double kappa, const double dkappa); double s() const; double heading() const; @@ -45,6 +42,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d { double dkappa() const; const std::string &lane_id() const; void set_lane_id(const std::string &lane_id); + private: double _s; double _heading; @@ -53,7 +51,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d { std::string _lane_id; }; -} // namespace planning -} // namespace apollo +} // namespace planning +} // namespace apollo -#endif // MODULES_PLANNING_COMMON_LOGIC_POINT_H +#endif // MODULES_PLANNING_COMMON_LOGIC_POINT_H diff --git a/modules/planning/common/path/BUILD b/modules/planning/common/path/BUILD new file mode 100644 index 0000000000..4d08effa03 --- /dev/null +++ b/modules/planning/common/path/BUILD @@ -0,0 +1,75 @@ +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "path_point_util", + srcs = [ + "path_point_util.cc", + "sl_point_util.cc", + ], + hdrs = [ + "path_point_util.h", + "sl_point_util.h", + ], + deps = [ + "//modules/common/proto:path_point_proto", + "//modules/planning/math:hermite_spline", + "//modules/planning/math:integration", + "@eigen//:eigen", + ], +) + +cc_library( + name = "path", + hdrs = [ + "path.h", + ], +) + +cc_library( + name = "discretized_path", + srcs = [ + "discretized_path.cc", + ], + hdrs = [ + "discretized_path.h", + ], + deps = [ + ":path", + ":path_point_util", + "//modules/common/proto:path_point_proto", + ], +) + +cc_library( + name = "frenet_frame_path", + srcs = [ + "frenet_frame_path.cc", + ], + hdrs = [ + "frenet_frame_path.h", + ], + deps = [ + ":path", + ":path_point_util", + "//modules/common/proto:path_point_proto", + ], +) + +cc_library( + name = "path_data", + srcs = [ + "path_data.cc", + ], + hdrs = [ + "path_data.h", + ], + deps = [ + ":discretized_path", + ":frenet_frame_path", + "//modules/planning/math:double", + ], +) + +cpplint() diff --git a/modules/planning/common/path/discretized_path.cc b/modules/planning/common/path/discretized_path.cc new file mode 100644 index 0000000000..7e69a6b7b9 --- /dev/null +++ b/modules/planning/common/path/discretized_path.cc @@ -0,0 +1,120 @@ +/****************************************************************************** + * 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 path.cc + **/ + +#include "modules/planning/common/path/discretized_path.h" + +#include +#include "glog/logging.h" + +#include "modules/planning/common/path/path_point_util.h" + +namespace apollo { +namespace planning { + +DiscretizedPath::DiscretizedPath(std::vector path_points) { + path_points_ = std::move(path_points); +} + +common::PathPoint DiscretizedPath::evaluate(const double param) const { + CHECK_GT(path_points_.size(), 1); + CHECK(path_points_.front().s() <= param && path_points_.back().s() <= param); + + auto it_lower = query_lower_bound(param); + if (it_lower == path_points_.begin()) { + return path_points_.front(); + } + + return util::interpolate(*(it_lower - 1), *it_lower, param); +} + +double DiscretizedPath::param_length() const { + if (path_points_.empty()) { + return 0.0; + } + return path_points_.back().s() - path_points_.front().s(); +} + +int DiscretizedPath::query_closest_point(const double param) const { + if (path_points_.empty()) { + return -1; + } + auto it_lower = query_lower_bound(param); + + if (it_lower == path_points_.begin()) { + return 0; + } + if (it_lower == path_points_.end()) { + return path_points_.size() - 1; + } + + double d0 = param - (it_lower - 1)->s(); + double d1 = it_lower->s() - param; + + if (d0 < d1) { + return static_cast(it_lower - path_points_.begin()) - 1; + } else { + return static_cast(it_lower - path_points_.begin()); + } +} + +std::vector* DiscretizedPath::mutable_path_points() { + return &path_points_; +} + +const std::vector& DiscretizedPath::path_points() const { + return path_points_; +} + +std::size_t DiscretizedPath::num_of_points() const { + return path_points_.size(); +} + +const common::PathPoint& DiscretizedPath::path_point_at( + const std::size_t index) const { + CHECK(index < path_points_.size()); + return path_points_[index]; +} + +common::PathPoint DiscretizedPath::start_point() const { + CHECK(!path_points_.empty()); + return path_points_.front(); +} + +common::PathPoint DiscretizedPath::end_point() const { + CHECK(!path_points_.empty()); + return path_points_.back(); +} + +common::PathPoint& DiscretizedPath::path_point_at(const std::size_t index) { + CHECK(index < path_points_.size()); + return path_points_[index]; +} + +std::vector::const_iterator +DiscretizedPath::query_lower_bound(const double param) const { + auto func = [](const common::PathPoint& tp, const double param) { + return tp.s() < param; + }; + return std::lower_bound(path_points_.begin(), path_points_.end(), param, + func); +} + +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/path/discretized_path.h b/modules/planning/common/path/discretized_path.h new file mode 100644 index 0000000000..84ea68ae3d --- /dev/null +++ b/modules/planning/common/path/discretized_path.h @@ -0,0 +1,70 @@ +/****************************************************************************** + * 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_path.h + **/ + +#ifndef MODULES_PLANNING_COMMON_PATH_DISCRETIZED_PATH_H_ +#define MODULES_PLANNING_COMMON_PATH_DISCRETIZED_PATH_H_ + +#include + +#include "modules/planning/common/path/path.h" +#include "modules/planning/common/path/path_point_util.h" + +namespace apollo { +namespace planning { + +class DiscretizedPath : public Path { + public: + DiscretizedPath() = default; + + explicit DiscretizedPath(std::vector path_points); + + virtual ~DiscretizedPath() = default; + + common::PathPoint evaluate(const double param) const override; + + double param_length() const override; + + common::PathPoint start_point() const override; + + common::PathPoint end_point() const override; + + int query_closest_point(const double param) const; + + std::vector* mutable_path_points(); + + const std::vector& path_points() const; + + std::size_t num_of_points() const; + + const common::PathPoint& path_point_at(const std::size_t index) const; + + common::PathPoint& path_point_at(const std::size_t index); + + private: + std::vector::const_iterator query_lower_bound( + const double param) const; + + std::vector path_points_; +}; + +} // namespace planning +} // namespace apollo + +#endif /* MODULES_PLANNING_COMMON_PATH_PATH_H_ */ diff --git a/modules/planning/common/path/frenet_frame_path.cc b/modules/planning/common/path/frenet_frame_path.cc new file mode 100644 index 0000000000..9dff35c1ae --- /dev/null +++ b/modules/planning/common/path/frenet_frame_path.cc @@ -0,0 +1,62 @@ +/****************************************************************************** + * 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 frenet_frame_path.cc + **/ +#include "modules/planning/common/path/frenet_frame_path.h" + +#include +#include "glog/logging.h" + +#include "modules/common/proto/path_point.pb.h" + +namespace apollo { +namespace planning { + +FrenetFramePath::FrenetFramePath( + std::vector sl_points) { + points_ = std::move(sl_points); +} + +void FrenetFramePath::set_frenet_points( + const std::vector& points) { + points_ = points; +} + +std::vector* FrenetFramePath::mutable_points() { + return &points_; +} + +const std::vector& FrenetFramePath::points() const { + return points_; +} + +std::size_t FrenetFramePath::num_points() const { return points_.size(); } + +const common::FrenetFramePoint& FrenetFramePath::point_at( + const std::size_t index) const { + CHECK(index < points_.size()); + return points_[index]; +} + +common::FrenetFramePoint& FrenetFramePath::point_at(const std::size_t index) { + CHECK(index < points_.size()); + return points_[index]; +} + +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/path/frenet_frame_path.h b/modules/planning/common/path/frenet_frame_path.h new file mode 100644 index 0000000000..149ab67864 --- /dev/null +++ b/modules/planning/common/path/frenet_frame_path.h @@ -0,0 +1,58 @@ +/****************************************************************************** + * 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 frenet_frame_path.h + **/ + +#ifndef BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_ +#define BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_ + +#include + +#include "modules/common/proto/path_point.pb.h" + +namespace apollo { +namespace planning { + +class FrenetFramePath { + public: + FrenetFramePath() = default; + + explicit FrenetFramePath(std::vector sl_points); + + virtual ~FrenetFramePath() = default; + + void set_frenet_points(const std::vector& points); + + std::vector* mutable_points(); + + const std::vector& points() const; + + std::size_t num_points() const; + + const common::FrenetFramePoint& point_at(const std::size_t index) const; + + common::FrenetFramePoint& point_at(const std::size_t index); + + private: + std::vector points_; +}; + +} // namespace planning +} // namespace apollo + +#endif /* BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_ */ diff --git a/modules/planning/common/path/path.h b/modules/planning/common/path/path.h new file mode 100644 index 0000000000..62c25da74d --- /dev/null +++ b/modules/planning/common/path/path.h @@ -0,0 +1,47 @@ +/****************************************************************************** + * 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 path.h + **/ + +#ifndef MODULES_PLANNING_COMMON_PATH_PATH_H_ +#define MODULES_PLANNING_COMMON_PATH_PATH_H_ + +#include "modules/common/proto/path_point.pb.h" + +namespace apollo { +namespace planning { + +class Path { + public: + Path() = default; + + virtual ~Path() = default; + + virtual common::PathPoint evaluate(const double param) const = 0; + + virtual double param_length() const = 0; + + virtual common::PathPoint start_point() const = 0; + + virtual common::PathPoint end_point() const = 0; +}; + +} // namespace planning +} // namespace apollo + +#endif /* MODULES_PLANNING_COMMON_PATH_PATH_H_ */ diff --git a/modules/planning/common/path/path_data.cc b/modules/planning/common/path/path_data.cc new file mode 100644 index 0000000000..cc02a2d03f --- /dev/null +++ b/modules/planning/common/path/path_data.cc @@ -0,0 +1,107 @@ +/****************************************************************************** + * 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 path_data.cc + **/ + +#include "modules/planning/common/path/path_data.h" + +#include +#include "glog/logging.h" + +#include "modules/planning/math/double.h" + +namespace apollo { +namespace planning { + +void PathData::set_path(const DiscretizedPath& path) { path_ = path; } + +void PathData::set_frenet_path(const FrenetFramePath& frenet_path) { + frenet_path_ = frenet_path; +} + +DiscretizedPath* PathData::mutable_path() { return &path_; } + +const DiscretizedPath& PathData::path() const { return path_; } + +FrenetFramePath* PathData::mutable_frenet_frame_path() { return &frenet_path_; } + +const FrenetFramePath& PathData::frenet_frame_path() const { + return frenet_path_; +} + +bool PathData::get_path_point_with_path_s( + const double s, common::PathPoint* const path_point) const { + const auto& path_points = path_.path_points(); + + if (path_points.size() < 2 || s < path_points.front().s() || + path_points.back().s() < s) { + return false; + } + + auto comp = [](const common::PathPoint& path_point, const double s) { + return path_point.s() < s; + }; + + auto it_lower = + std::lower_bound(path_points.begin(), path_points.end(), s, comp); + if (it_lower == path_points.begin()) { + *path_point = *it_lower; + } else { + double s0 = (it_lower - 1)->s(); + double s1 = it_lower->s(); + CHECK(s0 < s1); + *path_point = + util::interpolate_linear_approximation(*(it_lower - 1), *it_lower, s); + } + return true; +} + +bool PathData::get_path_point_with_ref_s( + const double ref_s, common::PathPoint* const path_point) const { + const auto& frenet_points = frenet_path_.points(); + if (frenet_points.size() < 2 || ref_s < frenet_points.front().s() || + frenet_points.back().s() < ref_s) { + return false; + } + + auto comp = [](const common::FrenetFramePoint& frenet_point, + const double ref_s) { return frenet_point.s() < ref_s; }; + + auto it_lower = + std::lower_bound(frenet_points.begin(), frenet_points.end(), ref_s, comp); + if (it_lower == frenet_points.begin()) { + *path_point = path_.path_points().front(); + } else { + // std::size_t index_lower = (std::size_t)(it_lower - + // frenet_points.begin()); + // + // double ref_s0 = (it_lower - 1)->s(); + // double ref_s1 = it_lower->s(); + // + // CHECK(ref_s0 < ref_s1); + // double weight = (ref_s - ref_s0) / (ref_s1 - ref_s0); + // *path_point = + // common::PathPoint::interpolate_linear_approximation(path_.path_point_at(index_lower + // - 1), + // path_.path_point_at(index_lower), weight); + } + return true; +} + +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/path/path_data.h b/modules/planning/common/path/path_data.h new file mode 100644 index 0000000000..6d5d511959 --- /dev/null +++ b/modules/planning/common/path/path_data.h @@ -0,0 +1,64 @@ +/****************************************************************************** + * 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 path_data.h + **/ + +#ifndef MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_ +#define MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_ + +#include + +#include "modules/planning/common/path/discretized_path.h" +#include "modules/planning/common/path/frenet_frame_path.h" + +namespace apollo { +namespace planning { + +class PathData { + public: + PathData() = default; + + void set_path(const DiscretizedPath& path); + + void set_frenet_path(const FrenetFramePath& frenet_path); + + DiscretizedPath* mutable_path(); + + const DiscretizedPath& path() const; + + FrenetFramePath* mutable_frenet_frame_path(); + + const FrenetFramePath& frenet_frame_path() const; + + bool get_path_point_with_path_s(const double s, + common::PathPoint* const path_point) const; + + bool get_path_point_with_ref_s(const double ref_s, + common::PathPoint* const path_point) const; + + // TODO(fanhaoyang) add check if the path data is valid + private: + DiscretizedPath path_; + + FrenetFramePath frenet_path_; +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_ diff --git a/modules/planning/common/path/path_point_util.cc b/modules/planning/common/path/path_point_util.cc new file mode 100644 index 0000000000..e5730b6cc7 --- /dev/null +++ b/modules/planning/common/path/path_point_util.cc @@ -0,0 +1,98 @@ +/****************************************************************************** + * 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 path_point_util.cc + **/ + +#include "modules/planning/common/path/path_point_util.h" + +#include + +#include "modules/planning/math/hermite_spline.h" +#include "modules/planning/math/integration.h" + +namespace apollo { +namespace planning { +namespace util { + +common::PathPoint interpolate(const common::PathPoint& p0, + const common::PathPoint& p1, const double s) { + double s0 = p0.s(); + double s1 = p1.s(); + CHECK(s0 <= s && s <= s1); + + std::array gx0{{p0.theta(), p0.kappa()}}; + std::array gx1{{p1.theta(), p1.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 = p0.x() + Integration::gauss_legendre(func_cos_theta, s0, s); + double y = p0.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); + + common::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 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(); + CHECK(s0 < s1); + + common::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 cos_heading = + (1 - weight) * std::cos(left.theta()) + weight * std::cos(right.theta()); + double sin_heading = + (1 - weight) * std::sin(left.theta()) + weight * std::sin(right.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(); + 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(ddkappa); + return path_point; +} + +} // 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 new file mode 100644 index 0000000000..39a241d4d6 --- /dev/null +++ b/modules/planning/common/path/path_point_util.h @@ -0,0 +1,44 @@ +/****************************************************************************** + * 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 path_point_util.h + **/ + +#ifndef MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_ +#define MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_ + +#include "Eigen/Dense" + +#include "modules/common/proto/path_point.pb.h" + +namespace apollo { +namespace planning { +namespace util { + +common::PathPoint interpolate(const common::PathPoint& p0, + const 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, + const double s); + +} // namespace util +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_ diff --git a/modules/planning/common/path/sl_point_util.cc b/modules/planning/common/path/sl_point_util.cc new file mode 100644 index 0000000000..6d24df2884 --- /dev/null +++ b/modules/planning/common/path/sl_point_util.cc @@ -0,0 +1,39 @@ +/****************************************************************************** + * 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 sl_point_util.cc + **/ + +#include "modules/planning/common/path/sl_point_util.h" + +namespace apollo { +namespace planning { +namespace util { + +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; +} + +} // namespace util +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/path/sl_point_util.h b/modules/planning/common/path/sl_point_util.h new file mode 100644 index 0000000000..8a3541a5e2 --- /dev/null +++ b/modules/planning/common/path/sl_point_util.h @@ -0,0 +1,38 @@ +/****************************************************************************** + * 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 sl_point_util.h + **/ + +#ifndef MODULES_PLANNING_COMMON_PATH_SL_POINT_H_ +#define MODULES_PLANNING_COMMON_PATH_SL_POINT_H_ + +#include "modules/common/proto/path_point.pb.h" + +namespace apollo { +namespace planning { +namespace util { + +static common::SLPoint interpolate(const common::SLPoint& start, + const common::SLPoint& end, + const double weight); +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_COMMON_PATH_SL_POINT_H_ diff --git a/modules/planning/common/speed/speed_data.cc b/modules/planning/common/speed/speed_data.cc index 5836e60108..0ca12153df 100644 --- a/modules/planning/common/speed/speed_data.cc +++ b/modules/planning/common/speed/speed_data.cc @@ -29,75 +29,80 @@ namespace apollo { namespace planning { namespace { - bool speed_time_comp(const double t, const SpeedPoint& speed_point) { - return t < speed_point.t(); - } +bool speed_time_comp(const double t, const SpeedPoint& speed_point) { + return t < speed_point.t(); +} } SpeedData::SpeedData(std::vector speed_points) { - speed_vector_ = std::move(speed_points); + speed_vector_ = std::move(speed_points); } std::vector* SpeedData::mutable_speed_vector() { - return &speed_vector_; + return &speed_vector_; } const std::vector& SpeedData::speed_vector() const { - return speed_vector_; + return speed_vector_; } void SpeedData::set_speed_vector(const std::vector& speed_points) { - speed_vector_ = std::move(speed_points); + speed_vector_ = std::move(speed_points); } -bool SpeedData::get_speed_point_with_time(const double t, SpeedPoint* const speed_point) const { - if (speed_vector_.size() < 2) { - return false; - } - std::size_t index = find_index(t); - if (Double::compare(t, speed_vector_[index].t()) < 0 || index + 1 >= speed_vector_.size()) { - return false; - } - - // index index + 1 - double weight = 0.0; - if (Double::compare(speed_vector_[index + 1].t(), speed_vector_[index].t()) > 0) { - weight = (t - speed_vector_[index].t()) / - (speed_vector_[index].t() - speed_vector_[index + 1].t()); - } - - *speed_point = SpeedPoint::interpolate(speed_vector_[index], speed_vector_[index + 1], weight); - return true; +bool SpeedData::get_speed_point_with_time(const double t, + SpeedPoint* const speed_point) const { + if (speed_vector_.size() < 2) { + return false; + } + std::size_t index = find_index(t); + if (Double::compare(t, speed_vector_[index].t()) < 0 || + index + 1 >= speed_vector_.size()) { + return false; + } + + // index index + 1 + double weight = 0.0; + if (Double::compare(speed_vector_[index + 1].t(), speed_vector_[index].t()) > + 0) { + weight = (t - speed_vector_[index].t()) / + (speed_vector_[index].t() - speed_vector_[index + 1].t()); + } + + *speed_point = SpeedPoint::interpolate(speed_vector_[index], + speed_vector_[index + 1], weight); + return true; } double SpeedData::total_time() const { - if (speed_vector_.empty()) { - return 0.0; - } - return speed_vector_.back().t() - speed_vector_.front().t(); + if (speed_vector_.empty()) { + return 0.0; + } + return speed_vector_.back().t() - speed_vector_.front().t(); } std::string SpeedData::DebugString() const { - std::ostringstream sout; - sout << "[" << std::endl; - for (std::size_t i = 0; i < speed_vector_.size(); ++i) { - if (i > 0) { - sout << "," << std::endl; - } - sout << speed_vector_[i].DebugString(); + std::ostringstream sout; + sout << "[" << std::endl; + for (std::size_t i = 0; i < speed_vector_.size(); ++i) { + if (i > 0) { + sout << "," << std::endl; } - sout << "]" << std::endl; - sout.flush(); - return sout.str(); + sout << speed_vector_[i].DebugString(); + } + sout << "]" << std::endl; + sout.flush(); + return sout.str(); } std::size_t SpeedData::find_index(const double t) const { - auto upper_bound = std::upper_bound(speed_vector_.begin() + 1, speed_vector_.end(), - t, speed_time_comp); - return std::min(speed_vector_.size() - 1, - static_cast(upper_bound - speed_vector_.begin())) - 1; + auto upper_bound = std::upper_bound(speed_vector_.begin() + 1, + speed_vector_.end(), t, speed_time_comp); + return std::min( + speed_vector_.size() - 1, + static_cast(upper_bound - speed_vector_.begin())) - + 1; } -} // namespace planning -} // namespace apollo - +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/speed/speed_data.h b/modules/planning/common/speed/speed_data.h index b9f8a4f7af..8bc4ee9bb2 100644 --- a/modules/planning/common/speed/speed_data.h +++ b/modules/planning/common/speed/speed_data.h @@ -28,31 +28,31 @@ namespace apollo { namespace planning { class SpeedData { -public: - SpeedData() = default; + public: + SpeedData() = default; - SpeedData(std::vector speed_points); + SpeedData(std::vector speed_points); - std::vector* mutable_speed_vector(); + std::vector* mutable_speed_vector(); - const std::vector& speed_vector() const; + const std::vector& speed_vector() const; - void set_speed_vector(const std::vector& speed_points); + void set_speed_vector(const std::vector& speed_points); - virtual std::string DebugString() const; + virtual std::string DebugString() const; - bool get_speed_point_with_time(const double time, SpeedPoint* const speed_point) const; + bool get_speed_point_with_time(const double time, + SpeedPoint* const speed_point) const; - double total_time() const; -private: - std::size_t find_index(const double s) const; + double total_time() const; - std::vector speed_vector_; -}; - -} // namespace planning -} // namespace apollo + private: + std::size_t find_index(const double s) const; -#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_DATA_H + std::vector speed_vector_; +}; +} // namespace planning +} // namespace apollo +#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_DATA_H diff --git a/modules/planning/common/speed/speed_point.cc b/modules/planning/common/speed/speed_point.cc index 0fcf3fdc96..ab2d46fe3b 100644 --- a/modules/planning/common/speed/speed_point.cc +++ b/modules/planning/common/speed/speed_point.cc @@ -1,18 +1,18 @@ - /****************************************************************************** - * 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. - *****************************************************************************/ +/****************************************************************************** +* 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 @@ -25,56 +25,38 @@ namespace apollo { namespace planning { -SpeedPoint::SpeedPoint(const STPoint& st_point, const double v, const double a, const double j) : - STPoint(st_point), v_(v), a_(a), j_(j) { -} +SpeedPoint::SpeedPoint(const STPoint& st_point, const double v, const double a, + const double j) + : STPoint(st_point), v_(v), a_(a), j_(j) {} -SpeedPoint::SpeedPoint( - const double s, - const double t, - const double v, - const double a, - const double j) : STPoint(s, t), v_(v), a_(a), j_(j) { -} +SpeedPoint::SpeedPoint(const double s, const double t, const double v, + const double a, const double j) + : STPoint(s, t), v_(v), a_(a), j_(j) {} -void SpeedPoint::set_v(const double v) { - v_ = v; -} - -void SpeedPoint::set_a(const double a) { - a_ = a; -} +void SpeedPoint::set_v(const double v) { v_ = v; } -void SpeedPoint::set_j(const double j) { - j_ = j; -} +void SpeedPoint::set_a(const double a) { a_ = a; } -double SpeedPoint::v() const { - return v_; -} +void SpeedPoint::set_j(const double j) { j_ = j; } -double SpeedPoint::a() const { - return a_; -} +double SpeedPoint::v() const { return v_; } -double SpeedPoint::j() const { - return j_; -} +double SpeedPoint::a() const { return a_; } -SpeedPoint SpeedPoint::interpolate(const SpeedPoint& left, const SpeedPoint& right, - const double weight) { - double s = (1 - weight) * left.s() + weight * right.s(); - double t = (1 - weight) * left.t() + weight * right.t(); - double v = (1 - weight) * left.v() + weight * right.v(); - double a = (1 - weight) * left.a() + weight * right.a(); - double j = (1 - weight) * left.j() + weight * right.j(); - return SpeedPoint(s, t, v, a, j); -} +double SpeedPoint::j() const { return j_; } -std::string SpeedPoint::DebugString() const { - return ""; +SpeedPoint SpeedPoint::interpolate(const SpeedPoint& left, + const SpeedPoint& right, + const double weight) { + double s = (1 - weight) * left.s() + weight * right.s(); + double t = (1 - weight) * left.t() + weight * right.t(); + double v = (1 - weight) * left.v() + weight * right.v(); + double a = (1 - weight) * left.a() + weight * right.a(); + double j = (1 - weight) * left.j() + weight * right.j(); + return SpeedPoint(s, t, v, a, j); } -} // namespace planning -} // namespace apollo +std::string SpeedPoint::DebugString() const { return ""; } +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/speed/speed_point.h b/modules/planning/common/speed/speed_point.h index 5f1ab3cc62..4519ba0a80 100644 --- a/modules/planning/common/speed/speed_point.h +++ b/modules/planning/common/speed/speed_point.h @@ -27,33 +27,31 @@ namespace apollo { namespace planning { class SpeedPoint : public STPoint { -public: - SpeedPoint() = default; - SpeedPoint(const double s, - const double t, - const double v, - const double a, - const double j); - SpeedPoint(const STPoint& st_point, const double v, const double a, const double j); - void set_v(const double v); - void set_a(const double a); - void set_j(const double j); - - double v() const; - double a() const; - double j() const; - - std::string DebugString() const; - static SpeedPoint interpolate(const SpeedPoint& left, const SpeedPoint& right, - const double weight); -private: - double v_ = 0.0; - double a_ = 0.0; - double j_ = 0.0; + public: + SpeedPoint() = default; + SpeedPoint(const double s, const double t, const double v, const double a, + const double j); + SpeedPoint(const STPoint& st_point, const double v, const double a, + const double j); + void set_v(const double v); + void set_a(const double a); + void set_j(const double j); + + double v() const; + double a() const; + double j() const; + + std::string DebugString() const; + static SpeedPoint interpolate(const SpeedPoint& left, const SpeedPoint& right, + const double weight); + + private: + double v_ = 0.0; + double a_ = 0.0; + double j_ = 0.0; }; -} // namespace planning -} // namespace apollo - -#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_POINT_H +} // namespace planning +} // namespace apollo +#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_POINT_H diff --git a/modules/planning/common/speed/st_point.cc b/modules/planning/common/speed/st_point.cc index e517e9dacf..32f93447f9 100644 --- a/modules/planning/common/speed/st_point.cc +++ b/modules/planning/common/speed/st_point.cc @@ -26,33 +26,23 @@ namespace apollo { namespace planning { -STPoint::STPoint(const double s, const double t) : Vec2d(t, s) { -}; +STPoint::STPoint(const double s, const double t) : Vec2d(t, s){}; -double STPoint::s() const { - return y_; -} +double STPoint::s() const { return y_; } -double STPoint::t() const { - return x_; -} +double STPoint::t() const { return x_; } -void STPoint::set_s(const double s) { - return set_y(s); -} +void STPoint::set_s(const double s) { return set_y(s); } -void STPoint::set_t(const double t) { - return set_x(t); -} +void STPoint::set_t(const double t) { return set_x(t); } std::string STPoint::DebugString() const { - std::ostringstream sout; - sout << "{ \"s\" : " << std::setprecision(6) << s() - << ", \"t\" : " << std::setprecision(6) << t() << " }"; - sout.flush(); - return sout.str(); + std::ostringstream sout; + sout << "{ \"s\" : " << std::setprecision(6) << s() + << ", \"t\" : " << std::setprecision(6) << t() << " }"; + sout.flush(); + return sout.str(); } -} // namespace planning -} // namespace apollo - +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/speed/st_point.h b/modules/planning/common/speed/st_point.h index 4c4b78d617..f81569c8b7 100644 --- a/modules/planning/common/speed/st_point.h +++ b/modules/planning/common/speed/st_point.h @@ -27,18 +27,17 @@ namespace apollo { namespace planning { class STPoint : public ::apollo::common::math::Vec2d { -public: - STPoint() = default; - STPoint(const double s, const double t); - double s() const; - double t() const; - void set_s(const double s); - void set_t(const double t); - std::string DebugString() const; + public: + STPoint() = default; + STPoint(const double s, const double t); + double s() const; + double t() const; + void set_s(const double s); + void set_t(const double t); + std::string DebugString() const; }; -} // namespace planning -} // namespace apollo - -#endif // MODULES_PLANNING_COMMON_SPEED_ST_POINT_H +} // namespace planning +} // namespace apollo +#endif // MODULES_PLANNING_COMMON_SPEED_ST_POINT_H diff --git a/modules/planning/math/spiral_curve/quintic_spiral_curve.cc b/modules/planning/math/spiral_curve/quintic_spiral_curve.cc index be1d85a5a7..eaede18a33 100644 --- a/modules/planning/math/spiral_curve/quintic_spiral_curve.cc +++ b/modules/planning/math/spiral_curve/quintic_spiral_curve.cc @@ -33,7 +33,8 @@ namespace planning { using ::apollo::common::ErrorCode; using ::apollo::common::PathPoint; -QuinticSpiralCurve::QuinticSpiralCurve(const PathPoint& s, const PathPoint& e) +QuinticSpiralCurve::QuinticSpiralCurve(const common::PathPoint& s, + const common::PathPoint& e) : SpiralCurve(s, e, 5) { // generate an order 5 spiral path with four parameters } @@ -191,7 +192,7 @@ bool QuinticSpiralCurve::calculate_path() { } ErrorCode QuinticSpiralCurve::get_path_vec( - const std::size_t n, std::vector* path_points) const { + const std::size_t n, std::vector* path_points) const { CHECK_NOTNULL(path_points); if (n <= 1 || error() > spiral_config().newton_raphson_tol()) { @@ -199,7 +200,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec( } path_points->resize(n); - std::vector& result = *path_points; + std::vector& result = *path_points; result[0].set_x(start_point().x()); result[0].set_y(start_point().y()); @@ -247,7 +248,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec( ErrorCode QuinticSpiralCurve::get_path_vec_with_s( const std::vector& vec_s, - std::vector* path_points) const { + std::vector* path_points) const { CHECK_NOTNULL(path_points); if (error() > spiral_config().newton_raphson_tol()) { @@ -260,10 +261,10 @@ ErrorCode QuinticSpiralCurve::get_path_vec_with_s( const std::size_t n = vec_s.size(); - PathPoint ref_point = start_point(); + common::PathPoint ref_point = start_point(); path_points->resize(n); - std::vector& result = *path_points; + std::vector& result = *path_points; std::array p_value; std::copy(p_params().begin(), p_params().end(), p_value.begin()); std::array a_params = SpiralFormula::p_to_a_k5(sg(), p_value); diff --git a/modules/planning/math/spiral_curve/spiral_curve.cc b/modules/planning/math/spiral_curve/spiral_curve.cc index f2be6e4580..b12c201a64 100644 --- a/modules/planning/math/spiral_curve/spiral_curve.cc +++ b/modules/planning/math/spiral_curve/spiral_curve.cc @@ -27,7 +27,7 @@ namespace planning { using apollo::common::PathPoint; -SpiralCurve::SpiralCurve(const PathPoint& s, const PathPoint& e, +SpiralCurve::SpiralCurve(const common::PathPoint& s, const common::PathPoint& e, const std::size_t order) : p_params_(order + 1, 0.0), sg_(0.0), @@ -42,9 +42,11 @@ void SpiralCurve::set_spiral_config(const SpiralCurveConfig& spiral_config) { } // output params -const PathPoint& SpiralCurve::start_point() const { return *start_point_; } +const common::PathPoint& SpiralCurve::start_point() const { + return *start_point_; +} -const PathPoint& SpiralCurve::end_point() const { return *end_point_; } +const common::PathPoint& SpiralCurve::end_point() const { return *end_point_; } double SpiralCurve::sg() const { return sg_; } diff --git a/modules/planning/planner/em_planner.cc b/modules/planning/planner/em_planner.cc index 39464bbe23..636758f66b 100644 --- a/modules/planning/planner/em_planner.cc +++ b/modules/planning/planner/em_planner.cc @@ -28,13 +28,10 @@ namespace planning { using apollo::common::TrajectoryPoint; using apollo::common::vehicle_state::VehicleState; -EMPlanner::EMPlanner() { -} - -bool EMPlanner::Plan( - const TrajectoryPoint& start_point, - std::vector* discretized_trajectory) { +EMPlanner::EMPlanner() {} +bool EMPlanner::Plan(const TrajectoryPoint& start_point, + std::vector* discretized_trajectory) { return true; } diff --git a/modules/planning/planner/em_planner.h b/modules/planning/planner/em_planner.h index 64fa4f50d0..3704596c27 100644 --- a/modules/planning/planner/em_planner.h +++ b/modules/planning/planner/em_planner.h @@ -52,9 +52,8 @@ class EMPlanner : public Planner { * @param discretized_trajectory The computed trajectory * @return true if planning succeeds; false otherwise. */ - bool Plan( - const apollo::common::TrajectoryPoint& start_point, - std::vector* trajectory) override; + bool Plan(const apollo::common::TrajectoryPoint& start_point, + std::vector* trajectory) override; private: }; diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc index b037d3ca2e..15541524d2 100644 --- a/modules/planning/planning.cc +++ b/modules/planning/planning.cc @@ -13,6 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ +#include "modules/planning/planning.h" #include "modules/common/adapters/adapter_manager.h" #include "modules/common/time/time.h" #include "modules/planning/common/planning_gflags.h" diff --git a/modules/planning/planning.h b/modules/planning/planning.h index 62739b9a9a..156ee012de 100644 --- a/modules/planning/planning.h +++ b/modules/planning/planning.h @@ -102,9 +102,7 @@ class Planning : public apollo::common::ApolloApp { const double header_time, const std::vector& discretized_trajectory); - - apollo::common::util::Factory + apollo::common::util::Factory planner_factory_; PlanningConfig config_; diff --git a/modules/planning/planning_test.cc b/modules/planning/planning_test.cc index 9c9e328990..d762774ff7 100644 --- a/modules/planning/planning_test.cc +++ b/modules/planning/planning_test.cc @@ -14,9 +14,9 @@ * limitations under the License. *****************************************************************************/ +#include "modules/planning/planning.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/planning/common/planning_gflags.h" -#include "modules/planning/planning.h" #include "modules/planning/proto/planning.pb.h" #include "gmock/gmock.h" -- GitLab