From e3f099ce0ff1d3f5805af5424e6b81caee17c49c Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Thu, 27 Jul 2017 12:06:05 -0700 Subject: [PATCH] uniform the naming of 'QP' --- modules/common/math/mpc_solver.cc | 6 +-- .../math/qp_solver/active_set_qp_solver.cc | 40 +++++++++---------- .../math/qp_solver/active_set_qp_solver.h | 4 +- modules/common/math/qp_solver/qp_solver.cc | 16 ++++---- modules/common/math/qp_solver/qp_solver.h | 4 +- .../smoothing_spline/spline_1d_generator.cc | 2 +- .../smoothing_spline/spline_1d_generator.h | 2 +- .../math/smoothing_spline/spline_2d_solver.cc | 2 +- .../math/smoothing_spline/spline_2d_solver.h | 2 +- .../qp_spline_path_generator.cc | 16 ++++---- .../qp_spline_path/qp_spline_path_generator.h | 6 +-- .../qp_spline_path_optimizer.cc | 6 +-- .../qp_spline_path/qp_spline_path_optimizer.h | 6 +-- .../qp_spline_path/qp_spline_path_sampler.cc | 2 +- .../qp_spline_path/qp_spline_path_sampler.h | 4 +- modules/planning/planner/em/em_planner.cc | 2 +- .../proto/qp_spline_path_config.proto | 2 +- 17 files changed, 61 insertions(+), 61 deletions(-) diff --git a/modules/common/math/mpc_solver.cc b/modules/common/math/mpc_solver.cc index 038b1d5be3..bbeb75a2da 100644 --- a/modules/common/math/mpc_solver.cc +++ b/modules/common/math/mpc_solver.cc @@ -24,8 +24,8 @@ namespace common { namespace math { using Matrix = Eigen::MatrixXd; -using ::apollo::common::math::QPSolver; -using ::apollo::common::math::ActiveSetQPSolver; +using ::apollo::common::math::QpSolver; +using ::apollo::common::math::ActiveSetQpSolver; // discrete linear predictive control solver, with control format // x(i + 1) = A * x(i) + B * u (i) + C @@ -130,7 +130,7 @@ void SolveLinearMPC(const Matrix &matrix_a, Eigen::MatrixXd matrix_equality_boundary = Eigen::MatrixXd::Zero( matrix_ll.rows() + matrix_uu.rows(), matrix_ll.cols()); - std::unique_ptr qp_solver(new ActiveSetQPSolver(matrix_m1, + std::unique_ptr qp_solver(new ActiveSetQpSolver(matrix_m1, matrix_m2, matrix_inequality_constrain, matrix_inequality_boundary, diff --git a/modules/common/math/qp_solver/active_set_qp_solver.cc b/modules/common/math/qp_solver/active_set_qp_solver.cc index 758ec591cd..bf16cc0b3f 100644 --- a/modules/common/math/qp_solver/active_set_qp_solver.cc +++ b/modules/common/math/qp_solver/active_set_qp_solver.cc @@ -29,13 +29,13 @@ namespace apollo { namespace common { namespace math { -ActiveSetQPSolver::ActiveSetQPSolver( +ActiveSetQpSolver::ActiveSetQpSolver( const Eigen::MatrixXd& kernel_matrix, const Eigen::MatrixXd& offset, const Eigen::MatrixXd& affine_inequality_matrix, const Eigen::MatrixXd& affine_inequality_boundary, const Eigen::MatrixXd& affine_equality_matrix, const Eigen::MatrixXd& affine_equality_boundary) - : QPSolver(kernel_matrix, offset, affine_inequality_matrix, + : QpSolver(kernel_matrix, offset, affine_inequality_matrix, affine_inequality_boundary, affine_equality_matrix, affine_equality_boundary) { num_param_ = kernel_matrix.rows(); @@ -48,7 +48,7 @@ ActiveSetQPSolver::ActiveSetQPSolver( debug_info_ = FLAGS_default_enable_active_set_debug_info; } -bool ActiveSetQPSolver::solve() { +bool ActiveSetQpSolver::solve() { ::qpOASES::QProblem qp_problem(num_param_, num_constraint_); ::qpOASES::Options my_options; my_options.epsNum = qp_eps_num_; @@ -138,54 +138,54 @@ bool ActiveSetQPSolver::solve() { return false; } -void ActiveSetQPSolver::set_qp_eps_num(const double eps) { qp_eps_num_ = eps; } +void ActiveSetQpSolver::set_qp_eps_num(const double eps) { qp_eps_num_ = eps; } -void ActiveSetQPSolver::set_qp_eps_den(const double eps) { qp_eps_den_ = eps; } +void ActiveSetQpSolver::set_qp_eps_den(const double eps) { qp_eps_den_ = eps; } -void ActiveSetQPSolver::set_qp_eps_iter_ref(const double eps) { +void ActiveSetQpSolver::set_qp_eps_iter_ref(const double eps) { qp_eps_iter_ref_ = eps; } -void ActiveSetQPSolver::set_debug_info(const bool enable) { +void ActiveSetQpSolver::set_debug_info(const bool enable) { debug_info_ = enable; } -void ActiveSetQPSolver::set_l_lower_bound(const double l_lower_bound) { +void ActiveSetQpSolver::set_l_lower_bound(const double l_lower_bound) { l_lower_bound_ = l_lower_bound; } -void ActiveSetQPSolver::set_l_upper_bound(const double l_upper_bound) { +void ActiveSetQpSolver::set_l_upper_bound(const double l_upper_bound) { l_upper_bound_ = l_upper_bound; } -void ActiveSetQPSolver::set_constraint_upper_bound( +void ActiveSetQpSolver::set_constraint_upper_bound( const double la_upper_bound) { constraint_upper_bound_ = la_upper_bound; } -void ActiveSetQPSolver::set_max_iteration(const int max_iter) { +void ActiveSetQpSolver::set_max_iteration(const int max_iter) { max_iteration_ = max_iter; } -double ActiveSetQPSolver::qp_eps_num() const { return qp_eps_num_; } +double ActiveSetQpSolver::qp_eps_num() const { return qp_eps_num_; } -double ActiveSetQPSolver::qp_eps_den() const { return qp_eps_den_; } +double ActiveSetQpSolver::qp_eps_den() const { return qp_eps_den_; } -double ActiveSetQPSolver::qp_eps_iter_ref() const { return qp_eps_iter_ref_; } +double ActiveSetQpSolver::qp_eps_iter_ref() const { return qp_eps_iter_ref_; } -bool ActiveSetQPSolver::debug_info() const { return debug_info_; } +bool ActiveSetQpSolver::debug_info() const { return debug_info_; } -double ActiveSetQPSolver::l_lower_bound() const { return l_lower_bound_; } +double ActiveSetQpSolver::l_lower_bound() const { return l_lower_bound_; } -double ActiveSetQPSolver::l_upper_bound() const { return l_upper_bound_; } +double ActiveSetQpSolver::l_upper_bound() const { return l_upper_bound_; } -double ActiveSetQPSolver::constraint_upper_bound() const { +double ActiveSetQpSolver::constraint_upper_bound() const { return constraint_upper_bound_; } -int ActiveSetQPSolver::max_iteration() const { return max_iteration_; } +int ActiveSetQpSolver::max_iteration() const { return max_iteration_; } // pure virtual -bool ActiveSetQPSolver::sanity_check() { +bool ActiveSetQpSolver::sanity_check() { return kernel_matrix_.rows() == kernel_matrix_.cols() && kernel_matrix_.rows() == affine_inequality_matrix_.cols() && kernel_matrix_.rows() == affine_equality_matrix_.cols() && diff --git a/modules/common/math/qp_solver/active_set_qp_solver.h b/modules/common/math/qp_solver/active_set_qp_solver.h index 9cfac3b4f6..8dae1d38b8 100644 --- a/modules/common/math/qp_solver/active_set_qp_solver.h +++ b/modules/common/math/qp_solver/active_set_qp_solver.h @@ -29,9 +29,9 @@ namespace apollo { namespace common { namespace math { -class ActiveSetQPSolver : public QPSolver { +class ActiveSetQpSolver : public QpSolver { public: - ActiveSetQPSolver(const Eigen::MatrixXd& kernel_matrix, + ActiveSetQpSolver(const Eigen::MatrixXd& kernel_matrix, const Eigen::MatrixXd& offset, const Eigen::MatrixXd& affine_inequality_matrix, const Eigen::MatrixXd& affine_inequality_boundary, diff --git a/modules/common/math/qp_solver/qp_solver.cc b/modules/common/math/qp_solver/qp_solver.cc index 6d91d668e5..f3de962ed6 100644 --- a/modules/common/math/qp_solver/qp_solver.cc +++ b/modules/common/math/qp_solver/qp_solver.cc @@ -23,7 +23,7 @@ namespace apollo { namespace common { namespace math { -QPSolver::QPSolver(const Eigen::MatrixXd& kernel_matrix, +QpSolver::QpSolver(const Eigen::MatrixXd& kernel_matrix, const Eigen::MatrixXd& offset, const Eigen::MatrixXd& affine_inequality_matrix, const Eigen::MatrixXd& affine_inequality_boundary, @@ -36,27 +36,27 @@ QPSolver::QPSolver(const Eigen::MatrixXd& kernel_matrix, affine_equality_matrix_(affine_equality_matrix), affine_equality_boundary_(affine_equality_boundary) {} -const Eigen::MatrixXd& QPSolver::params() const { return params_; } +const Eigen::MatrixXd& QpSolver::params() const { return params_; } -const Eigen::MatrixXd& QPSolver::kernel_matrix() const { +const Eigen::MatrixXd& QpSolver::kernel_matrix() const { return kernel_matrix_; } -const Eigen::MatrixXd& QPSolver::offset() const { return offset_; } +const Eigen::MatrixXd& QpSolver::offset() const { return offset_; } -const Eigen::MatrixXd& QPSolver::affine_equality_matrix() const { +const Eigen::MatrixXd& QpSolver::affine_equality_matrix() const { return affine_equality_matrix_; } -const Eigen::MatrixXd& QPSolver::affine_equality_boundary() const { +const Eigen::MatrixXd& QpSolver::affine_equality_boundary() const { return affine_equality_boundary_; } -const Eigen::MatrixXd& QPSolver::affine_inequality_matrix() const { +const Eigen::MatrixXd& QpSolver::affine_inequality_matrix() const { return affine_inequality_matrix_; } -const Eigen::MatrixXd& QPSolver::affine_inequality_boundary() const { +const Eigen::MatrixXd& QpSolver::affine_inequality_boundary() const { return affine_inequality_boundary_; } diff --git a/modules/common/math/qp_solver/qp_solver.h b/modules/common/math/qp_solver/qp_solver.h index e098e3775c..021643c050 100644 --- a/modules/common/math/qp_solver/qp_solver.h +++ b/modules/common/math/qp_solver/qp_solver.h @@ -33,9 +33,9 @@ namespace apollo { namespace common { namespace math { -class QPSolver { +class QpSolver { public: - QPSolver(const Eigen::MatrixXd& kernel_matrix, const Eigen::MatrixXd& offset, + QpSolver(const Eigen::MatrixXd& kernel_matrix, const Eigen::MatrixXd& offset, const Eigen::MatrixXd& affine_inequality_matrix, const Eigen::MatrixXd& affine_inequality_boundary, const Eigen::MatrixXd& affine_equality_matrix, diff --git a/modules/planning/math/smoothing_spline/spline_1d_generator.cc b/modules/planning/math/smoothing_spline/spline_1d_generator.cc index b341107ad9..639c6e32ff 100644 --- a/modules/planning/math/smoothing_spline/spline_1d_generator.cc +++ b/modules/planning/math/smoothing_spline/spline_1d_generator.cc @@ -64,7 +64,7 @@ bool Spline1dGenerator::solve() { const Eigen::MatrixXd& equality_constraint_boundary = spline_constraint_.equality_constraint().constraint_boundary(); - qp_solver_.reset(new apollo::common::math::ActiveSetQPSolver( + qp_solver_.reset(new apollo::common::math::ActiveSetQpSolver( kernel_matrix, offset, inequality_constraint_matrix, inequality_constraint_boundary, equality_constraint_matrix, equality_constraint_boundary)); diff --git a/modules/planning/math/smoothing_spline/spline_1d_generator.h b/modules/planning/math/smoothing_spline/spline_1d_generator.h index 325b105831..218b0fa982 100644 --- a/modules/planning/math/smoothing_spline/spline_1d_generator.h +++ b/modules/planning/math/smoothing_spline/spline_1d_generator.h @@ -88,7 +88,7 @@ class Spline1dGenerator { Spline1dConstraint spline_constraint_; Spline1dKernel spline_kernel_; - std::unique_ptr qp_solver_ = nullptr; + std::unique_ptr qp_solver_ = nullptr; Eigen::MatrixXd init_x_; Eigen::MatrixXd init_y_; Eigen::MatrixXd init_z_; diff --git a/modules/planning/math/smoothing_spline/spline_2d_solver.cc b/modules/planning/math/smoothing_spline/spline_2d_solver.cc index e05d137dc7..55a3899ff3 100644 --- a/modules/planning/math/smoothing_spline/spline_2d_solver.cc +++ b/modules/planning/math/smoothing_spline/spline_2d_solver.cc @@ -52,7 +52,7 @@ bool Spline2dSolver::solve() { const Eigen::MatrixXd& equality_constraint_boundary = constraint_.equality_constraint().constraint_boundary(); - qp_solver_.reset(new apollo::common::math::ActiveSetQPSolver( + qp_solver_.reset(new apollo::common::math::ActiveSetQpSolver( kernel_matrix, offset, inequality_constraint_matrix, inequality_constraint_boundary, equality_constraint_matrix, equality_constraint_boundary)); diff --git a/modules/planning/math/smoothing_spline/spline_2d_solver.h b/modules/planning/math/smoothing_spline/spline_2d_solver.h index 5ad18b1b30..3c2a9d3b22 100644 --- a/modules/planning/math/smoothing_spline/spline_2d_solver.h +++ b/modules/planning/math/smoothing_spline/spline_2d_solver.h @@ -51,7 +51,7 @@ class Spline2dSolver { Spline2d spline_; Spline2dKernel kernel_; Spline2dConstraint constraint_; - std::unique_ptr qp_solver_ = nullptr; + std::unique_ptr qp_solver_ = nullptr; }; } // namespace planning 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 6e165261ba..b471550fc0 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 @@ -36,7 +36,7 @@ namespace apollo { namespace planning { -bool QPSplinePathGenerator::SetConfig(const std::string& config_file) { +bool QpSplinePathGenerator::SetConfig(const std::string& config_file) { if (!common::util::GetProtoFromFile(config_file, &_qp_spline_path_config)) { AERROR << "Failed to load config file " << config_file; return false; @@ -44,7 +44,7 @@ bool QPSplinePathGenerator::SetConfig(const std::string& config_file) { return true; } -bool QPSplinePathGenerator::generate(const ReferenceLine& reference_line, +bool QpSplinePathGenerator::generate(const ReferenceLine& reference_line, const DecisionData& decision_data, const SpeedData& speed_data, const common::TrajectoryPoint& init_point, @@ -140,7 +140,7 @@ bool QPSplinePathGenerator::generate(const ReferenceLine& reference_line, return true; } -bool QPSplinePathGenerator::calculate_sl_point( +bool QpSplinePathGenerator::calculate_sl_point( const ReferenceLine& reference_line, const common::TrajectoryPoint& traj_point, common::FrenetFramePoint* const frenet_frame_point) { @@ -173,7 +173,7 @@ bool QPSplinePathGenerator::calculate_sl_point( return true; } -bool QPSplinePathGenerator::init_coord_range(double* const start_s, +bool QpSplinePathGenerator::init_coord_range(double* const start_s, double* const end_s) { // TODO(all): step 1 get current sl coordinate - with init coordinate point double start_point = std::max(_init_point.s() - 5.0, 0.0); @@ -194,7 +194,7 @@ bool QPSplinePathGenerator::init_coord_range(double* const start_s, return true; } -bool QPSplinePathGenerator::init_smoothing_spline( +bool QpSplinePathGenerator::init_smoothing_spline( const ReferenceLine& reference_line, const double start_s, const double end_s) { QpSplinePathSampler sampler(_qp_spline_path_config); @@ -212,7 +212,7 @@ bool QPSplinePathGenerator::init_smoothing_spline( return true; } -bool QPSplinePathGenerator::setup_constraint() { +bool QpSplinePathGenerator::setup_constraint() { Spline1dConstraint* spline_constraint = _spline_generator->mutable_spline_constraint(); // add init status constraint @@ -284,7 +284,7 @@ bool QPSplinePathGenerator::setup_constraint() { return true; } -bool QPSplinePathGenerator::setup_kernel() { +bool QpSplinePathGenerator::setup_kernel() { Spline1dKernel* spline_kernel = _spline_generator->mutable_spline_kernel(); if (_qp_spline_path_config.regularization_weight() > 0) { @@ -311,7 +311,7 @@ bool QPSplinePathGenerator::setup_kernel() { return true; } -bool QPSplinePathGenerator::solve() { +bool QpSplinePathGenerator::solve() { if (!_spline_generator->solve()) { AERROR << "Could not solve the qp problem in spline path generator."; 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 db05ebf7c5..c992478f89 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 @@ -36,9 +36,9 @@ namespace apollo { namespace planning { -class QPSplinePathGenerator { +class QpSplinePathGenerator { public: - QPSplinePathGenerator() = default; + QpSplinePathGenerator() = default; bool SetConfig(const std::string& config_file); bool generate(const ReferenceLine& reference_line, const DecisionData& decision_data, const SpeedData& speed_data, @@ -68,7 +68,7 @@ class QPSplinePathGenerator { double* const y_diff) const; private: - QPSplinePathConfig _qp_spline_path_config; + QpSplinePathConfig _qp_spline_path_config; common::FrenetFramePoint _init_point; QpFrenetFrame _qp_frenet_frame; std::unique_ptr _spline_generator; diff --git a/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc b/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc index 16de6406c8..1daaa68d85 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc +++ b/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc @@ -27,10 +27,10 @@ namespace planning { using apollo::common::ErrorCode; using apollo::common::Status; -QPSplinePathOptimizer::QPSplinePathOptimizer(const std::string& name) +QpSplinePathOptimizer::QpSplinePathOptimizer(const std::string& name) : PathOptimizer(name) {} -bool QPSplinePathOptimizer::Init() { +bool QpSplinePathOptimizer::Init() { if (!_path_generator.SetConfig(FLAGS_qp_spline_path_config_file)) { AERROR << "Fail to set config file for path generator."; return false; @@ -39,7 +39,7 @@ bool QPSplinePathOptimizer::Init() { return true; } -Status QPSplinePathOptimizer::Process(const SpeedData& speed_data, +Status QpSplinePathOptimizer::Process(const SpeedData& speed_data, const ReferenceLine& reference_line, const common::TrajectoryPoint& init_point, DecisionData* const decision_data, diff --git a/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h b/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h index 735d34ab3e..de808e924e 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h +++ b/modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h @@ -30,9 +30,9 @@ namespace apollo { namespace planning { -class QPSplinePathOptimizer : public PathOptimizer { +class QpSplinePathOptimizer : public PathOptimizer { public: - explicit QPSplinePathOptimizer(const std::string& name); + explicit QpSplinePathOptimizer(const std::string& name); bool Init() override; private: @@ -43,7 +43,7 @@ class QPSplinePathOptimizer : public PathOptimizer { PathData* const path_data) override; private: - QPSplinePathGenerator _path_generator; + QpSplinePathGenerator _path_generator; }; } // namespace planning diff --git a/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc b/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc index 24cdf7e113..96644c3b06 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc +++ b/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc @@ -28,7 +28,7 @@ namespace apollo { namespace planning { -QpSplinePathSampler::QpSplinePathSampler(const QPSplinePathConfig& config) +QpSplinePathSampler::QpSplinePathSampler(const QpSplinePathConfig& config) : config_(config) {} bool QpSplinePathSampler::Sample(const common::FrenetFramePoint& init_point, diff --git a/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h b/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h index 5e08067d2c..49ca6dc9b5 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h +++ b/modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h @@ -31,14 +31,14 @@ namespace planning { class QpSplinePathSampler { public: - explicit QpSplinePathSampler(const QPSplinePathConfig& config); + explicit QpSplinePathSampler(const QpSplinePathConfig& config); bool Sample(const common::FrenetFramePoint& init_point, const ReferenceLine& reference_line, const double s_lower_bound, const double s_upper_bound, std::vector* const sampling_point); private: - const QPSplinePathConfig config_; + const QpSplinePathConfig config_; }; } // namespace planning diff --git a/modules/planning/planner/em/em_planner.cc b/modules/planning/planner/em/em_planner.cc index 45039c5520..6ad1ad323c 100644 --- a/modules/planning/planner/em/em_planner.cc +++ b/modules/planning/planner/em/em_planner.cc @@ -55,7 +55,7 @@ void EMPlanner::RegisterOptimizers() { return new DpStSpeedOptimizer("DpStSpeedOptimizer"); }); optimizer_factory_.Register(QP_SPLINE_PATH_OPTIMIZER, []() -> Optimizer* { - return new QPSplinePathOptimizer("QPSplinePathOptimizer"); + return new QpSplinePathOptimizer("QpSplinePathOptimizer"); }); optimizer_factory_.Register(QP_SPLINE_ST_SPEED_OPTIMIZER, []() -> Optimizer* { return new QpSplineStSpeedOptimizer("QpSplineStSpeedOptimizer"); diff --git a/modules/planning/proto/qp_spline_path_config.proto b/modules/planning/proto/qp_spline_path_config.proto index 4bab199e37..119950c7c3 100644 --- a/modules/planning/proto/qp_spline_path_config.proto +++ b/modules/planning/proto/qp_spline_path_config.proto @@ -2,7 +2,7 @@ syntax = "proto2"; package apollo.planning; -message QPSplinePathConfig { +message QpSplinePathConfig { optional uint32 spline_order = 1 [default = 6]; optional uint32 number_of_spline = 2 [default = 5]; optional uint32 number_of_fx_constraint_knots = 3 [default = 6]; -- GitLab