add comment for creat reoutesegment

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_; }
 

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) {

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;
 

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_;

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;

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,24 +359,28 @@ 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;
   }
 
   // 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,

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;
 };

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,17 +267,17 @@ 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));
     const double start_time = Clock::NowInSeconds();
     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,27 +657,29 @@ 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()),
                 &sl)) {
           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;
   }