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/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();
@@ -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) {
@@ -121,14 +121,15 @@ 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 ||
       sl_boundary.end_l() < -right_width) {
     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,18 +165,20 @@ 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;
       }
       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));
@@ -363,32 +372,43 @@ 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();
+  // xu todo
+  // 目标物是压着车道线走的
   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);