From afc009becf5d88d3721459596267fd1fa06c9a70 Mon Sep 17 00:00:00 2001
From: xuboluo <xubolun1@lixiang.com>
Date: Thu, 30 May 2024 17:56:05 +0800
Subject: [PATCH 1/5] add comment for creat reoutesegment

---
 modules/common/math/vec2d.cc                  |  4 +-
 modules/map/hdmap/hdmap_common.cc             |  2 +
 modules/map/pnc_map/path.cc                   | 12 ++++--
 modules/map/pnc_map/path.h                    |  3 ++
 modules/map/pnc_map/pnc_map_base.cc           |  3 +-
 .../reference_line/reference_line.h           |  4 +-
 .../reference_line/reference_line_provider.cc | 34 +++++++++++------
 .../lane_follow_map/lane_follow_map.cc        | 37 ++++++++++++++++---
 8 files changed, 76 insertions(+), 23 deletions(-)

diff --git a/modules/common/math/vec2d.cc b/modules/common/math/vec2d.cc
index cf50225be15..199da4d5cc1 100644
--- a/modules/common/math/vec2d.cc
+++ b/modules/common/math/vec2d.cc
@@ -29,8 +29,8 @@ namespace math {
 Vec2d Vec2d::CreateUnitVec2d(const double angle) {
   return Vec2d(std::cos(angle), std::sin(angle));
 }
-
-double Vec2d::Length() const { return std::hypot(x_, y_); }
+// 计算两点之间的距离
+double Vec2d::Length() const { return std::hypot(x_, y_); } 
 
 double Vec2d::LengthSquare() const { return x_ * x_ + y_ * y_; }
 
diff --git a/modules/map/hdmap/hdmap_common.cc b/modules/map/hdmap/hdmap_common.cc
index badcc16a352..7cfdf29afae 100644
--- a/modules/map/hdmap/hdmap_common.cc
+++ b/modules/map/hdmap/hdmap_common.cc
@@ -401,9 +401,11 @@ bool LaneInfo::GetProjection(const Vec2d &point, double *accumulate_s,
   if (segments_.empty()) {
     return false;
   }
+  // 最小距离初始化
   double min_dist = std::numeric_limits<double>::infinity();
   int seg_num = static_cast<int>(segments_.size());
   int min_index = 0;
+  // 判断 ego 的位置与哪个 segment 最近
   for (int i = 0; i < seg_num; ++i) {
     const double distance = segments_[i].DistanceSquareTo(point);
     if (distance < min_dist) {
diff --git a/modules/map/pnc_map/path.cc b/modules/map/pnc_map/path.cc
index ad0e10ed62a..48aff730194 100644
--- a/modules/map/pnc_map/path.cc
+++ b/modules/map/pnc_map/path.cc
@@ -317,11 +317,14 @@ Path::Path(const std::vector<MapPathPoint>& path_points,
 
 Path::Path(const std::vector<LaneSegment>& segments)
     : lane_segments_(segments) {
+  // 得到 LaneSegment
   for (const auto& segment : lane_segments_) {
+    // 得到 LaneSegment 上的点，将这些点放入 path_points_ 中
     const auto points = MapPathPoint::GetPointsFromLane(
         segment.lane, segment.start_s, segment.end_s);
     path_points_.insert(path_points_.end(), points.begin(), points.end());
   }
+  // 移除重复的点
   MapPathPoint::RemoveDuplicates(&path_points_);
   CHECK_GE(path_points_.size(), 2U);
   Init();
@@ -365,32 +368,35 @@ void Path::InitPoints() {
 
   accumulated_s_.clear();
   accumulated_s_.reserve(num_points_);
+  // 更新线段
   segments_.clear();
   segments_.reserve(num_points_);
+  // 单位向量
   unit_directions_.clear();
   unit_directions_.reserve(num_points_);
   double s = 0.0;
   for (int i = 0; i < num_points_; ++i) {
     accumulated_s_.push_back(s);
     Vec2d heading;
-    if (i + 1 >= num_points_) {
+    if (i + 1 >= num_points_) { // 最后一个点
       heading = path_points_[i] - path_points_[i - 1];
       heading.Normalize();
     } else {
       segments_.emplace_back(path_points_[i], path_points_[i + 1]);
       heading = path_points_[i + 1] - path_points_[i];
-      float heading_length = heading.Length();
+      float heading_length = heading.Length(); // 得到两点距离
       // TODO(All): use heading.length when all adjacent lanes are guarantee to
       // be connected.
       s += heading_length;
       // Normalize "heading".
       if (heading_length > 0.0) {
-        heading /= heading_length;
+        heading /= heading_length; // 归一化得到两个点之间的单位向量
       }
     }
     unit_directions_.push_back(heading);
   }
   length_ = s;
+  // 将点进行降采样，采样距离是 0.25m
   num_sample_points_ = static_cast<int>(length_ / kSampleDistance) + 1;
   num_segments_ = num_points_ - 1;
 
diff --git a/modules/map/pnc_map/path.h b/modules/map/pnc_map/path.h
index 14d4df3cbb4..24bb24c8f17 100644
--- a/modules/map/pnc_map/path.h
+++ b/modules/map/pnc_map/path.h
@@ -365,10 +365,13 @@ class Path {
  protected:
   int num_points_ = 0;
   int num_segments_ = 0;
+  // 路径点的信息
   std::vector<MapPathPoint> path_points_;
   std::vector<LaneSegment> lane_segments_;
+  // 车道累计的长度
   std::vector<double> lane_accumulated_s_;
   std::vector<LaneSegment> lane_segments_to_next_point_;
+  // 单位向量
   std::vector<common::math::Vec2d> unit_directions_;
   double length_ = 0.0;
   std::vector<double> accumulated_s_;
diff --git a/modules/map/pnc_map/pnc_map_base.cc b/modules/map/pnc_map/pnc_map_base.cc
index d507ec91c5a..4c5884051cb 100644
--- a/modules/map/pnc_map/pnc_map_base.cc
+++ b/modules/map/pnc_map/pnc_map_base.cc
@@ -47,8 +47,9 @@ bool PncMapBase::UpdatePlanningCommand(
 }
 
 double PncMapBase::LookForwardDistance(const double velocity) {
+  // 判断 8s 内行驶的距离
   auto forward_distance = velocity * FLAGS_look_forward_time_sec;
-
+  // 距离 > 180 ? 250 : 180
   return forward_distance > FLAGS_look_forward_short_distance
              ? FLAGS_look_forward_long_distance
              : FLAGS_look_forward_short_distance;
diff --git a/modules/planning/planning_base/reference_line/reference_line.h b/modules/planning/planning_base/reference_line/reference_line.h
index 86f5d30b622..f7091a14a63 100644
--- a/modules/planning/planning_base/reference_line/reference_line.h
+++ b/modules/planning/planning_base/reference_line/reference_line.h
@@ -259,8 +259,10 @@ class ReferenceLine {
   /**
    * This speed limit overrides the lane speed limit
    **/
-  std::vector<SpeedLimit> speed_limit_;
+  // Reference Line 中存在很多个 Lane 会有多种限速
+  std::vector<SpeedLimit> speed_limit_; 
   std::vector<ReferencePoint> reference_points_;
+  // 道路信息
   hdmap::Path map_path_;
   uint32_t priority_ = 0;
 };
diff --git a/modules/planning/planning_base/reference_line/reference_line_provider.cc b/modules/planning/planning_base/reference_line/reference_line_provider.cc
index a7019e29dbd..dd24a34a34f 100644
--- a/modules/planning/planning_base/reference_line/reference_line_provider.cc
+++ b/modules/planning/planning_base/reference_line/reference_line_provider.cc
@@ -127,9 +127,12 @@ bool ReferenceLineProvider::UpdatePlanningCommand(
              "used!";
   }
   // Update routing in pnc_map
+  // 判断 PNC 中的 Routing 是否与 Routing 模块中的相同
   std::lock_guard<std::mutex> lock(pnc_map_mutex_);
+  // 判断与上一帧的 Routing 是否一致
   if (current_pnc_map_->IsNewPlanningCommand(command)) {
     is_new_command_ = true;
+    // 更新 Routing 中的值
     if (!current_pnc_map_->UpdatePlanningCommand(command)) {
       AERROR << "Failed to update routing in pnc map: "
              << command.DebugString();
@@ -175,7 +178,7 @@ void ReferenceLineProvider::UpdateVehicleState(
   vehicle_state_ = vehicle_state;
 }
 
-bool ReferenceLineProvider::Start() {
+bool ReferenceLineProvider::Start() { // 参考线入口
   if (FLAGS_use_navigation_mode) {
     return true;
   }
@@ -185,6 +188,7 @@ bool ReferenceLineProvider::Start() {
   }
 
   if (FLAGS_enable_reference_line_provider_thread) {
+    // 参考线处理线程
     task_future_ = cyber::Async(&ReferenceLineProvider::GenerateThread, this);
   }
   return true;
@@ -263,8 +267,8 @@ void ReferenceLineProvider::UpdateReferenceLine(
     route_segments_history_.pop();
   }
 }
-
-void ReferenceLineProvider::GenerateThread() {
+// 参考线生成主入口
+void ReferenceLineProvider::GenerateThread() { 
   while (!is_stop_) {
     static constexpr int32_t kSleepTime = 50;  // milliseconds
     cyber::SleepFor(std::chrono::milliseconds(kSleepTime));
@@ -272,8 +276,8 @@ void ReferenceLineProvider::GenerateThread() {
     if (!has_planning_command_) {
       continue;
     }
-    std::list<ReferenceLine> reference_lines;
-    std::list<hdmap::RouteSegments> segments;
+    std::list<ReferenceLine> reference_lines; // 要生成的参考线
+    std::list<hdmap::RouteSegments> segments; // 根据 segments_ 生成参考线
     if (!CreateReferenceLine(&reference_lines, &segments)) {
       is_reference_line_updated_ = false;
       AERROR << "Fail to get reference line";
@@ -282,6 +286,7 @@ void ReferenceLineProvider::GenerateThread() {
     UpdateReferenceLine(reference_lines, segments);
     const double end_time = Clock::NowInSeconds();
     std::lock_guard<std::mutex> lock(reference_lines_mutex_);
+    // 参考线生成的耗时
     last_calculation_time_ = end_time - start_time;
     is_reference_line_updated_ = true;
   }
@@ -352,7 +357,9 @@ void ReferenceLineProvider::PrioritizeChangeLane(
   CHECK_NOTNULL(route_segments);
   auto iter = route_segments->begin();
   while (iter != route_segments->end()) {
+    // 如果当前 routesegment 为允许变道
     if (!iter->IsOnSegment()) {
+      // 将 routesegment 的第一个值设置为 iter
       route_segments->splice(route_segments->begin(), *route_segments, iter);
       break;
     }
@@ -613,6 +620,7 @@ bool ReferenceLineProvider::CreateRouteSegments(
     std::list<hdmap::RouteSegments> *segments) {
   {
     std::lock_guard<std::mutex> lock(pnc_map_mutex_);
+    // 根据当前自车的状态创建 routesegments
     if (!current_pnc_map_->GetRouteSegments(vehicle_state, segments)) {
       AERROR << "Failed to extract segments from routing";
       return false;
@@ -621,6 +629,7 @@ bool ReferenceLineProvider::CreateRouteSegments(
   for (auto &seg : *segments) {
     ADEBUG << seg.DebugString();
   }
+  // 判断是否是优先换道
   if (FLAGS_prioritize_change_lane) {
     PrioritizeChangeLane(segments);
   }
@@ -632,13 +641,13 @@ bool ReferenceLineProvider::CreateReferenceLine(
     std::list<hdmap::RouteSegments> *segments) {
   CHECK_NOTNULL(reference_lines);
   CHECK_NOTNULL(segments);
-
+  // 获取 vehicle_state
   common::VehicleState vehicle_state;
   {
     std::lock_guard<std::mutex> lock(vehicle_state_mutex_);
     vehicle_state = vehicle_state_;
   }
-
+  // 获取 Routing 
   planning::PlanningCommand command;
   {
     std::lock_guard<std::mutex> lock(routing_mutex_);
@@ -648,20 +657,21 @@ bool ReferenceLineProvider::CreateReferenceLine(
     AERROR << "Current pnc map is null! " << command.DebugString();
     return false;
   }
-
-  if (!CreateRouteSegments(vehicle_state, segments)) {
+  // 生成 RouteSegments
+  if (!CreateRouteSegments(vehicle_state, segments)) { 
     AERROR << "Failed to create reference line from routing";
     return false;
   }
   if (is_new_command_ || !FLAGS_enable_reference_line_stitching) {
     for (auto iter = segments->begin(); iter != segments->end();) {
       reference_lines->emplace_back();
-      if (!SmoothRouteSegment(*iter, &reference_lines->back())) {
+      if (!SmoothRouteSegment(*iter, &reference_lines->back())) { // 根据segments生成ReferenceLine 
         AERROR << "Failed to create reference line from route segments";
         reference_lines->pop_back();
         iter = segments->erase(iter);
       } else {
         common::SLPoint sl;
+        // 参考线未优化成功
         if (!reference_lines->back().XYToSL(
                 vehicle_state.heading(),
                 common::math::Vec2d(vehicle_state.x(), vehicle_state.y()),
@@ -669,6 +679,7 @@ bool ReferenceLineProvider::CreateReferenceLine(
           AWARN << "Failed to project point: {" << vehicle_state.x() << ","
                 << vehicle_state.y() << "} to stitched reference line";
         }
+        // 收缩参考线和 Segment
         Shrink(sl, &reference_lines->back(), &(*iter));
         ++iter;
       }
@@ -723,6 +734,7 @@ bool ReferenceLineProvider::ExtendReferenceLine(const VehicleState &state,
   }
   const double prev_segment_length = RouteSegments::Length(*prev_segment);
   const double remain_s = prev_segment_length - sl_point.s();
+  // 提取前向距离
   const double look_forward_required_distance =
       planning::PncMapBase::LookForwardDistance(state.linear_velocity());
   if (remain_s > look_forward_required_distance) {
@@ -1018,7 +1030,7 @@ bool ReferenceLineProvider::SmoothReferenceLine(
   std::vector<AnchorPoint> anchor_points;
   GetAnchorPoints(raw_reference_line, &anchor_points);
   smoother_->SetAnchorPoints(anchor_points);
-  if (!smoother_->Smooth(raw_reference_line, reference_line)) {
+  if (!smoother_->Smooth(raw_reference_line, reference_line)) { // 参考线平滑
     AERROR << "Failed to smooth reference line with anchor points";
     return false;
   }
diff --git a/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc b/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc
index be9cd89ffea..c6bbb520509 100644
--- a/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc
+++ b/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc
@@ -151,11 +151,13 @@ bool LaneFollowMap::IsValid(const planning::PlanningCommand &command) const {
   if (num_road == 0) {
     return false;
   }
+  // 判断 waypoint 中点的个数
   if (!routing.has_routing_request() ||
       routing.routing_request().waypoint_size() < 2) {
     AERROR << "Routing does not have request.";
     return false;
   }
+  // 判断 Routing 中的 id 和 s 值
   for (const auto &waypoint : routing.routing_request().waypoint()) {
     if (!waypoint.has_id() || !waypoint.has_s()) {
       AERROR << "Routing waypoint has no lane_id or s.";
@@ -172,6 +174,7 @@ void LaneFollowMap::UpdateRoutingRange(int adc_index) {
   range_end_ = range_start_;
   while (range_end_ < static_cast<int>(route_indices_.size())) {
     const auto &lane_id = route_indices_[range_end_].segment.lane->id().id();
+    // 如果 lane_id 不在 range_lane_id 中，将其插入
     if (range_lane_ids_.count(lane_id) != 0) {
       break;
     }
@@ -181,10 +184,12 @@ void LaneFollowMap::UpdateRoutingRange(int adc_index) {
 }
 
 bool LaneFollowMap::UpdateVehicleState(const VehicleState &vehicle_state) {
+  // 判断 Routing 是否有效
   if (!IsValid(last_command_)) {
     AERROR << "The routing is invalid when updating vehicle state.";
     return false;
   }
+  // 判断该帧与上一帧的 vehicle state 是否差距过大
   if (!adc_state_.has_x() ||
       (common::util::DistanceXY(adc_state_, vehicle_state) >
        FLAGS_replan_lateral_distance_threshold +
@@ -194,14 +199,16 @@ bool LaneFollowMap::UpdateVehicleState(const VehicleState &vehicle_state) {
     adc_route_index_ = -1;
     stop_for_destination_ = false;
   }
-
+  // 将该帧进行赋值
   adc_state_ = vehicle_state;
+  // 根据当前自车位置求离它最近的 adc_waypoint 
   if (!GetNearestPointFromRouting(vehicle_state, &adc_waypoint_)) {
     AERROR << "Failed to get waypoint from routing with point: "
            << "(" << vehicle_state.x() << ", " << vehicle_state.y() << ", "
            << vehicle_state.z() << ").";
     return false;
   }
+  // 根据 waypoint 找到其 index 
   int route_index = GetWaypointIndex(adc_waypoint_);
   if (route_index < 0 ||
       route_index >= static_cast<int>(route_indices_.size())) {
@@ -211,15 +218,17 @@ bool LaneFollowMap::UpdateVehicleState(const VehicleState &vehicle_state) {
   ADEBUG << "adc_waypoint_" << adc_waypoint_.DebugString() << "route_index"
          << route_index;
   // Track how many routing request waypoints the adc have passed.
+  // 找到下一个 waypoint
   UpdateNextRoutingWaypointIndex(route_index);
   adc_route_index_ = route_index;
+  // 根据 adc_route_index_ 更新 routingrang
   UpdateRoutingRange(adc_route_index_);
 
   if (routing_waypoint_index_.empty()) {
     AERROR << "No routing waypoint index.";
     return false;
   }
-
+  // 如果自车行驶到最后一个点，停止
   if (next_routing_waypoint_index_ == routing_waypoint_index_.size() - 1) {
     stop_for_destination_ = true;
   }
@@ -236,9 +245,9 @@ bool LaneFollowMap::UpdatePlanningCommand(
     return false;
   }
   const auto &routing = command.lane_follow_command();
-  range_lane_ids_.clear();
-  route_indices_.clear();
-  all_lane_ids_.clear();
+  range_lane_ids_.clear(); // 根据 adc_index(当前车的位置)更新车道 id 范围
+  route_indices_.clear(); // 路线索引
+  all_lane_ids_.clear(); // 收集所有地图中 routing request 获得的车道 id 
   for (int road_index = 0; road_index < routing.road_size(); ++road_index) {
     const auto &road_segment = routing.road(road_index);
     for (int passage_index = 0; passage_index < road_segment.passage_size();
@@ -261,8 +270,11 @@ bool LaneFollowMap::UpdatePlanningCommand(
 
   range_start_ = 0;
   range_end_ = 0;
+  // 通过 adc_route_index_ 判断自车在 那个 road 哪个 passage 哪个 lane 上
   adc_route_index_ = -1;
+  // 下一个 waypoint 的 routing_index
   next_routing_waypoint_index_ = 0;
+  // 初始化 routing_index 
   UpdateRoutingRange(adc_route_index_);
 
   routing_waypoint_index_.clear();
@@ -272,6 +284,7 @@ bool LaneFollowMap::UpdatePlanningCommand(
     return false;
   }
   int i = 0;
+  // 将 waypoint 中的 lane 的值和 index 保存在 routing_waypoint_index_ 中
   for (size_t j = 0; j < route_indices_.size(); ++j) {
     while (i < request_waypoints.size() &&
            hdmap::RouteSegments::WithinLaneSegment(route_indices_[j].segment,
@@ -358,11 +371,14 @@ std::vector<int> LaneFollowMap::GetNeighborPassages(
   CHECK_GE(start_passage, 0);
   CHECK_LE(start_passage, road.passage_size());
   std::vector<int> result;
+  // 根据 passage index 找到 passage
   const auto &source_passage = road.passage(start_passage);
   result.emplace_back(start_passage);
+  // 无变道行为 
   if (source_passage.change_lane_type() == routing::FORWARD) {
     return result;
   }
+  // 当前 passage 已经无法连接到另一条 routing 上
   if (source_passage.can_exit()) {  // No need to change lane
     return result;
   }
@@ -412,8 +428,10 @@ std::vector<int> LaneFollowMap::GetNeighborPassages(
 bool LaneFollowMap::GetRouteSegments(
     const VehicleState &vehicle_state,
     std::list<hdmap::RouteSegments> *const route_segments) {
+  // 参考线前向距离的选择是未来 8s 可以走过的路径算
   double look_forward_distance =
       LookForwardDistance(vehicle_state.linear_velocity());
+  // 后视 50
   double look_backward_distance = FLAGS_look_backward_distance;
   return GetRouteSegments(vehicle_state, look_backward_distance,
                           look_forward_distance, route_segments);
@@ -423,6 +441,7 @@ bool LaneFollowMap::GetRouteSegments(
     const VehicleState &vehicle_state, const double backward_length,
     const double forward_length,
     std::list<hdmap::RouteSegments> *const route_segments) {
+  // 根据自车位置找到 adc_waypoit_index
   if (!UpdateVehicleState(vehicle_state)) {
     AERROR << "Failed to update vehicle state in pnc_map.";
     return false;
@@ -434,6 +453,7 @@ bool LaneFollowMap::GetRouteSegments(
     AERROR << "Invalid vehicle state in pnc_map, update vehicle state first.";
     return false;
   }
+  // 根据 route index 获取相关 road passage lane 的信息
   const auto &route_index = route_indices_[adc_route_index_].index;
   const int road_index = route_index[0];
   const int passage_index = route_index[1];
@@ -498,9 +518,11 @@ bool LaneFollowMap::GetNearestPointFromRouting(
     const common::VehicleState &state, hdmap::LaneWaypoint *waypoint) const {
   waypoint->lane = nullptr;
   std::vector<hdmap::LaneInfoConstPtr> lanes;
+  // 转换成 ENU 格式
   const auto point = PointFactory::ToPointENU(state);
   std::vector<hdmap::LaneInfoConstPtr> valid_lanes;
   for (auto lane_id : all_lane_ids_) {
+    // 判断该 lane_id 是否在高精地图中可以找到
     hdmap::Id id = hdmap::MakeMapId(lane_id);
     auto lane = hdmap_->GetLaneById(id);
     if (nullptr != lane) {
@@ -511,6 +533,7 @@ bool LaneFollowMap::GetNearestPointFromRouting(
   // Get nearest_waypoints for current position
   std::vector<hdmap::LaneWaypoint> valid_way_points;
   for (const auto &lane : valid_lanes) {
+    // lane 不在 range_lane_ids_ 中
     if (range_lane_ids_.count(lane->id().id()) == 0) {
       ADEBUG << "not in range" << lane->id().id();
       continue;
@@ -518,11 +541,13 @@ bool LaneFollowMap::GetNearestPointFromRouting(
     double s = 0.0;
     double l = 0.0;
     {
+      // 根据当前车辆位置计算车辆在 lane 上的 s 和 l 值
       if (!lane->GetProjection({point.x(), point.y()}, &s, &l)) {
         continue;
       }
       ADEBUG << lane->id().id() << "," << s << "," << l;
       // Use large epsilon to allow projection diff
+      // 计算的 s 比车道还长
       static constexpr double kEpsilon = 0.5;
       if (s > (lane->total_length() + kEpsilon) || (s + kEpsilon) < 0.0) {
         continue;
@@ -547,6 +572,7 @@ bool LaneFollowMap::GetNearestPointFromRouting(
   }
 
   // find closest lane that satisfy vehicle heading
+  // 如果超过一个 waypoint ，选择正确航向角的车道线
   int closest_index = -1;
   double distance = std::numeric_limits<double>::max();
   double lane_heading = 0.0;
@@ -557,6 +583,7 @@ bool LaneFollowMap::GetNearestPointFromRouting(
         M_PI_2 * 1.5) {
       continue;
     }
+    // 判断距离是否是最近的
     if (std::fabs(valid_way_points[i].l) < distance) {
       distance = std::fabs(valid_way_points[i].l);
       closest_index = i;

From 2e8a865e383ffc73d43e4eb6af96da59b286d210 Mon Sep 17 00:00:00 2001
From: xuboluo <xubolun1@lixiang.com>
Date: Fri, 31 May 2024 18:07:34 +0800
Subject: [PATCH 2/5] add comment

---
 modules/map/pnc_map/route_segments.cc               |  7 ++++++-
 .../pnc_map/lane_follow_map/lane_follow_map.cc      | 13 +++++++++++--
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/modules/map/pnc_map/route_segments.cc b/modules/map/pnc_map/route_segments.cc
index 9ff0dc63236..b7d865977d8 100644
--- a/modules/map/pnc_map/route_segments.cc
+++ b/modules/map/pnc_map/route_segments.cc
@@ -129,7 +129,7 @@ LaneWaypoint RouteSegments::FirstWaypoint() const {
 LaneWaypoint RouteSegments::LastWaypoint() const {
   return LaneWaypoint(back().lane, back().end_s, 0.0);
 }
-
+// 给 segment 设置属性
 void RouteSegments::SetProperties(const RouteSegments &other) {
   route_end_waypoint_ = other.RouteEndWaypoint();
   can_exit_ = other.CanExit();
@@ -359,6 +359,7 @@ bool RouteSegments::CanDriveFrom(const LaneWaypoint &waypoint) const {
   auto point = waypoint.lane->GetSmoothPoint(waypoint.s);
 
   // 0 if waypoint is on segment, ok
+  // 判断 waypoint 是否在 RoutSegment 上
   if (IsWaypointOnSegment(waypoint)) {
     return true;
   }
@@ -366,17 +367,20 @@ bool RouteSegments::CanDriveFrom(const LaneWaypoint &waypoint) const {
   // 1. should have valid projection.
   LaneWaypoint segment_waypoint;
   common::SLPoint route_sl;
+  // adc_waypoint 与 segment 进行投影，得到 sl 值和在 segment 上的 way_point
   bool has_projection = GetProjection(point, &route_sl, &segment_waypoint);
   if (!has_projection) {
     AERROR << "No projection from waypoint: " << waypoint.DebugString();
     return false;
   }
+  // 横向距离 >20 
   static constexpr double kMaxLaneWidth = 10.0;
   if (std::fabs(route_sl.l()) > 2 * kMaxLaneWidth) {
     return false;
   }
 
   // 2. heading should be the same.
+  // 判断 heading 
   double waypoint_heading = waypoint.lane->Heading(waypoint.s);
   double segment_heading = segment_waypoint.lane->Heading(segment_waypoint.s);
   double heading_diff =
@@ -387,6 +391,7 @@ bool RouteSegments::CanDriveFrom(const LaneWaypoint &waypoint) const {
   }
 
   // 3. the waypoint and the projected lane should not be separated apart.
+  // 判断相邻车道是否是跨车道
   double waypoint_left_width = 0.0;
   double waypoint_right_width = 0.0;
   waypoint.lane->GetWidth(waypoint.s, &waypoint_left_width,
diff --git a/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc b/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc
index c6bbb520509..e05a9b35fed 100644
--- a/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc
+++ b/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc
@@ -366,7 +366,7 @@ bool LaneFollowMap::PassageToSegments(routing::Passage passage,
   return !segments->empty();
 }
 
-std::vector<int> LaneFollowMap::GetNeighborPassages(
+std::vector<int> LaneFollowMap:: GetNeighborPassages(
     const routing::RoadSegment &road, int start_passage) const {
   CHECK_GE(start_passage, 0);
   CHECK_LE(start_passage, road.passage_size());
@@ -383,10 +383,12 @@ std::vector<int> LaneFollowMap::GetNeighborPassages(
     return result;
   }
   hdmap::RouteSegments source_segments;
+  // 提取 passage 到 segment 
   if (!PassageToSegments(source_passage, &source_segments)) {
     AERROR << "Failed to convert passage to segments";
     return result;
   }
+  // 下一个查找到的 routing waypoint 在当前车辆所在的 passage 中，则不需要变道，直接返回
   if (next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
       source_segments.IsWaypointOnSegment(
           routing_waypoint_index_[next_routing_waypoint_index_].waypoint)) {
@@ -394,6 +396,7 @@ std::vector<int> LaneFollowMap::GetNeighborPassages(
            << "] before change lane";
     return result;
   }
+  // 可以变道，将变道结果保存
   std::unordered_set<std::string> neighbor_lanes;
   if (source_passage.change_lane_type() == routing::LEFT) {
     for (const auto &segment : source_segments) {
@@ -410,7 +413,7 @@ std::vector<int> LaneFollowMap::GetNeighborPassages(
       }
     }
   }
-
+  // 根据 neighbor_lanes 找到对应的 segment 并保存
   for (int i = 0; i < road.passage_size(); ++i) {
     if (i == start_passage) {
       continue;
@@ -459,7 +462,9 @@ bool LaneFollowMap::GetRouteSegments(
   const int passage_index = route_index[1];
   const auto &road = last_command_.lane_follow_command().road(road_index);
   // Raw filter to find all neighboring passages
+  // 找到相关临近车道，最终返回 passage index 
   auto drive_passages = GetNeighborPassages(road, passage_index);
+  // 根据 passage 找到 segment 
   for (const int index : drive_passages) {
     const auto &passage = road.passage(index);
     hdmap::RouteSegments segments;
@@ -467,6 +472,7 @@ bool LaneFollowMap::GetRouteSegments(
       ADEBUG << "Failed to convert passage to lane segments.";
       continue;
     }
+    // 
     const PointENU nearest_point =
         index == passage_index
             ? adc_waypoint_.lane->GetSmoothPoint(adc_waypoint_.s)
@@ -478,6 +484,7 @@ bool LaneFollowMap::GetRouteSegments(
              << nearest_point.ShortDebugString();
       continue;
     }
+    // 如果 passage 不是当前车道进行可驶入检查
     if (index != passage_index) {
       if (!segments.CanDriveFrom(adc_waypoint_)) {
         ADEBUG << "You cannot drive from current waypoint to passage: "
@@ -668,6 +675,7 @@ bool LaneFollowMap::ExtendSegments(
   std::unordered_set<std::string> unique_lanes;
   static constexpr double kRouteEpsilon = 1e-3;
   // Extend the trajectory towards the start of the trajectory.
+  // 查找轨迹的起点是在哪里
   if (start_s < 0) {
     const auto &first_segment = *segments.begin();
     auto lane = first_segment.lane;
@@ -725,6 +733,7 @@ bool LaneFollowMap::ExtendSegments(
     return true;
   }
   // Extend the trajectory towards the end of the trajectory.
+  // 查找轨迹的终点在哪里
   if (router_s < end_s && !truncated_segments->empty()) {
     auto &back = truncated_segments->back();
     if (back.lane->total_length() > back.end_s) {

From c2a83245e9a164a3aaeb98e8f0587870bf0419b8 Mon Sep 17 00:00:00 2001
From: xuboluo <xubolun1@lixiang.com>
Date: Tue, 25 Mar 2025 18:21:08 +0800
Subject: [PATCH 3/5] add comment

---
 .../lateral_osqp_optimizer.cc                 | 48 ++++++++++++-------
 .../lateral_qp_optimizer.cc                   |  2 +-
 .../public_road/public_road_planner.cc        |  5 +-
 .../planners/public_road/scenario_manager.cc  |  3 ++
 .../planning_component/planning_component.cc  | 11 +++--
 .../scenario_base/scenario.cc                 |  3 ++
 6 files changed, 48 insertions(+), 24 deletions(-)

diff --git a/modules/planning/planners/lattice/trajectory_generation/lateral_osqp_optimizer.cc b/modules/planning/planners/lattice/trajectory_generation/lateral_osqp_optimizer.cc
index 69884285bac..6de620a051c 100644
--- a/modules/planning/planners/lattice/trajectory_generation/lateral_osqp_optimizer.cc
+++ b/modules/planning/planners/lattice/trajectory_generation/lateral_osqp_optimizer.cc
@@ -22,17 +22,20 @@
 
 namespace apollo {
 namespace planning {
-
+// d_state: 初始 d，d_bounds：这个 reference 上的 d_bounds ，大概有 70 个点
 bool LateralOSQPOptimizer::optimize(
     const std::array<double, 3>& d_state, const double delta_s,
     const std::vector<std::pair<double, double>>& d_bounds) {
+  // 按照列来存储非零元素的值
   std::vector<c_float> P_data;
+  // 存储非零元素的行索引
   std::vector<c_int> P_indices;
   std::vector<c_int> P_indptr;
   CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
   delta_s_ = delta_s;
   const int num_var = static_cast<int>(d_bounds.size());
   const int kNumParam = 3 * static_cast<int>(d_bounds.size());
+  // 得到约束中 6n 的维度
   const int kNumConstraint = kNumParam + 3 * (num_var - 1) + 3;
   c_float lower_bounds[kNumConstraint];
   c_float upper_bounds[kNumConstraint];
@@ -41,15 +44,15 @@ bool LateralOSQPOptimizer::optimize(
   const int pprime_offset = 2 * num_var;
 
   std::vector<std::vector<std::pair<c_int, c_float>>> columns;
-  columns.resize(kNumParam);
-
+  columns.resize(kNumParam);  // 3n
+  // colum 的系数，constraint_index 的系数控制是第几个约束
   int constraint_index = 0;
 
-  // d_i+1'' - d_i''
+  // d_i+1'' - d_i'' 第 4 条约束
   for (int i = 0; i + 1 < num_var; ++i) {
     columns[pprime_offset + i].emplace_back(constraint_index, -1.0);
     columns[pprime_offset + i + 1].emplace_back(constraint_index, 1.0);
-
+    // l 和 u 的不等式约束
     lower_bounds[constraint_index] =
         -FLAGS_lateral_third_order_derivative_max * delta_s_;
     upper_bounds[constraint_index] =
@@ -57,23 +60,24 @@ bool LateralOSQPOptimizer::optimize(
     ++constraint_index;
   }
 
-  // d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')
+  // d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')--->第 5 条约束
   for (int i = 0; i + 1 < num_var; ++i) {
     columns[prime_offset + i].emplace_back(constraint_index, -1.0);
     columns[prime_offset + i + 1].emplace_back(constraint_index, 1.0);
     columns[pprime_offset + i].emplace_back(constraint_index, -0.5 * delta_s_);
     columns[pprime_offset + i + 1].emplace_back(constraint_index,
                                                 -0.5 * delta_s_);
-
+    // 没有对应的 lu 约束
     lower_bounds[constraint_index] = 0.0;
     upper_bounds[constraint_index] = 0.0;
     ++constraint_index;
   }
 
-  // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2
+  // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2 ---> 第
+  // 6 条约束
   for (int i = 0; i + 1 < num_var; ++i) {
-    columns[i].emplace_back(constraint_index, -1.0);
-    columns[i + 1].emplace_back(constraint_index, 1.0);
+    columns[i].emplace_back(constraint_index, -1.0);     // l
+    columns[i + 1].emplace_back(constraint_index, 1.0);  // l'
     columns[prime_offset + i].emplace_back(constraint_index, -delta_s_);
     columns[pprime_offset + i].emplace_back(constraint_index,
                                             -delta_s_ * delta_s_ / 3.0);
@@ -84,22 +88,23 @@ bool LateralOSQPOptimizer::optimize(
     upper_bounds[constraint_index] = 0.0;
     ++constraint_index;
   }
-
+  // 第 7 条约束
   columns[0].emplace_back(constraint_index, 1.0);
   lower_bounds[constraint_index] = d_state[0];
   upper_bounds[constraint_index] = d_state[0];
   ++constraint_index;
-
+  // 第 7 条约束
   columns[prime_offset].emplace_back(constraint_index, 1.0);
   lower_bounds[constraint_index] = d_state[1];
   upper_bounds[constraint_index] = d_state[1];
   ++constraint_index;
-
+  // 第 7 条约束
   columns[pprime_offset].emplace_back(constraint_index, 1.0);
   lower_bounds[constraint_index] = d_state[2];
   upper_bounds[constraint_index] = d_state[2];
   ++constraint_index;
 
+  // 第 123 条约束
   const double LARGE_VALUE = 2.0;
   for (int i = 0; i < kNumParam; ++i) {
     columns[i].emplace_back(constraint_index, 1.0);
@@ -116,6 +121,7 @@ bool LateralOSQPOptimizer::optimize(
   CHECK_EQ(constraint_index, kNumConstraint);
 
   // change affine_constraint to CSC format
+  // 转换为系数矩阵
   std::vector<c_float> A_data;
   std::vector<c_int> A_indices;
   std::vector<c_int> A_indptr;
@@ -131,9 +137,12 @@ bool LateralOSQPOptimizer::optimize(
   A_indptr.push_back(ind_p);
 
   // offset
+  // q 矩阵
   double q[kNumParam];
   for (int i = 0; i < kNumParam; ++i) {
     if (i < num_var) {
+      // q[i] = -optimizer_config_.get_weight_lateral_bias() * target_bias_; 将
+      // target_offset 看作是 reference_line
       q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
              (d_bounds[i].first + d_bounds[i].second);
     } else {
@@ -141,7 +150,7 @@ bool LateralOSQPOptimizer::optimize(
     }
   }
 
-  // Problem settings
+  // Problem settings 初始化求解器
   OSQPSettings* settings =
       reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
 
@@ -155,6 +164,7 @@ bool LateralOSQPOptimizer::optimize(
   settings->verbose = FLAGS_enable_osqp_debug;
 
   // Populate data
+  // OSQP 初始化
   OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
   data->n = kNumParam;
   data->m = kNumConstraint;
@@ -175,6 +185,7 @@ bool LateralOSQPOptimizer::optimize(
   osqp_solve(work);
 
   // extract primal results
+  // 得到优化后的 l,l',l''
   for (int i = 0; i < num_var; ++i) {
     opt_d_.push_back(work->solution->x[i]);
     opt_d_prime_.push_back(work->solution->x[i + num_var]);
@@ -197,11 +208,12 @@ void LateralOSQPOptimizer::CalculateKernel(
     const std::vector<std::pair<double, double>>& d_bounds,
     std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
     std::vector<c_int>* P_indptr) {
-  const int kNumParam = 3 * static_cast<int>(d_bounds.size());
-  P_data->resize(kNumParam);
-  P_indices->resize(kNumParam);
+  // [l0,l1,...,ln-1,l'0,l'1,...,l'n-1,l''0,...,l''n-1]
+  const int kNumParam = 3 * static_cast<int>(d_bounds.size());  // 3n 的维度
+  P_data->resize(kNumParam);                                    // 3n
+  P_indices->resize(kNumParam);                                 // 3n
   P_indptr->resize(kNumParam + 1);
-
+  // 构建 P 矩阵
   for (int i = 0; i < kNumParam; ++i) {
     if (i < static_cast<int>(d_bounds.size())) {
       P_data->at(i) = 2.0 * FLAGS_weight_lateral_offset +
diff --git a/modules/planning/planners/lattice/trajectory_generation/lateral_qp_optimizer.cc b/modules/planning/planners/lattice/trajectory_generation/lateral_qp_optimizer.cc
index 03d3b97d818..1ff9e3d33cb 100644
--- a/modules/planning/planners/lattice/trajectory_generation/lateral_qp_optimizer.cc
+++ b/modules/planning/planners/lattice/trajectory_generation/lateral_qp_optimizer.cc
@@ -20,7 +20,7 @@
 
 namespace apollo {
 namespace planning {
-
+// 对 osqp 结果赋值
 PiecewiseJerkTrajectory1d LateralQPOptimizer::GetOptimalTrajectory() const {
   ACHECK(!opt_d_.empty() && !opt_d_prime_.empty() && !opt_d_pprime_.empty());
 
diff --git a/modules/planning/planners/public_road/public_road_planner.cc b/modules/planning/planners/public_road/public_road_planner.cc
index f8dfeef3555..984978b1bdf 100644
--- a/modules/planning/planners/public_road/public_road_planner.cc
+++ b/modules/planning/planners/public_road/public_road_planner.cc
@@ -37,12 +37,15 @@ Status PublicRoadPlanner::Init(
 Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                                Frame* frame,
                                ADCTrajectory* ptr_computed_trajectory) {
+  // 更新场景
   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) {
@@ -53,7 +56,7 @@ Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
     scenario_debug->set_stage_plugin_type(scenario_->GetStage());
     scenario_debug->set_msg(scenario_->GetMsg());
   }
-
+  // 更新执行的状态
   if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_DONE) {
     // only updates scenario manager when previous scenario's status is
     // STATUS_DONE
diff --git a/modules/planning/planners/public_road/scenario_manager.cc b/modules/planning/planners/public_road/scenario_manager.cc
index e80bab8235e..f0e92bf2b71 100644
--- a/modules/planning/planners/public_road/scenario_manager.cc
+++ b/modules/planning/planners/public_road/scenario_manager.cc
@@ -36,6 +36,7 @@ bool ScenarioManager::Init(const std::shared_ptr<DependencyInjector>& injector,
     return true;
   }
   injector_ = injector;
+  // 从配置⽂件中读取⽀持的场景列表，并创建相应的场景
   for (int i = 0; i < planner_config.scenario_size(); i++) {
     auto scenario = PluginManager::Instance()->CreateInstance<Scenario>(
         ConfigUtil::GetFullPlanningClassName(
@@ -57,12 +58,14 @@ void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                              Frame* frame) {
   CHECK_NOTNULL(frame);
   for (auto scenario : scenario_list_) {
+    // 如果是当前场景并且状态为正在规划中
     if (current_scenario_.get() == scenario.get() &&
         current_scenario_->GetStatus() ==
             ScenarioStatusType::STATUS_PROCESSING) {
       // The previous scenario has higher priority
       return;
     }
+    // IsTransferable 判断是否可以场景切换
     if (scenario->IsTransferable(current_scenario_.get(), *frame)) {
       current_scenario_->Exit(frame);
       AINFO << "switch scenario from" << current_scenario_->Name() << " to "
diff --git a/modules/planning/planning_component/planning_component.cc b/modules/planning/planning_component/planning_component.cc
index ad7850c3d16..7699f41f3da 100644
--- a/modules/planning/planning_component/planning_component.cc
+++ b/modules/planning/planning_component/planning_component.cc
@@ -59,7 +59,7 @@ bool PlanningComponent::Init() {
   }
 
   planning_base_->Init(config_);
-
+  // 初始化流程指令信息 Reader
   planning_command_reader_ = node_->CreateReader<PlanningCommand>(
       config_.topic_config().planning_command_topic(),
       [this](const std::shared_ptr<PlanningCommand>& planning_command) {
@@ -68,7 +68,7 @@ bool PlanningComponent::Init() {
         std::lock_guard<std::mutex> lock(mutex_);
         planning_command_.CopyFrom(*planning_command);
       });
-
+  // 初始化交通 Reader
   traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
       config_.topic_config().traffic_light_detection_topic(),
       [this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {
@@ -102,6 +102,7 @@ bool PlanningComponent::Init() {
           relative_map_.CopyFrom(*map_message);
         });
   }
+  // 规划轨迹的 Writer
   planning_writer_ = node_->CreateWriter<ADCTrajectory>(
       config_.topic_config().planning_trajectory_topic());
 
@@ -115,14 +116,14 @@ bool PlanningComponent::Init() {
       FLAGS_planning_command_status);
   return true;
 }
-
+// planning 的处理函数
 bool PlanningComponent::Proc(
     const std::shared_ptr<prediction::PredictionObstacles>&
         prediction_obstacles,
     const std::shared_ptr<canbus::Chassis>& chassis,
     const std::shared_ptr<localization::LocalizationEstimate>&
         localization_estimate) {
-  ACHECK(prediction_obstacles != nullptr);
+  ACHECK(prediction_obstacles != nullptr);  // 判断输入是否为空
 
   // check and process possible rerouting request
   CheckRerouting();
@@ -200,6 +201,7 @@ bool PlanningComponent::Proc(
   }
 
   ADCTrajectory adc_trajectory_pb;
+  // 规划执行的函数
   planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
   auto start_time = adc_trajectory_pb.header().timestamp_sec();
   common::util::FillHeader(node_->Name(), &adc_trajectory_pb);
@@ -209,6 +211,7 @@ bool PlanningComponent::Proc(
   for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
     p.set_relative_time(p.relative_time() + dt);
   }
+  // 输出结果
   planning_writer_->Write(adc_trajectory_pb);
 
   // Send command execution feedback.
diff --git a/modules/planning/planning_interface_base/scenario_base/scenario.cc b/modules/planning/planning_interface_base/scenario_base/scenario.cc
index 29ee79fd215..7c909d77309 100644
--- a/modules/planning/planning_interface_base/scenario_base/scenario.cc
+++ b/modules/planning/planning_interface_base/scenario_base/scenario.cc
@@ -100,8 +100,10 @@ ScenarioResult Scenario::Process(
     scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_DONE);
     return scenario_result_;
   }
+  // 先执行当前 Stage
   auto ret = current_stage_->Process(planning_init_point, frame);
   scenario_result_.SetStageResult(ret);
+  // 得到当前 Stage 的状态机
   switch (ret.GetStageStatus()) {
     case StageStatusType::ERROR: {
       AERROR << "Stage '" << current_stage_->Name() << "' returns error";
@@ -127,6 +129,7 @@ ScenarioResult Scenario::Process(
               ScenarioStatusType::STATUS_UNKNOWN);
           return scenario_result_;
         }
+        // 创建下一个 Stage
         current_stage_ = CreateStage(*stage_pipeline_map_[next_stage]);
         if (current_stage_ == nullptr) {
           AWARN << "Current stage is a null pointer.";

From 40f620238cc8eab473b345722f4a33853f21ab61 Mon Sep 17 00:00:00 2001
From: xuboluo <xubolun1@lixiang.com>
Date: Wed, 26 Mar 2025 18:23:02 +0800
Subject: [PATCH 4/5] add comment for obstacle

---
 .../lattice/behavior/path_time_graph.cc       | 20 ++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/modules/planning/planners/lattice/behavior/path_time_graph.cc b/modules/planning/planners/lattice/behavior/path_time_graph.cc
index 067a8789bf3..8a84294a037 100644
--- a/modules/planning/planners/lattice/behavior/path_time_graph.cc
+++ b/modules/planning/planners/lattice/behavior/path_time_graph.cc
@@ -107,7 +107,7 @@ void PathTimeGraph::SetupObstacles(
     path_time_obstacles_.push_back(path_time_obstacle.second);
   }
 }
-
+// 得到 obstacle 的 boundry
 void PathTimeGraph::SetStaticObstacle(
     const Obstacle* obstacle,
     const std::vector<PathPoint>& discretized_ref_points) {
@@ -363,32 +363,42 @@ void PathTimeGraph::UpdateLateralBoundsByObstacle(
   if (sl_boundary.start_s() > s_end || sl_boundary.end_s() < s_start) {
     return;
   }
-  auto start_iter = std::lower_bound(
-      discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
-  auto end_iter = std::upper_bound(
-      discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
+  // 查找障碍物 start_s 所在的 iter
+  auto start_iter =
+      std::lower_bound(discretized_path.begin(), discretized_path.end(),
+                       sl_boundary.start_s());  // 大于等于
+  auto end_iter =
+      std::upper_bound(discretized_path.begin(), discretized_path.end(),
+                       sl_boundary.start_s());  // 大于
+  // 得到 index
   size_t start_index = start_iter - discretized_path.begin();
   size_t end_index = end_iter - discretized_path.begin();
+  // 目标物是压着车道线走的
   if (sl_boundary.end_l() > -FLAGS_numerical_epsilon &&
       sl_boundary.start_l() < FLAGS_numerical_epsilon) {
+    // 重新赋值为压着车道线走
     for (size_t i = start_index; i < end_index; ++i) {
       bounds->operator[](i).first = -FLAGS_numerical_epsilon;
       bounds->operator[](i).second = FLAGS_numerical_epsilon;
     }
     return;
   }
+  // 障碍物完全在中心线右边
   if (sl_boundary.end_l() < FLAGS_numerical_epsilon) {
     for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
          ++i) {
+      // 将右边界往左移动。更新右边界：障碍物左侧
       bounds->operator[](i).first =
           std::max(bounds->operator[](i).first,
                    sl_boundary.end_l() + FLAGS_nudge_buffer);
     }
     return;
   }
+  // 目标物完全在左边
   if (sl_boundary.start_l() > -FLAGS_numerical_epsilon) {
     for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
          ++i) {
+      // 更新左边界： 障碍物的右侧
       bounds->operator[](i).second =
           std::min(bounds->operator[](i).second,
                    sl_boundary.start_l() - FLAGS_nudge_buffer);

From 76bfc86ffb1deb3794009ed5f9127946fe27a1d8 Mon Sep 17 00:00:00 2001
From: xuboluo <xubolun1@lixiang.com>
Date: Mon, 31 Mar 2025 18:27:48 +0800
Subject: [PATCH 5/5] add comment for boundary projection st

---
 .../lattice/behavior/path_time_graph.cc       | 20 ++++++++++++++-----
 .../trajectory1d_generator.cc                 | 10 +++++++---
 .../trajectory1d_generator.h                  | 11 +++++-----
 3 files changed, 28 insertions(+), 13 deletions(-)

diff --git a/modules/planning/planners/lattice/behavior/path_time_graph.cc b/modules/planning/planners/lattice/behavior/path_time_graph.cc
index 8a84294a037..97ffad0aca5 100644
--- a/modules/planning/planners/lattice/behavior/path_time_graph.cc
+++ b/modules/planning/planners/lattice/behavior/path_time_graph.cc
@@ -57,7 +57,7 @@ PathTimeGraph::PathTimeGraph(
 
   SetupObstacles(obstacles, discretized_ref_points);
 }
-
+// 将目标物映射到 Frenet 坐标系
 SLBoundary PathTimeGraph::ComputeObstacleBoundary(
     const std::vector<common::math::Vec2d>& vertices,
     const std::vector<PathPoint>& discretized_ref_points) const {
@@ -97,7 +97,7 @@ void PathTimeGraph::SetupObstacles(
       SetDynamicObstacle(obstacle, discretized_ref_points);
     }
   }
-
+  // 由近及远进行排序
   std::sort(static_obs_sl_boundaries_.begin(), static_obs_sl_boundaries_.end(),
             [](const SLBoundary& sl0, const SLBoundary& sl1) {
               return sl0.start_s() < sl1.start_s();
@@ -121,6 +121,7 @@ void PathTimeGraph::SetStaticObstacle(
   double right_width = FLAGS_default_reference_line_width * 0.5;
   ptr_reference_line_info_->reference_line().GetLaneWidth(
       sl_boundary.start_s(), &left_width, &right_width);
+  // s 方向超出参考线
   if (sl_boundary.start_s() > path_range_.second ||
       sl_boundary.end_s() < path_range_.first ||
       sl_boundary.start_l() > left_width ||
@@ -128,7 +129,7 @@ void PathTimeGraph::SetStaticObstacle(
     ADEBUG << "Obstacle [" << obstacle_id << "] is out of range.";
     return;
   }
-
+  // 得到 st 图初始化
   path_time_obstacle_map_[obstacle_id].set_id(obstacle_id);
   path_time_obstacle_map_[obstacle_id].set_bottom_left_point(
       SetPathTimePoint(obstacle_id, sl_boundary.start_s(), 0.0));
@@ -151,7 +152,9 @@ void PathTimeGraph::SetDynamicObstacle(
     const std::vector<PathPoint>& discretized_ref_points) {
   double relative_time = time_range_.first;
   while (relative_time < time_range_.second) {
+    // 得到障碍物某一时刻的位置
     TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
+    // 将点膨胀成 box
     Box2d box = obstacle->GetBoundingBox(point);
     SLBoundary sl_boundary =
         ComputeObstacleBoundary(box.GetAllCorners(), discretized_ref_points);
@@ -162,10 +165,12 @@ void PathTimeGraph::SetDynamicObstacle(
         sl_boundary.start_s(), &left_width, &right_width);
 
     // The obstacle is not shown on the region to be considered.
+    // 剔除较远目标
     if (sl_boundary.start_s() > path_range_.second ||
         sl_boundary.end_s() < path_range_.first ||
         sl_boundary.start_l() > left_width ||
         sl_boundary.end_l() < -right_width) {
+      // 这个障碍物从没有在 path_time_obstacle_map 中出现过
       if (path_time_obstacle_map_.find(obstacle->Id()) !=
           path_time_obstacle_map_.end()) {
         break;
@@ -173,7 +178,7 @@ void PathTimeGraph::SetDynamicObstacle(
       relative_time += FLAGS_trajectory_time_resolution;
       continue;
     }
-
+    // ST 图
     if (path_time_obstacle_map_.find(obstacle->Id()) ==
         path_time_obstacle_map_.end()) {
       path_time_obstacle_map_[obstacle->Id()].set_id(obstacle->Id());
@@ -310,13 +315,14 @@ bool PathTimeGraph::IsObstacleInGraph(const std::string& obstacle_id) {
   return path_time_obstacle_map_.find(obstacle_id) !=
          path_time_obstacle_map_.end();
 }
-
+// 初始化 boundary 和 segment
 std::vector<std::pair<double, double>> PathTimeGraph::GetLateralBounds(
     const double s_start, const double s_end, const double s_resolution) {
   CHECK_LT(s_start, s_end);
   CHECK_GT(s_resolution, FLAGS_numerical_epsilon);
   std::vector<std::pair<double, double>> bounds;
   std::vector<double> discretized_path;
+  // 得到 segmant 的总长度
   double s_range = s_end - s_start;
   double s_curr = s_start;
   size_t num_bound = static_cast<size_t>(s_range / s_resolution);
@@ -331,8 +337,11 @@ std::vector<std::pair<double, double>> PathTimeGraph::GetLateralBounds(
     double right_width = FLAGS_default_reference_line_width / 2.0;
     ptr_reference_line_info_->reference_line().GetLaneWidth(s_curr, &left_width,
                                                             &right_width);
+    // 找到自车左右边界
     double ego_d_lower = init_d_[0] - ego_width / 2.0;
     double ego_d_upper = init_d_[0] + ego_width / 2.0;
+    // xu todo
+    // 将 Boundary 定义为更靠左或者靠右的那一个
     bounds.emplace_back(
         std::min(-right_width, ego_d_lower - FLAGS_bound_buffer),
         std::max(left_width, ego_d_upper + FLAGS_bound_buffer));
@@ -373,6 +382,7 @@ void PathTimeGraph::UpdateLateralBoundsByObstacle(
   // 得到 index
   size_t start_index = start_iter - discretized_path.begin();
   size_t end_index = end_iter - discretized_path.begin();
+  // xu todo
   // 目标物是压着车道线走的
   if (sl_boundary.end_l() > -FLAGS_numerical_epsilon &&
       sl_boundary.start_l() < FLAGS_numerical_epsilon) {
diff --git a/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.cc b/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.cc
index 08624f10a04..a869e4debf3 100644
--- a/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.cc
+++ b/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.cc
@@ -53,7 +53,7 @@ void Trajectory1dGenerator::GenerateTrajectoryBundles(
     Trajectory1DBundle* ptr_lat_trajectory_bundle) {
   GenerateLongitudinalTrajectoryBundle(planning_target,
                                        ptr_lon_trajectory_bundle);
-
+  // lateral boundary
   GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle);
 }
 
@@ -119,13 +119,17 @@ void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
 
 void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
     Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
-  if (!FLAGS_lateral_optimization) {
-    auto end_conditions = end_condition_sampler_.SampleLatEndConditions();
+  if (!FLAGS_lateral_optimization) {  // 采样法
+    auto end_conditions =
+        end_condition_sampler_.SampleLatEndConditions();  // 速度采样
 
     // Use the common function to generate trajectory bundles.
+    // 生成 5 条采样的轨迹
     GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
                                   ptr_lat_trajectory_bundle);
   } else {
+    // 大致这么个意思：在一段长60m的里程中，以ds=1m进行采样，遍历每个采样点的横向可达范围，
+    // 即每个点的横向约束，通过这么多约束构建对应的二次型，最后通过调用OSQP进行二次规划求解
     double s_min = init_lon_state_[0];
     double s_max = s_min + FLAGS_max_s_lateral_optimization;
 
diff --git a/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.h b/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.h
index 814dc7ff948..871daa57bfd 100644
--- a/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.h
+++ b/modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.h
@@ -117,14 +117,15 @@ inline void Trajectory1dGenerator::GenerateTrajectory1DBundle<5>(
     std::vector<std::shared_ptr<Curve1d>>* ptr_trajectory_bundle) const {
   CHECK_NOTNULL(ptr_trajectory_bundle);
   ACHECK(!end_conditions.empty());
-
+  // 创建空间
   ptr_trajectory_bundle->reserve(ptr_trajectory_bundle->size() +
                                  end_conditions.size());
   for (const auto& end_condition : end_conditions) {
-    auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(
-        std::shared_ptr<Curve1d>(new QuinticPolynomialCurve1d(
-            init_state, end_condition.first, end_condition.second)));
-
+    auto ptr_trajectory1d =
+        std::make_shared<LatticeTrajectory1d>(std::shared_ptr<Curve1d>(
+            new QuinticPolynomialCurve1d(  // 构建五次多项式
+                init_state, end_condition.first, end_condition.second)));
+    // 赋值
     ptr_trajectory1d->set_target_position(end_condition.first[0]);
     ptr_trajectory1d->set_target_velocity(end_condition.first[1]);
     ptr_trajectory1d->set_target_time(end_condition.second);