From f0b53f76ccb978fbe5fda4a45a94ed4a051f4383 Mon Sep 17 00:00:00 2001 From: Liangliang Zhang Date: Sun, 7 Oct 2018 00:30:21 -0700 Subject: [PATCH] Planning: removed unused comments. --- apollo.sh | 12 +- ...tive_set_augmented_lateral_qp_optimizer.cc | 106 ++++++------------ .../qp_piecewise_jerk_path_optimizer.cc | 5 +- 3 files changed, 43 insertions(+), 80 deletions(-) diff --git a/apollo.sh b/apollo.sh index 4a370a62eb..aadb72461d 100755 --- a/apollo.sh +++ b/apollo.sh @@ -418,10 +418,11 @@ function citest_basic() { JOB_ARG="--jobs=$(nproc) --ram_utilization_factor 80" echo "$BUILD_TARGETS" | grep "modules\/" | grep "test" \ - | grep -v "planning\/" \ - | grep -v "prediction\/" \ - | grep -v "control\/" \ - | grep -v "common\/" \ + | grep -v "modules\/integration_test" \ + | grep -v "modules\/planning" \ + | grep -v "modules\/prediction" \ + | grep -v "modules\/control" \ + | grep -v "modules\/common" \ | grep -v "can_client" \ | grep -v "blob_test" \ | grep -v "syncedmem_test" | grep -v "blob_test" \ @@ -447,7 +448,8 @@ function citest_extended() { source framework/cybertron/setup.bash BUILD_TARGETS=" - `bazel query //modules/planning/... union //framework/... union //modules/prediction/... union //modules/control/... union //modules/common/...` + `bazel query //modules/planning/... union //modules/common/... union //modules/integration_test/...` + `bazel query //modules/prediction/... union //modules/control/...` " JOB_ARG="--jobs=$(nproc) --ram_utilization_factor 80" diff --git a/modules/planning/lattice/trajectory_generation/active_set_augmented_lateral_qp_optimizer.cc b/modules/planning/lattice/trajectory_generation/active_set_augmented_lateral_qp_optimizer.cc index e1deece52a..8b413f3222 100644 --- a/modules/planning/lattice/trajectory_generation/active_set_augmented_lateral_qp_optimizer.cc +++ b/modules/planning/lattice/trajectory_generation/active_set_augmented_lateral_qp_optimizer.cc @@ -28,11 +28,11 @@ bool ActiverSetAugmentedLateralQPOptimizer::optimize( const std::array& d_state, const double delta_s, const std::vector>& d_bounds) { delta_s_ = delta_s; - const int num_var = d_bounds.size(); + const int kNumVariable = d_bounds.size(); - const int kNumConstraint = (num_var - 3); + const int kNumConstraint = (kNumVariable - 3); ::qpOASES::HessianType hessian_type = ::qpOASES::HST_POSDEF; - ::qpOASES::QProblem qp_problem(num_var, kNumConstraint, hessian_type); + ::qpOASES::QProblem qp_problem(kNumVariable, kNumConstraint, hessian_type); ::qpOASES::Options my_options; my_options.enableRegularisation = ::qpOASES::BT_TRUE; @@ -45,7 +45,7 @@ bool ActiverSetAugmentedLateralQPOptimizer::optimize( int max_iter = 1000; // Construct kernel matrix - const int kNumOfMatrixElement = num_var * num_var; + const int kNumOfMatrixElement = kNumVariable * kNumVariable; double h_matrix[kNumOfMatrixElement]; std::fill(h_matrix, h_matrix + kNumOfMatrixElement, 0.0); @@ -57,56 +57,60 @@ bool ActiverSetAugmentedLateralQPOptimizer::optimize( const double one_over_delta_s_quad_coeff = 1.0 / delta_s_quad * FLAGS_weight_lateral_second_order_derivative; - for (int i = 0; i < num_var; ++i) { - h_matrix[i * num_var + i] += 2.0 * FLAGS_weight_lateral_offset; - h_matrix[i * num_var + i] += 2.0 * FLAGS_weight_lateral_obstacle_distance; + for (int i = 0; i < kNumVariable; ++i) { + h_matrix[i * kNumVariable + i] += 2.0 * FLAGS_weight_lateral_offset; + h_matrix[i * kNumVariable + i] += + 2.0 * FLAGS_weight_lateral_obstacle_distance; // first order derivative - int idx = (i + 1) * num_var + (i + 1); + int idx = (i + 1) * kNumVariable + (i + 1); if (idx < kNumOfMatrixElement) { h_matrix[idx] += 2 * one_over_delta_s_sq_coeff; - h_matrix[i * num_var + i] += 2 * one_over_delta_s_sq_coeff; - h_matrix[i * num_var + i + 1] += 2.0 * one_over_delta_s_sq_coeff; - h_matrix[(i + 1) * num_var + i] += 2.0 * one_over_delta_s_sq_coeff; + h_matrix[i * kNumVariable + i] += 2 * one_over_delta_s_sq_coeff; + h_matrix[i * kNumVariable + i + 1] += 2.0 * one_over_delta_s_sq_coeff; + h_matrix[(i + 1) * kNumVariable + i] += 2.0 * one_over_delta_s_sq_coeff; } // second order derivative - idx = (i + 2) * num_var + (i + 2); + idx = (i + 2) * kNumVariable + (i + 2); if (idx < kNumOfMatrixElement) { h_matrix[idx] += 2.0 * one_over_delta_s_quad_coeff; - h_matrix[(i + 1) * num_var + i + 1] += 8.0 * one_over_delta_s_quad_coeff; - h_matrix[i * num_var + i] += 2.0 * one_over_delta_s_quad_coeff; + h_matrix[(i + 1) * kNumVariable + i + 1] += + 8.0 * one_over_delta_s_quad_coeff; + h_matrix[i * kNumVariable + i] += 2.0 * one_over_delta_s_quad_coeff; - h_matrix[(i)*num_var + (i + 1)] += -4.0 * one_over_delta_s_quad_coeff; - h_matrix[(i + 1) * num_var + i] += -4.0 * one_over_delta_s_quad_coeff; + h_matrix[(i)*kNumVariable + (i + 1)] += + -4.0 * one_over_delta_s_quad_coeff; + h_matrix[(i + 1) * kNumVariable + i] += + -4.0 * one_over_delta_s_quad_coeff; - h_matrix[(i + 1) * num_var + (i + 2)] += + h_matrix[(i + 1) * kNumVariable + (i + 2)] += -4.0 * one_over_delta_s_quad_coeff; - h_matrix[(i + 2) * num_var + (i + 1)] += + h_matrix[(i + 2) * kNumVariable + (i + 1)] += -4.0 * one_over_delta_s_quad_coeff; - h_matrix[i * num_var + (i + 2)] += 2.0 * one_over_delta_s_quad_coeff; - h_matrix[(i + 2) * num_var + i] += 2.0 * one_over_delta_s_quad_coeff; + h_matrix[i * kNumVariable + (i + 2)] += 2.0 * one_over_delta_s_quad_coeff; + h_matrix[(i + 2) * kNumVariable + i] += 2.0 * one_over_delta_s_quad_coeff; } } // Construct offset vector - double g_matrix[num_var]; - for (int i = 0; i < num_var; ++i) { + double g_matrix[kNumVariable]; + for (int i = 0; i < kNumVariable; ++i) { g_matrix[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance * (d_bounds[i].first + d_bounds[i].second); } // Construct variable bounds - double param_lower_bounds[num_var]; - double param_upper_bounds[num_var]; - for (int i = 0; i < num_var; ++i) { + double param_lower_bounds[kNumVariable]; + double param_upper_bounds[kNumVariable]; + for (int i = 0; i < kNumVariable; ++i) { param_lower_bounds[i] = d_bounds[i].first; param_upper_bounds[i] = d_bounds[i].second; } // Construct constraint matrix - const int kNumOfConstraint = num_var * kNumConstraint; + const int kNumOfConstraint = kNumVariable * kNumConstraint; double affine_constraint_matrix[kNumOfConstraint]; std::fill(affine_constraint_matrix, affine_constraint_matrix + kNumOfConstraint, 0.0); @@ -117,8 +121,8 @@ bool ActiverSetAugmentedLateralQPOptimizer::optimize( const double third_order_derivative_max_coff = FLAGS_lateral_third_order_derivative_max * delta_s * delta_s * delta_s; - for (int i = 0; i + 3 < num_var; ++i) { - int idx = i * num_var + i; + for (int i = 0; i + 3 < kNumVariable; ++i) { + int idx = i * kNumVariable + i; affine_constraint_matrix[idx] = -1.0; affine_constraint_matrix[idx + 1] = 3.0; affine_constraint_matrix[idx + 2] = -3.0; @@ -132,48 +136,6 @@ bool ActiverSetAugmentedLateralQPOptimizer::optimize( // TODO(lianglia-apollo): // (1) Add initial status constraints // (2) Verify whether it is necessary to keep final dl and ddl to 0.0 - /* - const double kDeltaL = 1.0e-4; - int var_index = constraint_index * num_var; - affine_constraint_matrix[var_index] = 1.0; - constraint_lower_bounds[constraint_index] = d_state[0] - kDeltaL; - constraint_upper_bounds[constraint_index] = d_state[0] + kDeltaL; - ++constraint_index; - - const double kDeltaDL = 1.0e-2; - var_index = constraint_index; - affine_constraint_matrix[var_index] = -1.0 / delta_s; - affine_constraint_matrix[var_index + 1] = 1.0 / delta_s; - constraint_lower_bounds[constraint_index] = d_state[1] - kDeltaDL; - constraint_upper_bounds[constraint_index] = d_state[1] + kDeltaDL; - ++constraint_index; - - var_index = constraint_index; - affine_constraint_matrix[var_index] = 1.0 / delta_s_sq; - affine_constraint_matrix[var_index + 1] = -2.0 / delta_s_sq; - affine_constraint_matrix[var_index + 2] = 1.0 / delta_s_sq; - constraint_lower_bounds[constraint_index] = d_state[2]; - constraint_upper_bounds[constraint_index] = d_state[2]; - ++constraint_index; - */ - - /* - var_index = constraint_index * num_var - 2; - affine_constraint_matrix[var_index] = -1.0 / delta_s; - affine_constraint_matrix[var_index + 1] = 1.0 / delta_s; - constraint_lower_bounds[constraint_index] = 0.0; - constraint_upper_bounds[constraint_index] = 0.0; - ++constraint_index; - - var_index = constraint_index * num_var - 3; - affine_constraint_matrix[var_index] = 1.0 / delta_s_sq; - affine_constraint_matrix[var_index + 1] = -2.0 / delta_s_sq; - affine_constraint_matrix[var_index + 2] = 1.0 / delta_s_sq; - constraint_lower_bounds[constraint_index] = 0.0; - constraint_upper_bounds[constraint_index] = 0.0; - ++constraint_index; - */ - CHECK_EQ(constraint_index, kNumConstraint); auto ret = qp_problem.init(h_matrix, g_matrix, affine_constraint_matrix, @@ -188,9 +150,9 @@ bool ActiverSetAugmentedLateralQPOptimizer::optimize( "reasons"; } } - double result[num_var]; + double result[kNumVariable]; qp_problem.getPrimalSolution(result); - for (int i = 0; i + 2 < num_var; ++i) { + for (int i = 0; i + 2 < kNumVariable; ++i) { opt_d_.push_back(result[i]); if (i > 0) { opt_d_prime_.push_back((result[i] - result[i - 1]) / delta_s); diff --git a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc index ab199464df..4e1bd55f6f 100644 --- a/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc +++ b/modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc @@ -225,15 +225,14 @@ Status QpPiecewiseJerkPathOptimizer::Process( bool success = lateral_qp_optimizer_->optimize(lateral_state, qp_delta_s, lateral_bounds); auto end_time = Clock::NowInSeconds(); - AERROR << "lateral_qp_optimizer used time: " << (end_time - start_time) * 1000 + ADEBUG << "lateral_qp_optimizer used time: " << (end_time - start_time) * 1000 << " ms."; if (!success) { AERROR << "lateral qp optimizer failed"; - CHECK(false); + DCHECK(false); return Status(ErrorCode::PLANNING_ERROR, "lateral qp optimizer failed"); } - // CHECK(false); auto poly1d = lateral_qp_optimizer_->GetOptimalTrajectory(); -- GitLab