From 2e43db7edc7295cc865bfaf05f50b43b8303bec1 Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Fri, 4 Aug 2017 13:33:49 -0700 Subject: [PATCH] simplified AddConstraint function for qp_spline_path --- .../qp_spline_path_generator.cc | 93 ++++++++----------- .../qp_spline_path/qp_spline_path_generator.h | 5 +- 2 files changed, 42 insertions(+), 56 deletions(-) diff --git a/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc b/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc index 33983bab3c..7e4f7d9537 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc +++ b/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc @@ -68,7 +68,7 @@ bool QpSplinePathGenerator::Generate(const DecisionData& decision_data, } AINFO << "pss path start with " << start_s << ", end with " << end_s; - if (!InitSmoothingSpline(init_frenet_point_, start_s, end_s - 0.1)) { + if (!InitSpline(init_frenet_point_, start_s, end_s - 0.1)) { AERROR << "Init smoothing spline failed with (" << start_s << ", end_s " << end_s; return false; @@ -189,10 +189,11 @@ bool QpSplinePathGenerator::InitCoordRange(const QpFrenetFrame& qp_frenet_frame, return true; } -bool QpSplinePathGenerator::InitSmoothingSpline( +bool QpSplinePathGenerator::InitSpline( const common::FrenetFramePoint& init_frenet_point, const double start_s, const double end_s) { + // set knots if (qp_spline_path_config_.number_of_knots() <= 1) { AERROR << "Two few number of knots: " << qp_spline_path_config_.number_of_knots(); @@ -210,9 +211,29 @@ bool QpSplinePathGenerator::InitSmoothingSpline( knots_.push_back(curr_knot_s); curr_knot_s += delta_s; } + + // spawn a new spline generator spline_generator_.reset( new Spline1dGenerator(knots_, qp_spline_path_config_.spline_order())); + // set evaluated_s_ + std::uint32_t num_evaluated_s = + qp_spline_path_config_.number_of_fx_constraint_knots(); + if (num_evaluated_s <= 2) { + AERROR << "Too few evaluated positions. Suggest: > 2, current number: " + << num_evaluated_s; + return false; + } + const double ds = (spline_generator_->spline().x_knots().back() - + spline_generator_->spline().x_knots().front()) / + num_evaluated_s; + double curr_evaluated_s = spline_generator_->spline().x_knots().front(); + + for (uint32_t i = 0; i < num_evaluated_s; ++i) { + evaluated_s_.push_back(curr_evaluated_s); + curr_evaluated_s += ds; + } + return true; } @@ -231,64 +252,28 @@ bool QpSplinePathGenerator::AddConstraint( AINFO << "init frenet point: " << init_frenet_point_.ShortDebugString(); // add end point constraint - const std::vector spline_knots = - spline_generator_->spline().x_knots(); - if (spline_knots.size() < 2) { - AERROR << common::util::StrCat("Smoothing spline knot size(", - spline_knots.size(), ") < 2"); - return false; - } - double s_length = spline_knots.back() - spline_knots.front(); - if (s_length <= 0) { - AERROR << common::util::StrCat("Smoothing spline knot length(", s_length, - ") <= 0"); - return false; - } + spline_constraint->add_point_fx_constraint(knots_.back(), 0.0); + spline_constraint->add_point_derivative_constraint(knots_.back(), 0.0); + spline_constraint->add_point_second_derivative_constraint(knots_.back(), 0.0); - std::pair boundary = std::make_pair(0.0, 0.0); - double end_ref_l = (boundary.first + boundary.second) / 2.0; - spline_constraint->add_point_fx_constraint(spline_knots.back(), end_ref_l); - spline_constraint->add_point_derivative_constraint(spline_knots.back(), 0.0); - spline_constraint->add_point_second_derivative_constraint(spline_knots.back(), - 0.0); - AINFO << "end frenet point:" << s_length << ", " << end_ref_l - << ", 0.0, 0.0."; - const std::vector sampling_knots = spline_knots; - - if (sampling_knots.size() <= 2) { - return false; - } - - std::uint32_t num_fx_bound = - qp_spline_path_config_.number_of_fx_constraint_knots(); + // add map bound constraint std::vector boundary_low; std::vector boundary_high; - std::vector fx_knots; - - if (num_fx_bound > 1) { - double ds = (sampling_knots.back() - sampling_knots.front()) / num_fx_bound; - double s = sampling_knots.front(); - - for (std::uint32_t i = 0; i < num_fx_bound + 1; ++i) { - fx_knots.push_back(s); - std::pair boundary = std::make_pair(0.0, 0.0); - qp_frenet_frame.GetMapBound(s, &boundary); - boundary_low.push_back(boundary.first); - boundary_high.push_back(boundary.second); - s += ds; - // calculate boundary here - } - - if (!spline_constraint->add_fx_boundary(fx_knots, boundary_low, - boundary_high)) { - AERROR << "Add boundary constraint failed"; - return false; - } + for (const double s : evaluated_s_) { + std::pair boundary = std::make_pair(0.0, 0.0); + qp_frenet_frame.GetMapBound(s, &boundary); + boundary_low.push_back(boundary.first); + boundary_high.push_back(boundary.second); + } + if (!spline_constraint->add_fx_boundary(knots_, boundary_low, + boundary_high)) { + AERROR << "Add boundary constraint failed"; + return false; } - // add smooth jointness constraint + // add spline joint third derivative constraint if (!spline_constraint->add_third_derivative_smooth_constraint()) { - AERROR << "Add spline jointness constraint failed!"; + AERROR << "Add spline joint third derivative constraint failed!"; return false; } diff --git a/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h b/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h index 25b691c186..0143f435b1 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h +++ b/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h @@ -51,8 +51,8 @@ class QpSplinePathGenerator { bool InitCoordRange(const QpFrenetFrame& qp_frenet_frame, double* const start_s, double* const end_s); - bool InitSmoothingSpline(const common::FrenetFramePoint& init_frenet_point, - const double start_s, const double end_s); + bool InitSpline(const common::FrenetFramePoint& init_frenet_point, + const double start_s, const double end_s); bool AddConstraint(const QpFrenetFrame& qp_frenet_frame); @@ -67,6 +67,7 @@ class QpSplinePathGenerator { std::unique_ptr spline_generator_; std::vector knots_; + std::vector evaluated_s_; }; } // namespace planning -- GitLab