Planning: removed unused comments.

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"

modules/planning/lattice/trajectory_generation/active_set_augmented_lateral_qp_optimizer.cc

@@ -28,11 +28,11 @@ bool ActiverSetAugmentedLateralQPOptimizer::optimize(
     const std::array<double, 3>& d_state, const double delta_s,
     const std::vector<std::pair<double, double>>& 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);

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();