sync code

.gitignore

@@ -85,3 +85,4 @@ tools/FlameGraph
 modules/studio_connector
 
 #WORKSPACE
+.VSCodeCounter/

modules/planning/planners/public_road/public_road_planner.cc

@@ -28,23 +28,33 @@ using apollo::common::TrajectoryPoint;
 Status PublicRoadPlanner::Init(
     const std::shared_ptr<DependencyInjector>& injector,
     const std::string& config_path) {
+  // 根据配置注册不同的场景
+  // 支持的场景 modules/planning/planners/public_road/conf/planner_config.pb.txt
   Planner::Init(injector, config_path);
   LoadConfig<PlannerPublicRoadConfig>(config_path, &config_);
   scenario_manager_.Init(injector, config_);
   return Status::OK();
 }
 
+// 公路场景规划器的Plan函数，用于生成规划轨迹
 Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                                Frame* frame,
                                ADCTrajectory* ptr_computed_trajectory) {
+  // 更新场景管理器 frame包含场景指令信息
   scenario_manager_.Update(planning_start_point, frame);
+
+  // 获取场景
   scenario_ = scenario_manager_.mutable_scenario();
   if (!scenario_) {
+    // 如果场景为空，返回错误
     return Status(apollo::common::ErrorCode::PLANNING_ERROR,
                   "Unknown Scenario");
   }
+
+  // 处理场景并获取结果
   auto result = scenario_->Process(planning_start_point, frame);
 
+  // 如果启用了调试记录，则记录场景调试信息
   if (FLAGS_enable_record_debug) {
     auto scenario_debug = ptr_computed_trajectory->mutable_debug()
                               ->mutable_planning_data()
@@ -54,14 +64,18 @@ Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
     scenario_debug->set_msg(scenario_->GetMsg());
   }
 
+  // 仅在前一个场景的状态为STATUS_DONE时更新场景管理器
   if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_DONE) {
     // only updates scenario manager when previous scenario's status is
     // STATUS_DONE
     scenario_manager_.Update(planning_start_point, frame);
   } else if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_UNKNOWN) {
+    // 如果场景的状态为STATUS_UNKNOWN，返回错误
     return Status(common::PLANNING_ERROR,
                   result.GetTaskStatus().error_message());
   }
+
+  // 返回处理结果的状态
   return Status(common::OK, result.GetTaskStatus().error_message());
 }
 

modules/planning/planning_base/common/frame.h

@@ -227,6 +227,7 @@ class Frame {
   ADCTrajectory current_frame_planned_trajectory_;
 
   // current frame path for future possible speed fallback
+  // 当前帧中规划并发布的轨迹
   DiscretizedPath current_frame_planned_path_;
 
   const ReferenceLineProvider *reference_line_provider_ = nullptr;

modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.cc

@@ -47,15 +47,15 @@ PiecewiseJerkProblem::PiecewiseJerkProblem(
 
   weight_x_ref_vec_ = std::vector<double>(num_of_knots_, 0.0);
 }
-
+// 构建优化问题
 bool PiecewiseJerkProblem::FormulateProblem(OSQPData* data) {
   // calculate kernel
   std::vector<c_float> P_data;
   std::vector<c_int> P_indices;
   std::vector<c_int> P_indptr;
   CalculateKernel(&P_data, &P_indices, &P_indptr);
 
-  // calculate affine constraints
+  // calculate affine constraints 线性约束
   std::vector<c_float> A_data;
   std::vector<c_int> A_indices;
   std::vector<c_int> A_indptr;
@@ -64,6 +64,7 @@ bool PiecewiseJerkProblem::FormulateProblem(OSQPData* data) {
   CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
                             &upper_bounds);
 
+    // offset补偿，用于尽量拟合参考线
   // calculate offset
   std::vector<c_float> q;
   CalculateOffset(&q);
@@ -93,6 +94,7 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) {
     FreeData(data);
     return false;
   }
+  // OSQP设置
   OSQPSettings* settings = SolverDefaultSettings();
   settings->max_iter = max_iter;
   OSQPWorkspace* osqp_work = nullptr;
@@ -150,7 +152,8 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
       num_of_variables);
 
   int constraint_index = 0;
-  // set x, x', x'' bounds
+  // 变量约束，设置上下边界
+  //  set x, x', x'' bounds
   for (int i = 0; i < num_of_variables; ++i) {
     if (i < n) {
       variables[i].emplace_back(constraint_index, 1.0);
@@ -176,6 +179,8 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
   }
   CHECK_EQ(constraint_index, num_of_variables);
 
+  // 三个运动学约束
+
   // x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s
   for (int i = 0; i + 1 < n; ++i) {
     variables[2 * n + i].emplace_back(constraint_index, -1.0);
@@ -221,7 +226,7 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
     upper_bounds->at(constraint_index) = 0.0;
     ++constraint_index;
   }
-
+  // 初始状态约束 最终轨迹的初始状态要与当前车辆状态保持一致。
   // constrain on x_init
   variables[0].emplace_back(constraint_index, 1.0);
   lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];