From afc009becf5d88d3721459596267fd1fa06c9a70 Mon Sep 17 00:00:00 2001 From: xuboluo Date: Thu, 30 May 2024 17:56:05 +0800 Subject: [PATCH 1/2] 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::infinity(); int seg_num = static_cast(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& path_points, Path::Path(const std::vector& 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(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 path_points_; std::vector lane_segments_; + // 车道累计的长度 std::vector lane_accumulated_s_; std::vector lane_segments_to_next_point_; + // 单位向量 std::vector unit_directions_; double length_ = 0.0; std::vector 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 speed_limit_; + // Reference Line 中存在很多个 Lane 会有多种限速 + std::vector speed_limit_; std::vector 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 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 reference_lines; - std::list segments; + std::list reference_lines; // 要生成的参考线 + std::list 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 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 *segments) { { std::lock_guard 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 *segments) { CHECK_NOTNULL(reference_lines); CHECK_NOTNULL(segments); - + // 获取 vehicle_state common::VehicleState vehicle_state; { std::lock_guard lock(vehicle_state_mutex_); vehicle_state = vehicle_state_; } - + // 获取 Routing planning::PlanningCommand command; { std::lock_guard 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 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(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(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 LaneFollowMap::GetNeighborPassages( CHECK_GE(start_passage, 0); CHECK_LE(start_passage, road.passage_size()); std::vector 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 LaneFollowMap::GetNeighborPassages( bool LaneFollowMap::GetRouteSegments( const VehicleState &vehicle_state, std::list *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 *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 lanes; + // 转换成 ENU 格式 const auto point = PointFactory::ToPointENU(state); std::vector 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 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::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 Date: Fri, 31 May 2024 18:07:34 +0800 Subject: [PATCH 2/2] 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 LaneFollowMap::GetNeighborPassages( +std::vector 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 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 LaneFollowMap::GetNeighborPassages( << "] before change lane"; return result; } + // 可以变道，将变道结果保存 std::unordered_set neighbor_lanes; if (source_passage.change_lane_type() == routing::LEFT) { for (const auto &segment : source_segments) { @@ -410,7 +413,7 @@ std::vector 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 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) {