百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\ReferenceLineInfo类代码详解
Apollo planning 模块ReferenceLineInfo类代码详解
·
概述
ReferenceLineInfo类是apollo planning模块下modules\planning\common\reference_line_info.cc/.h实现
从类名来看，应该是参考线信息类。
从代码来看ReferenceLineInfo类主要是实现：
1.用自车状态，参考线信息，规划起始点，routing的结果去构造参考线信息类对象,同时也会储存这些信息到ReferenceLineInfo类对象中；
2.类成员path_decision_里存放障碍物对象列表以及对障碍物的横纵向决策信息；
3.判断是否到达终点以及距终点的距离，终点通常设置为一障碍物对象，其id为"DEST"；
4.设置及存储离散轨迹；
5.设置及存储参考线的cost？是什么cost?类成员cost_；
6.设置/返回参考线的巡航速度，停止点等；
7.获取相邻车道，左前/左后/右前/右后；
8.根据给定的s,找到参考线上给定s对应的车道；
9.判断当前参考线的起点位于上一次参考线上？
10.结合速度规划和路径规划，共同结合生成一条离散轨迹ptr_discretized_trajectory
11.将规划轨迹调整到从当前位置开始，裁剪掉轨迹上规划起始点之前的轨迹；
12.返回自车的SL边界，包含自车边界start_s,end_s,start_l,end_l信息；
13.存储及设置参考线是否可驾驶？
14.检查当前参考线是否是一个变道参考线，ADC当前位置就不在这条参考线上说明是？
15.检查当前的参考线是否是自车当前位置相邻参考线；
16.导出参与建议？engage_advice是planning发布轨迹ADCTrajectory中包含的信息，推测是判断规划是否应该介入？
17.导出决策信息到decision_result，planning_context中？这两都是待发布轨迹的成员？
18.设置/获取自车在指定纵坐标s对应路口的路权？是否有保护？
19.获取路径的转弯类型，指定纵向位置s处的转弯类型
20.获取指定纵向位置S处的PNC路口的overlap
21.设置车辆的信号灯信息
22.判断输入的障碍物是否可被忽略
23.储存阻塞参考线的障碍物id
感觉功能较为杂乱，也不是很好总结，只好罗列于此，基本上都是操作储存跟参考线相关的信息，譬如：障碍物，决策相关类对象，车道，各种场景元素与参考线重叠overlap等信息？
reference_line_info.h
#pragma once
#include <limits>
#include <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#include "modules/common/proto/drive_state.pb.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/map/pnc_map/pnc_map.h"
#include "modules/planning/common/path/path_data.h"
#include "modules/planning/common/path_boundary.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/common/st_graph_data.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/proto/lattice_structure.pb.h"
#include "modules/planning/proto/planning.pb.h"
namespace apollo {
namespace planning {
/**
* @class ReferenceLineInfo
* @brief ReferenceLineInfo holds all data for one reference line.
*/
class ReferenceLineInfo {
public:
//左前/左后/右前/右后 车道类型，指的是相对于自车车道的邻居车道？
enum class LaneType { LeftForward, LeftReverse, RightForward, RightReverse };
//默认构造函数
ReferenceLineInfo() = default;
//ReferenceLineInfo带参构造函数
//参数：vehicle_state自车状态
//参数：adc_planning_point自车规划起始点
//参数：reference_line参考线
//参数：segments是继承车道段的vector
//这个带参构造函数就是将输入参数拿去初始化自己的类成员vehicle_state_，adc_planning_point_，reference_line_，lanes_
ReferenceLineInfo(const common::VehicleState& vehicle_state,
const common::TrajectoryPoint& adc_planning_point,
const ReferenceLine& reference_line,
const hdmap::RouteSegments& segments);
//初始化函数，输入参数是障碍物类vector，也就是障碍物列表
bool Init(const std::vector<const Obstacle*>& obstacles);
//直接把输入参数障碍物列表加到类成员path_decision_里
bool AddObstacles(const std::vector<const Obstacle*>& obstacles);
// 在类成员路径决策path_decision_里增加一个障碍物对象
Obstacle* AddObstacle(const Obstacle* obstacle);
//返回类成员，车辆状态vehicle_state_
const common::VehicleState& vehicle_state() const { return vehicle_state_; }
//返回类成员路径决策类对象地址path_decision_，可修改
PathDecision* path_decision();
//返回类成员路径决策类对象path_decision_，不可修改
const PathDecision& path_decision() const;
//返回类成员参考线对象reference_line_，不可修改
const ReferenceLine& reference_line() const;
//返回类成员参考线对象reference_line_，可修改
ReferenceLine* mutable_reference_line();
//计算离目标点剩余的纵向距离
double SDistanceToDestination() const;
//判断是否到达目标点，到目标点剩余s距离<5cm?是的话就到了
bool ReachedDestination() const;
//设置类成员离散轨迹discretized_trajectory_ ，拷贝输入的离散轨迹trajectory
void SetTrajectory(const DiscretizedTrajectory& trajectory);
//返回类成员 离散轨迹类对象discretized_trajectory_
const DiscretizedTrajectory& trajectory() const;
//返回类成员cost_，成本，是routing的线路cost?
double Cost() const { return cost_; }
//类成员cost_加上输入的成本
void AddCost(double cost) { cost_ += cost; }
//设置类成员成本cost_
void SetCost(double cost) { cost_ = cost; }
//返回及设置类成员优先成本PriorityCost？
double PriorityCost() const { return priority_cost_; }
void SetPriorityCost(double cost) { priority_cost_ = cost; }
// 设置Lattice规划停止点
void SetLatticeStopPoint(const StopPoint& stop_point);
//设置Lattice巡航速度
void SetLatticeCruiseSpeed(double speed);
//返回类成员规划目标planning_target_
const PlanningTarget& planning_target() const { return planning_target_; }
//设置类成员巡航速度cruise_speed_ 为输入的速度
void SetCruiseSpeed(double speed) { cruise_speed_ = speed; }
//获取类成员巡航速度cruise_speed_ ，不可修改
double GetCruiseSpeed() const;
//根据给定的s,找到参考线对象上给定s对应的LaneInfoConstPtr类车道对象指针
hdmap::LaneInfoConstPtr LocateLaneInfo(const double s) const;
//获取相邻车道，获取其id和车道宽度放入输入参数ptr_lane_id，ptr_lane_width
//输入参数：lane_type，代表需要获取的是哪个邻居车道 左前/左后/右前/右后
bool GetNeighborLaneInfo(const ReferenceLineInfo::LaneType lane_type,
const double s, hdmap::Id* ptr_lane_id,
double* ptr_lane_width) const;
//判断当前参考线的起点位于上一次参考线上？
bool IsStartFrom(const ReferenceLineInfo& previous_reference_line_info) const;
//返回类成员指针debug_,可修改
planning_internal::Debug* mutable_debug() { return &debug_; }
//返回类成员指针debug_,不可修改
const planning_internal::Debug& debug() const { return debug_; }
//返回类成员延迟状态latency_stats_的地址，返回值可供修改
LatencyStats* mutable_latency_stats() { return &latency_stats_; }
//返回类成员延迟状态latency_stats_，返回值不可修改
const LatencyStats& latency_stats() const { return latency_stats_; }
//返回类成员path_data_路径数据，其就是一条路径？无法更改，是const
const PathData& path_data() const;
//返回类成员，规划失效的路径数据fallback_path_data_，const修饰不可更改
const PathData& fallback_path_data() const;
//返回类成员speed_data_，速度规划数据，const修饰，不可更改
const SpeedData& speed_data() const;
PathData* mutable_path_data();
//返回类成员，规划失效的路径数据fallback_path_data_的指针，可以修改
PathData* mutable_fallback_path_data();
//返回类成员speed_data_，速度规划数据指针，可更改
SpeedData* mutable_speed_data();
//返回类成员rss_info_，不可修改
const RSSInfo& rss_info() const;
//返回类成员rss_info_，可修改
RSSInfo* mutable_rss_info();
//结合速度规划和路径规划，共同结合生成一条离散轨迹放入输入参数ptr_discretized_trajectory
//输入参数：规划起始点的相对时间？relative_time
//输入参数：规划起始点的s纵坐标？
//输入参数：ptr_discretized_trajectory该指针用于存放结合结果
bool CombinePathAndSpeedProfile(
const double relative_time, const double start_s,
DiscretizedTrajectory* discretized_trajectory);
//插入规划起始点是一种直接的方式，一种优雅的方式以某种方式绕过轨迹缝合逻辑，或在起始时使用缝合的规划起始点？Apollo原注释
//说白了，整个函数就是将规划轨迹调整到从当前位置开始，起始点的时间戳为(当前时间+规划周期0.1s)，重新组织t,s，裁剪掉轨迹上规划起始点之前的轨迹。
//输入参数：规划起始点planning_start_point，这个参数推测可能是输入车辆当前实际位置？
//输入参数：轨迹点vector--trajectory,待调整的轨迹
//输入参数：离散轨迹类对象，用于存放调整后的轨迹
bool AdjustTrajectoryWhichStartsFromCurrentPos(
const common::TrajectoryPoint& planning_start_point,
const std::vector<common::TrajectoryPoint>& trajectory,
DiscretizedTrajectory* adjusted_trajectory);
//返回自车的SL边界，包含自车边界start_s,end_s,start_l,end_l信息
const SLBoundary& AdcSlBoundary() const;
//参考线上路径规划，速度规划debug字符打印
std::string PathSpeedDebugString() const;
/**
* Check if the current reference line is a change lane reference line, i.e.,
* ADC's current position is not on this reference line.
* 检查当前参考线是否是一个变道参考线，例如，ADC当前为置就不在这条参考线上说明是变道参考线？
*/
bool IsChangeLanePath() const;
/**
* Check if the current reference line is the neighbor of the vehicle
* current position
* 检查当前的参考线是否是自车当前位置相邻参考线
*/
bool IsNeighborLanePath() const;
/**
* Set if the vehicle can drive following this reference line
* A planner need to set this value to true if the reference line is OK
* 设置是否车辆可以沿着这条参考线驾驶，一个规划器需要设置该值为true，如果参考线是ok的话
*/
void SetDrivable(bool drivable);
//返回类成员是否可驾驶is_drivable_
bool IsDrivable() const;
//导出参与建议？engage_advice是planning发布轨迹ADCTrajectory中包含的信息，推测是判断规划是否应该介入？
//其实最终结果是存放在输入的参数指针engage_advice中
void ExportEngageAdvice(common::EngageAdvice* engage_advice,
PlanningContext* planning_context) const;
//返回参考线上所有的RouteSegments类的车道对象，也就是ReferenceLineInfo类成员lanes_
//包括这条参考线上的所有车道对象
const hdmap::RouteSegments& Lanes() const;
//获取目标车道id列表，其实就是获取参考线上所有车道的id的列表
std::list<hdmap::Id> TargetLaneId() const;
//导出决策信息到输入参数decision_result，planning_context中？
void ExportDecision(DecisionResult* decision_result,
PlanningContext* planning_context) const;
//设置junction路口的路权
//输入参数：路口junction_s纵坐标
//输入参数：是否有保护is_protected
//实际上这个就是设置特定的路口的路权为有保护或无保护？
void SetJunctionRightOfWay(const double junction_s,
const bool is_protected) const;
//查询当前自车所处的junction路口的路权，是有保护/无保护
ADCTrajectory::RightOfWayStatus GetRightOfWayStatus() const;
//设置路径的转弯类型，输入参数是纵向s坐标，指定位置s处的转弯类型
hdmap::Lane::LaneTurn GetPathTurnType(const double s) const;
//获取路口路权 输入参数是hdmap的pnc_junction_overlap PNC路口overlap
bool GetIntersectionRightofWayStatus(
const hdmap::PathOverlap& pnc_junction_overlap) const;
//返回及设置类成员相对相邻参考线的偏置offset_to_other_reference_line_
double OffsetToOtherReferenceLine() const {
return offset_to_other_reference_line_;
}
void SetOffsetToOtherReferenceLine(const double offset) {
offset_to_other_reference_line_ = offset;
}
//获取及设置候选路径的边界，PathBoundary主要是储存路径边界的SL坐标序列
const std::vector<PathBoundary>& GetCandidatePathBoundaries() const;
void SetCandidatePathBoundaries(
std::vector<PathBoundary>&& candidate_path_boundaries);
//返回及设置数据成员candidate_path_data_，候选路径数据，是一个路径数据PathData的vector,一个PathData就是一条路径？
const std::vector<PathData>& GetCandidatePathData() const;
void SetCandidatePathData(std::vector<PathData>&& candidate_path_data);
//返回及设置阻塞该参考线的障碍物对象
Obstacle* GetBlockingObstacle() const { return blocking_obstacle_; }
void SetBlockingObstacle(const std::string& blocking_obstacle_id);
//返回及设置类成员是否为借道标志位is_path_lane_borrow_
bool is_path_lane_borrow() const { return is_path_lane_borrow_; }
void set_is_path_lane_borrow(const bool is_path_lane_borrow) {
is_path_lane_borrow_ = is_path_lane_borrow;
}
//设置在参考线上位true，自车？
void set_is_on_reference_line() { is_on_reference_line_ = true; }
//返回参考线的优先级
uint32_t GetPriority() const { return reference_line_.GetPriority(); }
//设置参考线优先级
void SetPriority(uint32_t priority) { reference_line_.SetPriority(priority); }
//设置轨迹类型，实际就是将输入的轨迹类型设置到类成员trajectory_type_
void set_trajectory_type(
const ADCTrajectory::TrajectoryType trajectory_type) {
trajectory_type_ = trajectory_type;
}
//获取类成员轨迹类型trajectory_type_
ADCTrajectory::TrajectoryType trajectory_type() const {
return trajectory_type_;
}
//返回类成员ST图数据，可修改
StGraphData* mutable_st_graph_data() { return &st_graph_data_; }
//返回类成员ST图数据，不可修改
const StGraphData& st_graph_data() { return st_graph_data_; }
// different types of overlaps that can be handled by different scenarios.
//不同的overlap类型可以被处理成不同的场景？
enum OverlapType {
CLEAR_AREA = 1,
CROSSWALK = 2,
OBSTACLE = 3,
PNC_JUNCTION = 4,
SIGNAL = 5,
STOP_SIGN = 6,
YIELD_SIGN = 7,
};
//返回类成员第一个overlap的集合，各种overlap里s最小的那一个
const std::vector<std::pair<OverlapType, hdmap::PathOverlap>>&
FirstEncounteredOverlaps() const {
return first_encounter_overlaps_;
}
//获取参考线上指定纵向位置s处的pnc路口overlap，存放到输入参数指针pnc_junction_overlap
int GetPnCJunction(const double s,
hdmap::PathOverlap* pnc_junction_overlap) const;
//获得所有的停止决策的停止点的SL坐标？存入输入参数result中，其是一个SL坐标点的vector
std::vector<common::SLPoint> GetAllStopDecisionSLPoint() const;
//设置类成员信号灯vehicle_signal_，设置为给定的的转向信号灯
void SetTurnSignal(const common::VehicleSignal::TurnSignal& turn_signal);
//设置类成员信号灯vehicle_signal_，设置紧急双闪灯？
void SetEmergencyLight();
//设置及返回类成员路径可以重复使用？
void set_path_reusable(const bool path_reusable) {
path_reusable_ = path_reusable;
}
bool path_reusable() const { return path_reusable_; }
private:
//在上方类成员函数Init()被调用
//获取参考线上的第一个(s坐标最小的那个)clear_area_overlap、crosswalk_overlap、pnc_junction_overlap、signal_overlap、stop_sign_overlap、yield_sign_overlap放入类成员first_encounter_overlaps_里
void InitFirstOverlaps();
//检查是否位换道，但是没有代码实现？
bool CheckChangeLane() const;
//根据车道转弯类型设置转向灯，都设置到输入的指针vehicle_signal里
void SetTurnSignalBasedOnLaneTurnType(
common::VehicleSignal* vehicle_signal) const;
//将类成员的车辆信号灯对象导出到输入参数指针vehicle_signal？
void ExportVehicleSignal(common::VehicleSignal* vehicle_signal) const;
//判断输入的障碍物是否可被忽略
bool IsIrrelevantObstacle(const Obstacle& obstacle);
//设置决策结果(放入输入参数decision_result)，输入参数decision_result(决策结果)里的main_decision(主决策)里先默认为cruise巡航，再看是否有stop,设置主任务决策，最后设置障碍物决策，结果填入输入的两个参数指针中decision_result，planning_context
//planning待发布的轨迹里包含DecisionResult信息
void MakeDecision(DecisionResult* decision_result,
PlanningContext* planning_context) const;
//设置主停止决策，就看path_decision_的障碍物列表里是否有stop决策，有的话设置一下输入的decision_result的main_decision里的stop，planning待发布的轨迹里包含DecisionResult信息
int MakeMainStopDecision(DecisionResult* decision_result) const;
//主要是判断车辆是否已经完成主任务，已经到达目标点，若是设置输入的decision_result里的main_decision里的mission_complete里的停止点，停止heading等信息，planning_context主要是存放一些场景中间信息。planning待发布的轨迹里包含DecisionResult信息
void MakeMainMissionCompleteDecision(DecisionResult* decision_result,
PlanningContext* planning_context) const;
//作出急停决策，结果也是存放到输入参数指针decision_result
void MakeEStopDecision(DecisionResult* decision_result) const;
//设置障碍物决策？针对障碍物的决策都设置到输入参数指针object_decisions障碍物决策列表里
void SetObjectDecisions(ObjectDecisions* object_decisions) const;
//往类成员路径决策对象path_decision_里增加一个障碍物对象，增加的就是输入的障碍物
bool AddObstacleHelper(const std::shared_ptr<Obstacle>& obstacle);
//获取第一个Overlap对象，就是找到输入参数 overlap的vector里纵向坐标最小的那一个，找到的这个overlap存放到另一个输入参数path_overlap
//overlap的概念参见附录
bool GetFirstOverlap(const std::vector<hdmap::PathOverlap>& path_overlaps,
hdmap::PathOverlap* path_overlap);
private:
static std::unordered_map<std::string, bool> junction_right_of_way_map_;
const common::VehicleState vehicle_state_;
const common::TrajectoryPoint adc_planning_point_;
ReferenceLine reference_line_;
/**
* @brief this is the number that measures the goodness of this reference
* line. The lower the better.
*/
double cost_ = 0.0;
bool is_drivable_ = true;
PathDecision path_decision_;
Obstacle* blocking_obstacle_;
std::vector<PathBoundary> candidate_path_boundaries_;
std::vector<PathData> candidate_path_data_;
PathData path_data_;
PathData fallback_path_data_;
SpeedData speed_data_;
DiscretizedTrajectory discretized_trajectory_;
RSSInfo rss_info_;
/**
* @brief SL boundary of stitching point (starting point of plan trajectory)
* relative to the reference line
*/
SLBoundary adc_sl_boundary_;
planning_internal::Debug debug_;
LatencyStats latency_stats_;
hdmap::RouteSegments lanes_;
bool is_on_reference_line_ = false;
bool is_path_lane_borrow_ = false;
ADCTrajectory::RightOfWayStatus status_ = ADCTrajectory::UNPROTECTED;
double offset_to_other_reference_line_ = 0.0;
double priority_cost_ = 0.0;
PlanningTarget planning_target_;
ADCTrajectory::TrajectoryType trajectory_type_ = ADCTrajectory::UNKNOWN;
/**
* Overlaps encountered in the first time along the reference line in front of
* the vehicle
*/
std::vector<std::pair<OverlapType, hdmap::PathOverlap>>
first_encounter_overlaps_;
/**
* @brief Data generated by speed_bounds_decider for constructing st_graph for
* different st optimizer
*/
StGraphData st_graph_data_;
common::VehicleSignal vehicle_signal_;
double cruise_speed_ = 0.0;
bool path_reusable_ = false;
DISALLOW_COPY_AND_ASSIGN(ReferenceLineInfo);
};
} // namespace planning
} // namespace apollo
reference_line_info.cc
#include "modules/planning/common/reference_line_info.h"
#include <algorithm>
#include "absl/strings/str_cat.h"
#include "cyber/task/task.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/point_factory.h"
#include "modules/common/util/util.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/proto/planning_status.pb.h"
#include "modules/planning/proto/sl_boundary.pb.h"
namespace apollo {
namespace planning {
using apollo::canbus::Chassis;
using apollo::common::EngageAdvice;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleConfigHelper;
using apollo::common::VehicleSignal;
using apollo::common::math::Box2d;
using apollo::common::math::Vec2d;
using apollo::common::util::PointFactory;
//junction和路权的映射map?
std::unordered_map<std::string, bool>
ReferenceLineInfo::junction_right_of_way_map_;
//ReferenceLineInfo带参构造函数
//参数：vehicle_state自车状态
//参数：adc_planning_point自车规划起始点
//参数：reference_line参考线
//参数：segments是继承车道段的vector
//这个带参构造函数就是将输入参数拿去初始化自己的类成员vehicle_state_，adc_planning_point_，reference_line_，lanes_
ReferenceLineInfo::ReferenceLineInfo(const common::VehicleState& vehicle_state,
const TrajectoryPoint& adc_planning_point,
const ReferenceLine& reference_line,
const hdmap::RouteSegments& segments)
: vehicle_state_(vehicle_state),
adc_planning_point_(adc_planning_point),
reference_line_(reference_line),
lanes_(segments) {}
//初始化函数，输入参数是障碍物类vector，也就是障碍物列表
bool ReferenceLineInfo::Init(const std::vector<const Obstacle*>& obstacles) {
//获取车辆的物理参数放入 param
const auto& param = VehicleConfigHelper::GetConfig().vehicle_param();
// stitching point缝合点
//path_point取类成员adc_planning_point_,就是规划起始点的路径点？
const auto& path_point = adc_planning_point_.path_point();
//二维向量position就是车辆的规划起点的大地xy坐标
Vec2d position(path_point.x(), path_point.y());
//二维向量 vec_to_center 车辆几何中心的车体坐标(就是以后轴中心为原点，几何中心的xy坐标，几何中心相对于后轴中心的坐标，车辆纵向为车体x轴，横向为车体y轴)
Vec2d vec_to_center(
(param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
(param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);
//将几何中心的车体坐标转化到大地坐标系下的xy坐标
Vec2d center(position + vec_to_center.rotate(path_point.theta()));
//自车在轨迹缝合点，也就是规划起始点处的二维边界盒，几何中心+车长宽构造边界盒
Box2d box(center, path_point.theta(), param.length(), param.width());
// realtime vehicle position
//自车当前实际位置大地xy坐标
Vec2d vehicle_position(vehicle_state_.x(), vehicle_state_.y());
//车辆几何中心vehicle_center，在当前实际位置处的大地坐标
Vec2d vehicle_center(vehicle_position +
vec_to_center.rotate(vehicle_state_.heading()));
//车辆在当前位置处的二维边界盒
Box2d vehicle_box(vehicle_center, vehicle_state_.heading(), param.length(),
param.width());
//缝合点处二维边界盒，转化为相对于参考线的自车sl边界，SL坐标
//adc_sl_boundary_属于SLBoundary类，modules\planning\proto\sl_boundary.proto定义的message类，包含start_s，end_s，start_l，end_l起始就是车辆前后左右边缘中点相对于参考线的SL坐标，可以参见附录的图
if (!reference_line_.GetSLBoundary(box, &adc_sl_boundary_)) {
AERROR << "Failed to get ADC boundary from box: " << box.DebugString();
return false;
}
//获取参考线上的第一个(s坐标最小的那个)clear_area_overlap、crosswalk_overlap、pnc_junction_overlap、signal_overlap、stop_sign_overlap、yield_sign_overlap放入类成员first_encounter_overlaps_里
InitFirstOverlaps();
//如果自车在轨迹缝合点(规划起始点？)处的SL边界，若车头s<0或车尾的s比参考线的长度还要长，这样说明缝合点不在参考线上？报警告信息
if (adc_sl_boundary_.end_s() < 0 ||
adc_sl_boundary_.start_s() > reference_line_.Length()) {
AWARN << "Vehicle SL " << adc_sl_boundary_.ShortDebugString()
<< " is not on reference line:[0, " << reference_line_.Length()
<< "]";
}
//定义一个阈值kOutOfReferenceLineL 10.0m
//若自车的轨迹缝合点处车右边界start_l横向上相对参考线往左偏离10m以上或
//自车的轨迹缝合点处车左边界end_l横向上相对参考线往右偏离10m以上则说明自车当前位置离参考线过远，返回false
static constexpr double kOutOfReferenceLineL = 10.0; // in meters
if (adc_sl_boundary_.start_l() > kOutOfReferenceLineL ||
adc_sl_boundary_.end_l() < -kOutOfReferenceLineL) {
AERROR << "Ego vehicle is too far away from reference line.";
return false;
}
//根据自车的边界的SL坐标判断轨迹缝合点(规划起始点)在类成员参考线对象上？
//其实就是判断自车边界纵向的start_s,end_s是否在参考线0-参考线长度范围内？自车边界的
//横向start_l，end_l是否超出当前缝合点所处车道的车道宽度？从而判断轨迹缝合点处自车是否在车道内
is_on_reference_line_ = reference_line_.IsOnLane(adc_sl_boundary_);
//往类成员path_decision_路径决策对象里把输入参数障碍物列表放入path_decision_的类成员障碍物列表里obstacles_
if (!AddObstacles(obstacles)) {
AERROR << "Failed to add obstacles to reference line";
return false;
}
//获取参考线对象reference_line_上的地图路径map_path
const auto& map_path = reference_line_.map_path();
//遍历参考线上地图路径的每一个减速带overlap
for (const auto& speed_bump : map_path.speed_bump_overlaps()) {
// -1 and + 1.0 are added to make sure it can be sampled.
//将减速度带的起点s，终点s都往外扩1m的范围内增加16km/h的限速
//FLAGS_speed_bump_speed_limit gflags去XXX.gflags.cc里取出相应的值
reference_line_.AddSpeedLimit(speed_bump.start_s - 1.0,
speed_bump.end_s + 1.0,
FLAGS_speed_bump_speed_limit);
}
//设置默认的巡航速度，这个应该是从modules\planning\common\planning_gflags.cc中取出default_cruise_speed的值，默认为5m/s就是18km/h，将ReferenceLineInfo类数据成员cruise_speed_巡航速度设置为这个gflag值
SetCruiseSpeed(FLAGS_default_cruise_speed);
//设置lattice规划器的默认巡航速度(无任何其他障碍物影响情况下)？lattice网格？
SetLatticeCruiseSpeed(FLAGS_default_cruise_speed);
//情况车辆的灯光信号，也是类成员vehicle_signal_
vehicle_signal_.Clear();
return true;
}
//返回数据成员candidate_path_data_，候选路径数据，是一个路径数据PathData的vector,一个PathData就是一条路径？
const std::vector<PathData>& ReferenceLineInfo::GetCandidatePathData() const {
return candidate_path_data_;
}
//设置数据成员候选路径数据
void ReferenceLineInfo::SetCandidatePathData(
std::vector<PathData>&& candidate_path_data) {
candidate_path_data_ = std::move(candidate_path_data);
}
//获取候选路径的边界，PathBoundary主要是储存路径边界的SL坐标序列
const std::vector<PathBoundary>& ReferenceLineInfo::GetCandidatePathBoundaries()
const {
return candidate_path_boundaries_;
}
//设置候选路径的边界
void ReferenceLineInfo::SetCandidatePathBoundaries(
std::vector<PathBoundary>&& path_boundaries) {
candidate_path_boundaries_ = std::move(path_boundaries);
}
//获取参考线的巡航速度，是数据成员cruise_speed_,如果其大于0的话，否则就返回默认巡航速度
double ReferenceLineInfo::GetCruiseSpeed() const {
return cruise_speed_ > 0.0 ? cruise_speed_ : FLAGS_default_cruise_speed;
}
//根据给定的s,找到参考线对象上给定s对应的LaneInfoConstPtr类车道对象指针
hdmap::LaneInfoConstPtr ReferenceLineInfo::LocateLaneInfo(
const double s) const {
std::vector<hdmap::LaneInfoConstPtr> lanes;
reference_line_.GetLaneFromS(s, &lanes);
if (lanes.empty()) {
AWARN << "cannot get any lane using s";
return nullptr;
}
return lanes.front();
}
//获取相邻车道，获取其id和车道宽度放入输入参数ptr_lane_id，ptr_lane_width
//输入参数：lane_type，代表需要获取的是哪个邻居车道 左前/左后/右前/右后
bool ReferenceLineInfo::GetNeighborLaneInfo(
const ReferenceLineInfo::LaneType lane_type, const double s,
hdmap::Id* ptr_lane_id, double* ptr_lane_width) const {
auto ptr_lane_info = LocateLaneInfo(s);
if (ptr_lane_info == nullptr) {
return false;
}
switch (lane_type) {
case LaneType::LeftForward: {
if (ptr_lane_info->lane().left_neighbor_forward_lane_id().empty()) {
return false;
}
*ptr_lane_id = ptr_lane_info->lane().left_neighbor_forward_lane_id(0);
break;
}
case LaneType::LeftReverse: {
if (ptr_lane_info->lane().left_neighbor_reverse_lane_id().empty()) {
return false;
}
*ptr_lane_id = ptr_lane_info->lane().left_neighbor_reverse_lane_id(0);
break;
}
case LaneType::RightForward: {
if (ptr_lane_info->lane().right_neighbor_forward_lane_id().empty()) {
return false;
}
*ptr_lane_id = ptr_lane_info->lane().right_neighbor_forward_lane_id(0);
break;
}
case LaneType::RightReverse: {
if (ptr_lane_info->lane().right_neighbor_reverse_lane_id().empty()) {
return false;
}
*ptr_lane_id = ptr_lane_info->lane().right_neighbor_reverse_lane_id(0);
break;
}
default:
ACHECK(false);
}
auto ptr_neighbor_lane =
hdmap::HDMapUtil::BaseMapPtr()->GetLaneById(*ptr_lane_id);
if (ptr_neighbor_lane == nullptr) {
return false;
}
auto ref_point = reference_line_.GetReferencePoint(s);
double neighbor_s = 0.0;
double neighbor_l = 0.0;
if (!ptr_neighbor_lane->GetProjection({ref_point.x(), ref_point.y()},
&neighbor_s, &neighbor_l)) {
return false;
}
*ptr_lane_width = ptr_neighbor_lane->GetWidth(neighbor_s);
return true;
}
//获取第一个Overlap对象，就是找到输入参数 overlap的vector里纵向坐标最小的那一个，找到的这个overlap存放到另一个输入参数path_overlap
//overlap的概念参见附录
bool ReferenceLineInfo::GetFirstOverlap(
const std::vector<hdmap::PathOverlap>& path_overlaps,
hdmap::PathOverlap* path_overlap) {
CHECK_NOTNULL(path_overlap);
const double start_s = adc_sl_boundary_.end_s();
static constexpr double kMaxOverlapRange = 500.0;
double overlap_min_s = kMaxOverlapRange;
auto overlap_min_s_iter = path_overlaps.end();
for (auto iter = path_overlaps.begin(); iter != path_overlaps.end(); ++iter) {
if (iter->end_s < start_s) {
continue;
}
if (overlap_min_s > iter->start_s) {
overlap_min_s_iter = iter;
overlap_min_s = iter->start_s;
}
}
// Ensure that the path_overlaps is not empty.
if (overlap_min_s_iter != path_overlaps.end()) {
*path_overlap = *overlap_min_s_iter;
}
return overlap_min_s < kMaxOverlapRange;
}
//在上方类成员函数Init()被调用
//获取参考线上的第一个(s坐标最小的那个)clear_area_overlap、crosswalk_overlap、pnc_junction_overlap、signal_overlap、stop_sign_overlap、yield_sign_overlap放入类成员first_encounter_overlaps_里
void ReferenceLineInfo::InitFirstOverlaps() {
//获取参考线地图路径map_path
const auto& map_path = reference_line_.map_path();
// 找到参考线上第一个clear_zone的overlap塞入类成员first_encounter_overlaps_
hdmap::PathOverlap clear_area_overlap;
if (GetFirstOverlap(map_path.clear_area_overlaps(), &clear_area_overlap)) {
first_encounter_overlaps_.emplace_back(CLEAR_AREA, clear_area_overlap);
}
// crosswalk
// 找到参考线上第一个crosswalk的overlap塞入类成员first_encounter_overlaps_
hdmap::PathOverlap crosswalk_overlap;
if (GetFirstOverlap(map_path.crosswalk_overlaps(), &crosswalk_overlap)) {
first_encounter_overlaps_.emplace_back(CROSSWALK, crosswalk_overlap);
}
// pnc_junction
// 找到参考线上第一个pnc_junction的overlap塞入类成员first_encounter_overlaps_
hdmap::PathOverlap pnc_junction_overlap;
if (GetFirstOverlap(map_path.pnc_junction_overlaps(),
&pnc_junction_overlap)) {
first_encounter_overlaps_.emplace_back(PNC_JUNCTION, pnc_junction_overlap);
}
// signal
// 找到参考线上第一个signal的overlap塞入类成员first_encounter_overlaps_
hdmap::PathOverlap signal_overlap;
if (GetFirstOverlap(map_path.signal_overlaps(), &signal_overlap)) {
first_encounter_overlaps_.emplace_back(SIGNAL, signal_overlap);
}
// stop_sign
// 找到参考线上第一个stop_sign的overlap塞入类成员first_encounter_overlaps_
hdmap::PathOverlap stop_sign_overlap;
if (GetFirstOverlap(map_path.stop_sign_overlaps(), &stop_sign_overlap)) {
first_encounter_overlaps_.emplace_back(STOP_SIGN, stop_sign_overlap);
}
// yield_sign
// 找到参考线上第一个yield_sign的overlap塞入类成员first_encounter_overlaps_
hdmap::PathOverlap yield_sign_overlap;
if (GetFirstOverlap(map_path.yield_sign_overlaps(), &yield_sign_overlap)) {
first_encounter_overlaps_.emplace_back(YIELD_SIGN, yield_sign_overlap);
}
// sort by start_s
//将first_encounter_overlaps_里的这些overlap按照start_s从小到大排列
//这种排序的写法可以学习下，在附录里具体说明
if (!first_encounter_overlaps_.empty()) {
std::sort(first_encounter_overlaps_.begin(),
first_encounter_overlaps_.end(),
[](const std::pair<OverlapType, hdmap::PathOverlap>& a,
const std::pair<OverlapType, hdmap::PathOverlap>& b) {
return a.second.start_s < b.second.start_s;
});
}
}
//判断给定的纵向s坐标是否在输入参数overlap的范围内？
//s是否属于区间[start_s-0.01,end_s+0.01]？
//是的话，就说明给定的s在给定的overlap范围内
bool WithinOverlap(const hdmap::PathOverlap& overlap, double s) {
static constexpr double kEpsilon = 1e-2;
return overlap.start_s - kEpsilon <= s && s <= overlap.end_s + kEpsilon;
}
//设置junction路口的路权
//输入参数：路口junction_s纵坐标
//输入参数：是否有保护is_protected
//实际上这个就是设置特定的路口的路权为有保护或无保护？
void ReferenceLineInfo::SetJunctionRightOfWay(const double junction_s,
const bool is_protected) const {
//遍历参考线上所有的junction overlap
for (const auto& overlap : reference_line_.map_path().junction_overlaps()) {
//如果给定的s就是在这个junction overlap范围里
if (WithinOverlap(overlap, junction_s)) {
//在类成员里设置overlap id对应的路权为给定的输入参数 is_protected
junction_right_of_way_map_[overlap.object_id] = is_protected;
}
}
}
//查询当前自车所处的junction路口的路权，是有保护/无保护
ADCTrajectory::RightOfWayStatus ReferenceLineInfo::GetRightOfWayStatus() const {
//遍历参考线上的地图路径里的所有junction_overlap路口
for (const auto& overlap : reference_line_.map_path().junction_overlaps()) {
if (overlap.end_s < adc_sl_boundary_.start_s()) {
junction_right_of_way_map_.erase(overlap.object_id);
//如果自车的end_s在junction overlap范围内，就去类成员路权map里根据junction overlap id的路权
} else if (WithinOverlap(overlap, adc_sl_boundary_.end_s())) {
auto is_protected = junction_right_of_way_map_[overlap.object_id];
if (is_protected) {
return ADCTrajectory::PROTECTED;
}
}
}
return ADCTrajectory::UNPROTECTED;
}
//返回参考线上所有的RouteSegments类的车道对象，也就是ReferenceLineInfo类成员lanes_
//包括这条参考线上的所有车道对象
const hdmap::RouteSegments& ReferenceLineInfo::Lanes() const { return lanes_; }
//获取目标车道id列表，其实就是获取参考线上所有车道的id的列表
std::list<hdmap::Id> ReferenceLineInfo::TargetLaneId() const {
//定义个空的id列表lane_ids用来存放结果
std::list<hdmap::Id> lane_ids;
//遍历参考线上所有的车道，把每个车道的id塞入 lane_ids列表
for (const auto& lane_seg : lanes_) {
lane_ids.push_back(lane_seg.lane->id());
}
//返回车道id列表
return lane_ids;
}
//返回自车的SL边界，包含自车边界start_s,end_s,start_l,end_l信息
const SLBoundary& ReferenceLineInfo::AdcSlBoundary() const {
return adc_sl_boundary_;
}
//返回类成员路径决策类对象地址path_decision_，可修改
PathDecision* ReferenceLineInfo::path_decision() { return &path_decision_; }
//返回类成员路径决策类对象path_decision_，不可修改
const PathDecision& ReferenceLineInfo::path_decision() const {
return path_decision_;
}
//返回类成员参考线对象reference_line_，不可修改
const ReferenceLine& ReferenceLineInfo::reference_line() const {
return reference_line_;
}
//返回类成员参考线对象reference_line_，可修改
ReferenceLine* ReferenceLineInfo::mutable_reference_line() {
return &reference_line_;
}
//设置类成员离散轨迹discretized_trajectory_ ，拷贝输入的离散轨迹trajectory
void ReferenceLineInfo::SetTrajectory(const DiscretizedTrajectory& trajectory) {
discretized_trajectory_ = trajectory;
}
//往类成员路径决策对象path_decision_里增加一个障碍物对象，增加的就是输入的障碍物
bool ReferenceLineInfo::AddObstacleHelper(
const std::shared_ptr<Obstacle>& obstacle) {
return AddObstacle(obstacle.get()) != nullptr;
}
// 在类成员路径决策path_decision_里增加一个障碍物对象
Obstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) {
//若输入obstacle为空，直接返回一个空指针
if (!obstacle) {
AERROR << "The provided obstacle is empty";
return nullptr;
}
//定义指针mutable_obstacle 指向path_decision_新增的一个障碍物对象
auto* mutable_obstacle = path_decision_.AddObstacle(*obstacle);
//若指针为空，报错增加障碍物失败并返回空指针
if (!mutable_obstacle) {
AERROR << "failed to add obstacle " << obstacle->Id();
return nullptr;
}
//定义了一个SL边界类对象，SL边界通常就是start_s,end_s,start_l，end_l这些信息
SLBoundary perception_sl;
//类成员参考线对象将感知障碍物边界盒转化感知的障碍物的SL边界，转化结果放入引用变量perception_sl
if (!reference_line_.GetSLBoundary(obstacle->PerceptionBoundingBox(),
&perception_sl)) {
AERROR << "Failed to get sl boundary for obstacle: " << obstacle->Id();
return mutable_obstacle;
}
//通过指针mutable_obstacle设置path_decision_新增的障碍物对象的感知SL边界
mutable_obstacle->SetPerceptionSlBoundary(perception_sl);
//通过指针mutable_obstacle检查path_decision_新增的障碍物对象是否阻塞参考线
mutable_obstacle->CheckLaneBlocking(reference_line_);
//无论阻塞与否都报debug信息
if (mutable_obstacle->IsLaneBlocking()) {
ADEBUG << "obstacle [" << obstacle->Id() << "] is lane blocking.";
} else {
ADEBUG << "obstacle [" << obstacle->Id() << "] is NOT lane blocking.";
}
//判断新增的障碍物可否被忽略，若可忽略就往path_decision_里增加一个横向，纵向的可忽略决策
if (IsIrrelevantObstacle(*mutable_obstacle)) {
ObjectDecisionType ignore;
ignore.mutable_ignore();
path_decision_.AddLateralDecision("reference_line_filter", obstacle->Id(),
ignore);
path_decision_.AddLongitudinalDecision("reference_line_filter",
obstacle->Id(), ignore);
ADEBUG << "NO build reference line st boundary. id:" << obstacle->Id();
} else {
//若新增的障碍物不可被忽略，就根据障碍物去修正参考线的ST边界
ADEBUG << "build reference line st boundary. id:" << obstacle->Id();
mutable_obstacle->BuildReferenceLineStBoundary(reference_line_,
adc_sl_boundary_.start_s());
ADEBUG << "reference line st boundary: t["
<< mutable_obstacle->reference_line_st_boundary().min_t() << ", "
<< mutable_obstacle->reference_line_st_boundary().max_t() << "] s["
<< mutable_obstacle->reference_line_st_boundary().min_s() << ", "
<< mutable_obstacle->reference_line_st_boundary().max_s() << "]";
}
return mutable_obstacle;
}
//直接把输入参数障碍物列表加到类成员path_decision_里
bool ReferenceLineInfo::AddObstacles(
const std::vector<const Obstacle*>& obstacles) {
//打开多线程来增加障碍物，默认关闭，略过
if (FLAGS_use_multi_thread_to_add_obstacles) {
std::vector<std::future<Obstacle*>> results;
for (const auto* obstacle : obstacles) {
results.push_back(
cyber::Async(&ReferenceLineInfo::AddObstacle, this, obstacle));
}
for (auto& result : results) {
if (!result.get()) {
AERROR << "Fail to add obstacles.";
return false;
}
}
} else {
//如果不开多线程的话，遍历每一个障碍物对象，调用AddObstacle(obstacle)将其一一加到类成员path_decision_的成员里的障碍物列表里
for (const auto* obstacle : obstacles) {
if (!AddObstacle(obstacle)) {
AERROR << "Failed to add obstacle " << obstacle->Id();
return false;
}
}
}
return true;
}
//判断输入的障碍物是否可被忽略
bool ReferenceLineInfo::IsIrrelevantObstacle(const Obstacle& obstacle) {
//如果障碍物级别是Caution需要注意，那么就不能被忽略
if (obstacle.IsCautionLevelObstacle()) {
return false;
}
//如果障碍物的边界SL坐标里的end_s > 参考线长度，也就是障碍物都已经在参考线前方长度之外了，就直接可以忽略
const auto& obstacle_boundary = obstacle.PerceptionSLBoundary();
if (obstacle_boundary.end_s() > reference_line_.Length()) {
return true;
}
//如果自车在参考线上 且 不是换道路径 且 障碍物的车头纵向s坐标<自车车头纵向s坐标(障碍物在自车后面？不应该一个车头，一个车尾吗？) 且 (障碍物也在参考线上 或 障碍物车头s坐标<0代表障碍物在纵向上不在参考线范围内落后许多)，满足上述条件，a那说明障碍物也是可以被忽略
if (is_on_reference_line_ && !IsChangeLanePath() &&
obstacle_boundary.end_s() < adc_sl_boundary_.end_s() &&
(reference_line_.IsOnLane(obstacle_boundary) ||
obstacle_boundary.end_s() < 0.0)) { // if obstacle is far backward
return true;
}
//上面都没有返回的话，则说明障碍物是不可被忽略的
return false;
}
//返回类成员 离散轨迹类对象discretized_trajectory_
const DiscretizedTrajectory& ReferenceLineInfo::trajectory() const {
return discretized_trajectory_;
}
//设置Lattice规划停止点？设置类成员planning_target_里的成员stop_point，就是将输入的
//停止点，拷贝给它
void ReferenceLineInfo::SetLatticeStopPoint(const StopPoint& stop_point) {
planning_target_.mutable_stop_point()->CopyFrom(stop_point);
}
//设置Lattice规划的巡航速度，设置为输入的速度
//其实就是设置类成员planning_target_的成员cruise_speed为输入的速度
void ReferenceLineInfo::SetLatticeCruiseSpeed(double speed) {
planning_target_.set_cruise_speed(speed);
}
//判断当前参考线的起点位于上一次参考线上？
bool ReferenceLineInfo::IsStartFrom(
const ReferenceLineInfo& previous_reference_line_info) const {
//如果参考线对象的参考点为空则返回false
if (reference_line_.reference_points().empty()) {
return false;
}
//获取当前参考线对象起始参考点
auto start_point = reference_line_.reference_points().front();
//获取上一条参考线对象
const auto& prev_reference_line =
previous_reference_line_info.reference_line();
//定义一个空的SL点sl_point
common::SLPoint sl_point;
//将当前参考线的起始点大地XY坐标转化成相对于上一条参考线的SL坐标，放入sl_point
prev_reference_line.XYToSL(start_point, &sl_point);
//利用当前参考线的起始点在上一条参考线上的投影SL坐标作为参数判断其是否在上一条参考线车道上，在就返回true否则返回false
return previous_reference_line_info.reference_line_.IsOnLane(sl_point);
}
//返回类成员path_data_路径数据，其就是一条路径？无法更改，是const
const PathData& ReferenceLineInfo::path_data() const { return path_data_; }
//返回类成员，规划失效的路径数据fallback_path_data_，const修饰不可更改
const PathData& ReferenceLineInfo::fallback_path_data() const {
return fallback_path_data_;
}
//返回类成员speed_data_，速度规划数据，const修饰，不可更改
const SpeedData& ReferenceLineInfo::speed_data() const { return speed_data_; }
//返回类成员path_data_路径数据指针，其就是一条路径？可以更改
PathData* ReferenceLineInfo::mutable_path_data() { return &path_data_; }
//返回类成员，规划失效的路径数据fallback_path_data_的指针，可以修改
PathData* ReferenceLineInfo::mutable_fallback_path_data() {
return &fallback_path_data_;
}
//返回类成员speed_data_，速度规划数据指针，可更改
SpeedData* ReferenceLineInfo::mutable_speed_data() { return &speed_data_; }
//返回类成员rss_info_，不可修改
const RSSInfo& ReferenceLineInfo::rss_info() const { return rss_info_; }
//返回类成员rss_info_的地址指针，可修改
RSSInfo* ReferenceLineInfo::mutable_rss_info() { return &rss_info_; }
//结合速度规划和路径规划，共同结合生成一条离散轨迹放入输入参数ptr_discretized_trajectory
//输入参数：规划起始点的相对时间？relative_time
//输入参数：规划起始点的s纵坐标？
//输入参数：ptr_discretized_trajectory该指针用于存放结合结果
bool ReferenceLineInfo::CombinePathAndSpeedProfile(
const double relative_time, const double start_s,
DiscretizedTrajectory* ptr_discretized_trajectory) {
ACHECK(ptr_discretized_trajectory != nullptr);
//用不同的时间精度来减少数据负载但是也提供足够的数据点给control模块
//密集的时间采样精度0.02s
const double kDenseTimeResoltuion = FLAGS_trajectory_time_min_interval;
//稀疏的时间采样精度0.1s
const double kSparseTimeResolution = FLAGS_trajectory_time_max_interval;
//这个应该是稀疏和密集采样的时间分界点1.0s，规划轨迹1.0s以内按照0.02s采样，1.0s以外
//按照0.1s采样
const double kDenseTimeSec = FLAGS_trajectory_time_high_density_period;
//如果路径数据path_data_(ReferenceLineInfo类数据成员)为空，报错返回false
if (path_data_.discretized_path().empty()) {
AERROR << "path data is empty";
return false;
}
//如果速度规划数据speed_data_(ReferenceLineInfo类数据成员)为空，报错返回
if (speed_data_.empty()) {
AERROR << "speed profile is empty";
return false;
}
//遍历速度规划里速度点的时间，1.0s内每次+0.02s，1.0s外每次+0.1s
for (double cur_rel_time = 0.0; cur_rel_time < speed_data_.TotalTime();
cur_rel_time += (cur_rel_time < kDenseTimeSec ? kDenseTimeResoltuion
: kSparseTimeResolution)) { //定义一个空的速度点speed_point
common::SpeedPoint speed_point;
//根据当前的相对时间cur_rel_time，就是从0.0s开始 +0.02s or 0.1s叠加上来的
//根据当前的相对时间cur_rel_time去类成员速度规划数据speed_data_里去插值，插值得到的速度点(主要包含s,v,a,t,da等信息)放入速度点类对象speed_point里
if (!speed_data_.EvaluateByTime(cur_rel_time, &speed_point)) {
AERROR << "Fail to get speed point with relative time " << cur_rel_time;
return false;
}
//如果速度点的s已经大于离散轨迹的总长度了，直接跳出循环
if (speed_point.s() > path_data_.discretized_path().Length()) {
break;
}
//根据速度点speed_point对应的s，去类成员path_data_路径数据上插值得到，这个s对应的路径点path_point(主要包含x,y,z,曲率，曲率变化率等信息),根据速度点s插值得到的路径点放入path_point
common::PathPoint path_point =
path_data_.GetPathPointWithPathS(speed_point.s());
//设置path_point的s为路径点s+起始点s,因为路径数据path_data_和速度规划数据speed_data_其第一个点的s就是为0，所以加上规划起始点s和之前的参考线就接起来了。
path_point.set_s(path_point.s() + start_s);
//轨迹=路径+速度规划
//定义一个空的轨迹点类对象
common::TrajectoryPoint trajectory_point;
//轨迹点里的路径点设置为path_point
trajectory_point.mutable_path_point()->CopyFrom(path_point);
//轨迹点的v,a为speed_point的v,a
trajectory_point.set_v(speed_point.v());
trajectory_point.set_a(speed_point.a());
//轨迹点的相对时间为speed_point的t+规划起始点的相对时间
trajectory_point.set_relative_time(speed_point.t() + relative_time);
//最后离散轨迹指针里增加这个轨迹点，循环结束后所有的速度点和路径点就完成了融合，融合成了离散轨迹
ptr_discretized_trajectory->AppendTrajectoryPoint(trajectory_point);
}
return true;
}
//插入规划起始点是一种直接的方式，一种优雅的方式以某种方式绕过轨迹缝合逻辑，或在起始时使用缝合的规划起始点？Apollo原注释
//说白了，整个函数就是将规划轨迹调整到从当前位置开始，起始点的时间戳为(当前时间+规划周期0.1s)，重新组织t,s，裁剪掉轨迹上规划起始点之前的轨迹。
//输入参数：规划起始点planning_start_point，这个参数推测可能是输入车辆当前实际位置？
//输入参数：轨迹点vector--trajectory,待调整的轨迹
//输入参数：离散轨迹类对象，用于存放调整后的轨迹
bool ReferenceLineInfo::AdjustTrajectoryWhichStartsFromCurrentPos(
const common::TrajectoryPoint& planning_start_point,
const std::vector<common::TrajectoryPoint>& trajectory,
DiscretizedTrajectory* adjusted_trajectory) {
ACHECK(adjusted_trajectory != nullptr);
//通过检查heading找插入起始点的下标?下面定义了个常数kMaxAngleDiff 90°
static constexpr double kMaxAngleDiff = M_PI_2;
//获取调整前规划起始点的航向theta,x,y,相对时间
const double start_point_heading = planning_start_point.path_point().theta();
const double start_point_x = planning_start_point.path_point().x();
const double start_point_y = planning_start_point.path_point().y();
const double start_point_relative_time = planning_start_point.relative_time();
//初始定义一个规划起始点的插入下标为-1，-1通常代表最后一个
int insert_idx = -1;
//遍历待调整轨迹上的每个轨迹点
for (size_t i = 0; i < trajectory.size(); ++i) {
// skip trajectory_points early than planning_start_point
//跳过待调整轨迹上相对时间早于输入的规划起始点的那些轨迹点，跳过不处理，其实就是裁剪掉，并不塞入调整后的轨迹结果adjusted_trajectory中
if (trajectory[i].relative_time() <= start_point_relative_time) {
continue;
}
//当前遍历的第i个轨迹点x,y坐标
const double cur_point_x = trajectory[i].path_point().x();
const double cur_point_y = trajectory[i].path_point().y();
//track_heading是第i个轨迹点与规划起始点连线与大地X轴所成角
const double tracking_heading =
std::atan2(cur_point_y - start_point_y, cur_point_x - start_point_x);
//若(tracking_heading-起始点航向)的绝对值<90°，则规划起始点可以插入到第i个点前面？这要求也忒宽松了？只要找到待调整轨迹上第一个与给定规划起始点航向相差±90°以内的点，就将规划起始点插入？同时break跳出循环
if (std::fabs(common::math::AngleDiff(start_point_heading,
tracking_heading)) < kMaxAngleDiff) {
insert_idx = i;
break;
}
}
//如果for循环结束后最后插入下标还为-1，表明找到最后一个都没找到满足条件的点则报错返回
if (insert_idx == -1) {
AERROR << "All points are behind of planning init point";
return false;
}
//定义一条离散轨迹对象cut_trajectory，然后用输入的待调整轨迹去初始化它
DiscretizedTrajectory cut_trajectory(trajectory);
//裁剪掉cut_trajectory上，规划起始点插入下标之前的所有点都裁剪掉
cut_trajectory.erase(cut_trajectory.begin(),
cut_trajectory.begin() + insert_idx);
//在cut_trajectory的最开始插入给定的规划起始点（通常是车辆当前位置？）
cut_trajectory.insert(cut_trajectory.begin(), planning_start_point);
//在轨迹缝合类TrajectoryStitcher中，缝合点也就是规划起始点，相对时间戳应提前一个规划周期0.1s，这样规划起始点的相对时间线就会和当前位置相对时间=0点一样？以下的任何相对时间冲突都应该返回false并且注明原因
//如果最终调整后的轨迹cut_trajectory里轨迹点个数>1 且其第一个点的相对时间反而 >= 第二个点，报错返回，理论上应该是时间升序。
if (cut_trajectory.size() > 1 && cut_trajectory.front().relative_time() >=
cut_trajectory[1].relative_time()) {
AERROR << "planning init point relative_time["
<< cut_trajectory.front().relative_time()
<< "] larger than its next point's relative_time["
<< cut_trajectory[1].relative_time() << "]";
return false;
}
//在轨迹缝合类TrajectoryStitcher中，规划起始点s设置为0，因此需要对其他点进行相应的调整，其实就是轨迹第一个点s为0,后面点的s按照每相邻两个点之间距离累加设置
//初始累计纵向距离0.0m
double accumulated_s = 0.0;
//遍历调整后的轨迹上的每一个路径点，从第2个点也就是缝合点开始，第一个点是规划起始点
for (size_t i = 1; i < cut_trajectory.size(); ++i) {
//调整后的轨迹上第i-1个路径点pre_path_point
const auto& pre_path_point = cut_trajectory[i - 1].path_point();
//调整后的轨迹上第i个路径点指针cur_path_point，这里用指针是为了修改cut_trajectory里的值
auto* cur_path_point = cut_trajectory[i].mutable_path_point();
//累计纵向距离accumulated_s 累加一下第i-1到第i个点的距离
accumulated_s += std::sqrt((cur_path_point->x() - pre_path_point.x()) *
(cur_path_point->x() - pre_path_point.x()) +
(cur_path_point->y() - pre_path_point.y()) *
(cur_path_point->y() - pre_path_point.y()));
//设置第i个点的s为累计距离accumulated_s
cur_path_point->set_s(accumulated_s);
}
//重新插值t使得delta t保持相同？先清空用于存放调整后轨迹结果的指针adjusted_trajectory
adjusted_trajectory->clear();
//使用不同的时间采样精度来减少数据负载但是也提供足够多的数据点给control模块
//密集时间采样周期0.02s
const double kDenseTimeResoltuion = FLAGS_trajectory_time_min_interval;
//稀疏的时间采样周期0.1s
const double kSparseTimeResolution = FLAGS_trajectory_time_max_interval;
//密集和稀疏采样的时间分界点默认1.0s，1.0s以内密集采样，以外稀疏采样
//FLAGS_取出modules\planning\common\planning_gflags.cc中trajectory_time_high_density_period的值
const double kDenseTimeSec = FLAGS_trajectory_time_high_density_period;
//在上一个函数里，CombinePathAndSpeedProfile()路径规划和速度规划融合的轨迹已经是按这种前密后疏的方式采样，那裁剪缝合后只要按照这样方式再采样就好，第一个的相对时间，往后1.0s内每次增量0.02s，往后1.0s后每次增量0.1s，把新组织的相对时间cur_rel_time
for (double cur_rel_time = cut_trajectory.front().relative_time();
cur_rel_time <= cut_trajectory.back().relative_time();
cur_rel_time += (cur_rel_time < kDenseTimeSec ? kDenseTimeResoltuion
: kSparseTimeResolution)) {
//上面调整后的轨迹cut_trajectory根据相对时间cur_rel_time去插值得到的轨迹点塞入
//用于存放最后轨迹调整结果的adjusted_trajectory中
adjusted_trajectory->AppendTrajectoryPoint(
cut_trajectory.Evaluate(cur_rel_time));
}
return true;
}
//设置类成员是否可驾驶is_drivable_，用给定的参数
void ReferenceLineInfo::SetDrivable(bool drivable) { is_drivable_ = drivable; }
//返回类成员是否可驾驶
bool ReferenceLineInfo::IsDrivable() const { return is_drivable_; }
//判断当前这条参考线的是否在全局规划路径routingSegment上，如果在就不是变道路径，不在就是
bool ReferenceLineInfo::IsChangeLanePath() const {
return !Lanes().IsOnSegment();
}
//判断参考线是否隔壁车道路径
bool ReferenceLineInfo::IsNeighborLanePath() const {
return Lanes().IsNeighborSegment();
}
//参考线上路径规划，速度规划debug字符打印
std::string ReferenceLineInfo::PathSpeedDebugString() const {
return absl::StrCat("path_data:", path_data_.DebugString(),
"speed_data:", speed_data_.DebugString());
}
//根据车道转弯类型设置转向灯，都设置到输入的指针vehicle_signal里
void ReferenceLineInfo::SetTurnSignalBasedOnLaneTurnType(
common::VehicleSignal* vehicle_signal) const {
CHECK_NOTNULL(vehicle_signal);
//如果输入参数转向灯信号里已经设置过了且不为TURN_NONE就直接返回
if (vehicle_signal->has_turn_signal() &&
vehicle_signal->turn_signal() != VehicleSignal::TURN_NONE) {
return;
}
//先初始设置一下转向灯信号指针为TURN_NONE
vehicle_signal->set_turn_signal(VehicleSignal::TURN_NONE);
//设置转向信号灯基于变道
if (IsChangeLanePath()) {
//如果先前动作是向左变道，设置左转转向灯，右转设置向右
if (Lanes().PreviousAction() == routing::ChangeLaneType::LEFT) {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_LEFT);
} else if (Lanes().PreviousAction() == routing::ChangeLaneType::RIGHT) {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_RIGHT);
}
return;
}
//设置转向信号灯基于借道
//npos 是一个常数，用来表示不存在的位置
//类成员路径数据path_data_可以找到"left"标签，设置左转信号灯，向右同理
if (path_data_.path_label().find("left") != std::string::npos) {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_LEFT);
return;
}
if (path_data_.path_label().find("right") != std::string::npos) {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_RIGHT);
return;
}
//设置信号灯基于车道的转弯类型
//初始route_s为0
double route_s = 0.0;
//自车的SL边界的end_s，也就是车头的s坐标？adc_s
const double adc_s = adc_sl_boundary_.end_s();
//遍历类成员，车道段，lanes_是RouteSegments类型对象
for (const auto& seg : Lanes()) {
//车道的累计长度 > 自车车头s + 100m,自车车头100m外都没有设置的话就直接break了，太远了，不管，turn_signal_distance去XXX.gflags.cc里取出相应的值
if (route_s > adc_s + FLAGS_turn_signal_distance) {
break;
}
//route_s叠加车道长度
route_s += seg.end_s - seg.start_s;
//如果自车车头已经越过了当前车道终点end_s，直接跳到下一条车道段
if (route_s < adc_s) {
continue;
}
//获取这段车道的转弯类型
const auto& turn = seg.lane->lane().turn();
//左转设置左转向灯，右转同理
if (turn == hdmap::Lane::LEFT_TURN) {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_LEFT);
break;
} else if (turn == hdmap::Lane::RIGHT_TURN) {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_RIGHT);
break;
} else if (turn == hdmap::Lane::U_TURN) {
// 如果获取的这段车道类型为U_TURN的话，还要判断是左是右
auto start_xy =
PointFactory::ToVec2d(seg.lane->GetSmoothPoint(seg.start_s));
auto middle_xy = PointFactory::ToVec2d(
seg.lane->GetSmoothPoint((seg.start_s + seg.end_s) / 2.0));
auto end_xy = PointFactory::ToVec2d(seg.lane->GetSmoothPoint(seg.end_s));
auto start_to_middle = middle_xy - start_xy;
auto start_to_end = end_xy - start_xy;
if (start_to_middle.CrossProd(start_to_end) < 0) {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_RIGHT);
} else {
vehicle_signal->set_turn_signal(VehicleSignal::TURN_LEFT);
}
break;
}
}
}
//设置类成员信号灯vehicle_signal_，设置为给定的的转向信号灯
void ReferenceLineInfo::SetTurnSignal(
const VehicleSignal::TurnSignal& turn_signal) {
vehicle_signal_.set_turn_signal(turn_signal);
}
//设置类成员信号灯vehicle_signal_，设置紧急双闪灯？
void ReferenceLineInfo::SetEmergencyLight() {
vehicle_signal_.set_emergency_light(true);
}
//将类成员的车辆信号灯对象导出到输入参数指针vehicle_signal？
void ReferenceLineInfo::ExportVehicleSignal(
common::VehicleSignal* vehicle_signal) const {
CHECK_NOTNULL(vehicle_signal);
*vehicle_signal = vehicle_signal_;
SetTurnSignalBasedOnLaneTurnType(vehicle_signal);
}
//判断是否到达目标点，到目标点剩余s距离<5cm?是的话就到了
bool ReferenceLineInfo::ReachedDestination() const {
static constexpr double kDestinationDeltaS = 0.05;
const double distance_destination = SDistanceToDestination();
return distance_destination <= kDestinationDeltaS;
}
//计算离目标点剩余的纵向距离
double ReferenceLineInfo::SDistanceToDestination() const {
double res = std::numeric_limits<double>::max();
//首先去类成员路径数据path_data_里获得终点障碍物对象，终点的id通常设为"DEST"
const auto* dest_ptr = path_decision_.Find(FLAGS_destination_obstacle_id);
//如果没找到终点障碍物对象，返回一个很大的数
if (!dest_ptr) {
return res;
}
//如果终点障碍物对象没有纵向的stop决策，也返回一个很大的数
if (!dest_ptr->LongitudinalDecision().has_stop()) {
return res;
}
//如果终点障碍物对象不在参考线的车道上也返回一个很大的数
if (!reference_line_.IsOnLane(dest_ptr->PerceptionBoundingBox().center())) {
return res;
}
//否则的话终点障碍物start_s + 纵向决策stop的停止距离?
const double stop_s = dest_ptr->PerceptionSLBoundary().start_s() +
dest_ptr->LongitudinalDecision().stop().distance_s();
//停止点s - 自车车头s，剩下的就是距离终点的距离
return stop_s - adc_sl_boundary_.end_s();
}
//导出决策信息到输入参数decision_result，planning_context中？
void ReferenceLineInfo::ExportDecision(
DecisionResult* decision_result, PlanningContext* planning_context) const {
MakeDecision(decision_result, planning_context);
//导出车辆信号灯
ExportVehicleSignal(decision_result->mutable_vehicle_signal());
//指针指向输入参数指针decision_result里的main_decision
auto* main_decision = decision_result->mutable_main_decision();
//如果主决策有停止，那么就设置主决策的变道类型,参数为先前动作？
if (main_decision->has_stop()) {
main_decision->mutable_stop()->set_change_lane_type(
Lanes().PreviousAction());
} else if (main_decision->has_cruise()) {//有巡航，先前动作设置?
main_decision->mutable_cruise()->set_change_lane_type(
Lanes().PreviousAction());
}
}
//设置决策结果(放入输入参数decision_result)，输入参数decision_result(决策结果)里的main_decision(主决策)里先默认为cruise巡航，再看是否有stop,设置主任务决策，最后设置障碍物决策，结果填入输入的两个参数指针中decision_result，planning_context
//planning待发布的轨迹里包含DecisionResult信息
void ReferenceLineInfo::MakeDecision(DecisionResult* decision_result,
PlanningContext* planning_context) const {
CHECK_NOTNULL(decision_result);
decision_result->Clear();
// cruise by default
decision_result->mutable_main_decision()->mutable_cruise();
// check stop decision
int error_code = MakeMainStopDecision(decision_result);
if (error_code < 0) {
MakeEStopDecision(decision_result);
}
MakeMainMissionCompleteDecision(decision_result, planning_context);
SetObjectDecisions(decision_result->mutable_object_decision());
}
//主要是判断车辆是否已经完成主任务，已经到达目标点，若是设置输入的decision_result里的main_decision里的mission_complete里的停止点，停止heading等信息，planning_context主要是存放一些场景中间信息。planning待发布的轨迹里包含DecisionResult信息
void ReferenceLineInfo::MakeMainMissionCompleteDecision(
DecisionResult* decision_result, PlanningContext* planning_context) const {
//若main_decision无stop直接返回
if (!decision_result->main_decision().has_stop()) {
return;
}
//不返回的话就需要设置stop了
auto main_stop = decision_result->main_decision().stop();
//若停止的原因码不为 到终点 且不为 靠边停车，就直接返回
if (main_stop.reason_code() != STOP_REASON_DESTINATION &&
main_stop.reason_code() != STOP_REASON_PULL_OVER) {
return;
}
//自车位置路径点adc_pos到取其到停止点的距离 > 5.0m就返回
const auto& adc_pos = adc_planning_point_.path_point();
if (common::util::DistanceXY(adc_pos, main_stop.stop_point()) >
FLAGS_destination_check_distance) {
return;
}
//判断是否到达终点，从而设置decision_result里的main_decision里的mission_complete
//到了就设置为true
auto mission_complete =
decision_result->mutable_main_decision()->mutable_mission_complete();
if (ReachedDestination()) {
planning_context->mutable_planning_status()
->mutable_destination()
->set_has_passed_destination(true);
} else {
mission_complete->mutable_stop_point()->CopyFrom(main_stop.stop_point());
mission_complete->set_stop_heading(main_stop.stop_heading());
}
}
//设置主停止决策，就看path_decision_的障碍物列表里是否有stop决策，有的话设置一下输入的decision_result的main_decision里的stop，planning待发布的轨迹里包含DecisionResult信息
int ReferenceLineInfo::MakeMainStopDecision(
DecisionResult* decision_result) const {
double min_stop_line_s = std::numeric_limits<double>::infinity();
//初始化两指针，停止障碍物对象，停止决策对象
const Obstacle* stop_obstacle = nullptr;
const ObjectStop* stop_decision = nullptr;
//遍历类成员path_decision_路径数据里的障碍物列表
for (const auto* obstacle : path_decision_.obstacles().Items()) {
//获取遍历障碍物的纵向决策
const auto& object_decision = obstacle->LongitudinalDecision();
//如果障碍物决策没有stop,则continue
if (!object_decision.has_stop()) {
continue;
}
//执行到这说明有stop,停止点转SL坐标，放入stop_line_sl
apollo::common::PointENU stop_point = object_decision.stop().stop_point();
common::SLPoint stop_line_sl;
reference_line_.XYToSL(stop_point, &stop_line_sl);
//停止线的s
double stop_line_s = stop_line_sl.s();
//如果停止线s不在参考线的s范围内，报错忽略
if (stop_line_s < 0 || stop_line_s > reference_line_.Length()) {
AERROR << "Ignore object:" << obstacle->Id() << " fence route_s["
<< stop_line_s << "] not in range[0, " << reference_line_.Length()
<< "]";
continue;
}
// check stop_line_s vs adc_s
//检查停止线s 和 自车s
//停止线s<最小停止线s(初始化为无穷大)
//这是要找到参考线上第一个停止线？也就是s最小的一个
if (stop_line_s < min_stop_line_s) {
min_stop_line_s = stop_line_s;
stop_obstacle = obstacle;
stop_decision = &(object_decision.stop());
}
}
//若停止线不为空指针，设置主决策里的stop，停车原因，停止点等
if (stop_obstacle != nullptr) {
MainStop* main_stop =
decision_result->mutable_main_decision()->mutable_stop();
main_stop->set_reason_code(stop_decision->reason_code());
main_stop->set_reason("stop by " + stop_obstacle->Id());
main_stop->mutable_stop_point()->set_x(stop_decision->stop_point().x());
main_stop->mutable_stop_point()->set_y(stop_decision->stop_point().y());
main_stop->set_stop_heading(stop_decision->stop_heading());
ADEBUG << " main stop obstacle id:" << stop_obstacle->Id()
<< " stop_line_s:" << min_stop_line_s << " stop_point: ("
<< stop_decision->stop_point().x() << stop_decision->stop_point().y()
<< " ) stop_heading: " << stop_decision->stop_heading();
return 1;
}
return 0;
}
//设置障碍物决策？针对障碍物的决策都设置到输入参数指针object_decisions障碍物决策列表里
void ReferenceLineInfo::SetObjectDecisions(
ObjectDecisions* object_decisions) const {
//遍历类成员path_decision_里的障碍物列表里的障碍物对象
for (const auto obstacle : path_decision_.obstacles().Items()) {
//如果遍历的第i个障碍物决策是可以忽略，那么就直接跳到下一个循环
if (!obstacle->HasNonIgnoreDecision()) {
continue;
}
//执行到这，说明障碍物是不可忽略的，不然上面就continue了
//在输入参数障碍物决策列表指针里增加一个障碍物决策，用指针object_decision 指向它，然后修改object_decision ，就修改了object_decisions里这个新增的障碍物决策
auto* object_decision = object_decisions->add_decision();
//设置新增的障碍物决策的障碍物id,感知id,以及横纵向决策，从障碍物列表里本身存的障碍物决策里拷贝过来，如过有的话
object_decision->set_id(obstacle->Id());
object_decision->set_perception_id(obstacle->PerceptionId());
if (obstacle->HasLateralDecision() && !obstacle->IsLateralIgnore()) {
object_decision->add_object_decision()->CopyFrom(
obstacle->LateralDecision());
}
if (obstacle->HasLongitudinalDecision() &&
!obstacle->IsLongitudinalIgnore()) {
object_decision->add_object_decision()->CopyFrom(
obstacle->LongitudinalDecision());
}
}
}
//导出参与建议？engage_advice是planning发布轨迹ADCTrajectory中包含的信息，推测是判断规划是否应该介入？
//其实最终结果是存放在输入的参数指针engage_advice中
void ReferenceLineInfo::ExportEngageAdvice(
EngageAdvice* engage_advice, PlanningContext* planning_context) const {
static EngageAdvice prev_advice;
//定义 了个角度偏差阈值30°？
static constexpr double kMaxAngleDiff = M_PI / 6.0;
bool engage = false;
//如果不可驾驶设置prev_advice里设置原因参考线不可驾驶
if (!IsDrivable()) {
prev_advice.set_reason("Reference line not drivable");
} else if (!is_on_reference_line_) { //可驾驶但不在参考线上？
//获取scenario_type，就是输入参数planning_context里的planning_status里的scenario里的scenario_type，场景类型？
const auto& scenario_type =
planning_context->planning_status().scenario().scenario_type();
//如果场景类型是PARK_AND_GO(起步) 或者 是变道路径
if (scenario_type == ScenarioConfig::PARK_AND_GO || IsChangeLanePath()) {
//参与建议设置为true
engage = true;
} else { //否则的话因为自车不在参考线上，参与建议engage defaut为false,并设置原因
prev_advice.set_reason("Not on reference line");
}
} else { //否则的话，又可驾驶又在参考线上
// 检查自车航向角
//根据自车车头s去参考线上获取参考点ref_point
auto ref_point =
reference_line_.GetReferencePoint(adc_sl_boundary_.end_s());
//如果车辆的实际heading与参考点的heading相差不超过30°的话，设置参与建议为true
if (common::math::AngleDiff(vehicle_state_.heading(), ref_point.heading()) <
kMaxAngleDiff) {
engage = true;
} else { //若航向角已经偏离30
prev_advice.set_reason("Vehicle heading is not aligned");
}
}
//如果参与建议为true
if (engage) {
//如果车辆不在自驾模式
if (vehicle_state_.driving_mode() !=
Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) {
//设置参与建议为READY_TO_ENGAGE，准备参与已就绪
prev_advice.set_advice(EngageAdvice::READY_TO_ENGAGE);
} else {
//若在自驾状态，参与建议prev_advice就保持参与KEEP_ENGAGED
prev_advice.set_advice(EngageAdvice::KEEP_ENGAGED);
}
//清除之前存放的不参与的原因
prev_advice.clear_reason();
} else {//否则的话就若上一次参与建议不为DISALLOW_ENGAGE，不允许参与，就设置为准备参与
if (prev_advice.advice() != EngageAdvice::DISALLOW_ENGAGE) {
prev_advice.set_advice(EngageAdvice::PREPARE_DISENGAGE);
}
}
//输入的参数engage_advice指针就将上面的设置结果拷贝出来
//其实最终结果是存放在输入的这个参数指针中
engage_advice->CopyFrom(prev_advice);
}
//作出急停决策，结果也是存放到输入参数指针decision_result
void ReferenceLineInfo::MakeEStopDecision(
DecisionResult* decision_result) const {
//先清空
decision_result->Clear();
//先用一个指针main_estop指向输入参数decision_result里的main_decision里的estop
MainEmergencyStop* main_estop =
decision_result->mutable_main_decision()->mutable_estop();
//设置main_estop的原因码为ESTOP_REASON_INTERNAL_ERR，内部错误急停
main_estop->set_reason_code(MainEmergencyStop::ESTOP_REASON_INTERNAL_ERR);
//设置main_estop的原因
main_estop->set_reason("estop reason to be added");
//设置main_estop的巡航到stop
main_estop->mutable_cruise_to_stop();
// 设置障碍物决策 为decision_result里的object_decision目标决策
ObjectDecisions* object_decisions =
decision_result->mutable_object_decision();
//遍历障碍物列表
for (const auto obstacle : path_decision_.obstacles().Items()) {
//障碍物决策列表增加一个决策，把障碍物列表里的决策，id都拷贝到object_decisions？
auto* object_decision = object_decisions->add_decision();
object_decision->set_id(obstacle->Id());
object_decision->set_perception_id(obstacle->PerceptionId());
object_decision->add_object_decision()->mutable_avoid();
}
}
//返回路径的转弯类型，输入参数是纵向s坐标，指定位置s处的转弯类型
hdmap::Lane::LaneTurn ReferenceLineInfo::GetPathTurnType(const double s) const {
//向前的一个缓冲/提前量 20.0m
const double forward_buffer = 20.0;
//车道起点的s，从参考线第一条车道也就是0开始
double route_s = 0.0;
//遍历参考线上的所有车道
for (const auto& seg : Lanes()) {
//如果当前遍历车道起点>给定s+20.0m，太远了，直接break
if (route_s > s + forward_buffer) {
break;
}
//累加车道长度到下一段车道起点的s route_s
route_s += seg.end_s - seg.start_s;
//如果当前遍历的车道起点route_s < 给定s直接跳到下一段车道，直到找到一个车道起点刚好大于给定s，但又没超过20m远
if (route_s < s) {
continue;
}
//如果上面没有continued的话，设置转弯类型为当前遍历的这段车道的转弯类型 左/右/掉头并返回
const auto& turn_type = seg.lane->lane().turn();
if (turn_type == hdmap::Lane::LEFT_TURN ||
turn_type == hdmap::Lane::RIGHT_TURN ||
turn_type == hdmap::Lane::U_TURN) {
return turn_type;
}
}
//如果上面都没有返回的话，那么就返回无转弯类型 NO_TURN
return hdmap::Lane::NO_TURN;
}
//获取路口路权 输入参数是hdmap的pnc_junction_overlap PNC路口overlap
bool ReferenceLineInfo::GetIntersectionRightofWayStatus(
const hdmap::PathOverlap& pnc_junction_overlap) const {
//获取路口overlap的statr_s所处的车道的转弯类型，若不等于NO_TURN就返回false
//false是代表没有路权，就是路口不直行就没路权？
if (GetPathTurnType(pnc_junction_overlap.start_s) != hdmap::Lane::NO_TURN) {
return false;
}
//否则就返回true，代表有路权?
// TODO(all): iterate exits of intersection to check/compare speed-limit
return true;
}
//获取参考线上指定纵向位置s处的pnc路口overlap，存放到输入参数指针pnc_junction_overlap
int ReferenceLineInfo::GetPnCJunction(
const double s, hdmap::PathOverlap* pnc_junction_overlap) const {
CHECK_NOTNULL(pnc_junction_overlap);
//获取参考线地图路径上所有pnc路口overlap，存放到pnc_junction_overlaps
const std::vector<hdmap::PathOverlap>& pnc_junction_overlaps =
reference_line_.map_path().pnc_junction_overlaps();
//设置一个常数允许误差1.0m
static constexpr double kError = 1.0; // meter
//遍历参考线上所有的pnc路口overlap
for (const auto& overlap : pnc_junction_overlaps) {
//如果给定s在当前遍历的pnc路口±1.0m范围内，就将该pnc路口overlap放入输入参数指针pnc_junction_overlap
if (s >= overlap.start_s - kError && s <= overlap.end_s + kError) {
*pnc_junction_overlap = overlap;
return 1;
}
}
return 0;
}
//其实就是根据给定的阻塞障碍物的id去类成员path_decision_的障碍物列表里找到对应的障碍物对象拷贝给ReferenceLineInfo类成员blocking_obstacle_
void ReferenceLineInfo::SetBlockingObstacle(
const std::string& blocking_obstacle_id) {
blocking_obstacle_ = path_decision_.Find(blocking_obstacle_id);
}
//获得所有的停止决策的停止点的SL坐标？存入输入参数result中，其是一个SL坐标点的vector
std::vector<common::SLPoint> ReferenceLineInfo::GetAllStopDecisionSLPoint()
const {
std::vector<common::SLPoint> result;
//遍历类成员path_decision_里的障碍物列表
for (const auto* obstacle : path_decision_.obstacles().Items()) {
//如果该障碍物的决策纵向决策没有stop就直接continue
const auto& object_decision = obstacle->LongitudinalDecision();
if (!object_decision.has_stop()) {
continue;
}
//执行到这里肯定是纵向决策有stop的障碍物，获取停止点stop_point
apollo::common::PointENU stop_point = object_decision.stop().stop_point();
common::SLPoint stop_line_sl;
//将针对该障碍物的停止点转化为SL坐标放入stop_line_sl
reference_line_.XYToSL(stop_point, &stop_line_sl);
//如果停止点在纵向上的位置不在参考线范围内就continue
if (stop_line_sl.s() <= 0 || stop_line_sl.s() >= reference_line_.Length()) {
continue;
}
//将停止线的sl坐标塞入输入参数result中
result.push_back(stop_line_sl);
}
// sort by s
//将result中的停止点按s从小到大升序排列
if (!result.empty()) {
std::sort(result.begin(), result.end(),
[](const common::SLPoint& a, const common::SLPoint& b) {
return a.s() < b.s();
});
}
//返回这个停止点SL坐标vector
return result;
}
} // namespace planning
} // namespace apollo
附录
overlap概念
Overlap在注释里的解释是“任何一对在地图上重合的东西，包括（车道，路口，人行横道）”，比如路口的人行横道和道路是重叠的，还有一些交通标志和道路也是重叠的，这是根据认知逻辑创造的概念。
自车SL边界 adc_sl_boundary
排序的写法
if (!first_encounter_overlaps_.empty()) {
std::sort(first_encounter_overlaps_.begin(),
first_encounter_overlaps_.end(),
[](const std::pair<OverlapType, hdmap::PathOverlap>& a,
const std::pair<OverlapType, hdmap::PathOverlap>& b) {
return a.second.start_s < b.second.start_s;
});
sort的前两个参数表示范围，这里是从begin到end
sort的第三个参数是个函数，用于比较相邻两个元素
[](const std::pair<OverlapType, hdmap::PathOverlap>& a,
const std::pair<OverlapType, hdmap::PathOverlap>& b) {
return a.second.start_s < b.second.start_s;
}
这里代表返回较大的那一个，作为sort的参数表示升序排列
为开发者提供学习成长、分享交流、生态实践、资源工具等服务，帮助开发者快速成长。
更多推荐
9
0- 0
已为社区贡献4条内容
所有评论(0)