概述
Apollo\modules\planning\common\FeatureOutput类是关于基于学习的决策,暂时跳过,学完传统的规划模块再看。
Frame类是apollo planning模块下modules\planning\common\frame.cc/.h实现
通过frame_test.cc给出的示例,可以了解该类的功能及使用方式。
从类名来看,应该是数据帧类,存放规划一个周期的所有输入输出数据,以及一些中间量的查询设置。
从代码来看Frame类主要是实现:
frame.h
#pragma once
#include <list>
#include <map>
#include <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>
#include "modules/common/math/vec2d.h"
#include "modules/common/monitor_log/monitor_log_buffer.h"
#include "modules/common/proto/geometry.pb.h"
#include "modules/common/status/status.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/localization/proto/pose.pb.h"
#include "modules/planning/common/ego_info.h"
#include "modules/planning/common/indexed_queue.h"
#include "modules/planning/common/local_view.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/open_space_info.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/trajectory/publishable_trajectory.h"
#include "modules/planning/proto/pad_msg.pb.h"
#include "modules/planning/proto/planning.pb.h"
#include "modules/planning/proto/planning_config.pb.h"
#include "modules/planning/proto/planning_internal.pb.h"
#include "modules/planning/reference_line/reference_line_provider.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
#include "modules/routing/proto/routing.pb.h"
namespace apollo {
namespace planning {
/**
* @class Frame
*
* @brief Frame类储存了一个规划周期的所有数据
*/
class Frame {
public:
//Frame类的构造函数,参数是序列号
explicit Frame(uint32_t sequence_num);
//Frame类的带参构造函数,
//参数:序列号sequence_num
//参数:local_view modules\planning\common\local_view.h定义的LocalView类
//LocalView类包含了规划模块输入所需的所有数据
//参数:planning_start_point规划轨迹起始点
//参数:车辆状态vehicle_state
//参数:reference_line_provider参考线提供器
Frame(uint32_t sequence_num, const LocalView &local_view,
const common::TrajectoryPoint &planning_start_point,
const common::VehicleState &vehicle_state,
ReferenceLineProvider *reference_line_provider);
//Frame类带参构造函数,跟上面的构造函数区别就是没有最后一个参数参考线提供器
//但其实调用的还是上面的构造函数,只不过最后一个参数传个nullptr空指针
Frame(uint32_t sequence_num, const LocalView &local_view,
const common::TrajectoryPoint &planning_start_point,
const common::VehicleState &vehicle_state);
//析构函数
virtual ~Frame() = default;
//获取Frame类的数据成员规划起始点planning_start_point_
const common::TrajectoryPoint &PlanningStartPoint() const;
//Frame类的初始化函数
//输入参数 车辆状态提供器类指针对象vehicle_state_provider
//输入参数 参考线类对象列表reference_lines
//输入参数 导航路径段类对象列表segments
//输入参数: 将来的导航的车道航点序列future_route_waypoints
//输入参数: 自车信息类指针对象ego_info
//其实该函数主要就是用输入参数去初始化Frame类的数据成员
common::Status Init(
const common::VehicleStateProvider *vehicle_state_provider,
const std::list<ReferenceLine> &reference_lines,
const std::list<hdmap::RouteSegments> &segments,
const std::vector<routing::LaneWaypoint> &future_route_waypoints,
const EgoInfo *ego_info);
//针对OpenSpace的规划器的初始化
//相比上面的Init函数少了结构化的车道,以及车道上的segments的相关的操作
common::Status InitForOpenSpace(
const common::VehicleStateProvider *vehicle_state_provider,
const EgoInfo *ego_info);
//返回Frame类数据成员序列号,这个序列号是干什么的?
uint32_t SequenceNum() const;
//获取debug string打印序列号字符串,所以序列号是类似于id一样的识别号代表特定一帧frame
//数据?
std::string DebugString() const;
//返回待发布的规划轨迹?
const PublishableTrajectory &ComputedTrajectory() const;
//其实这个函数就记录planning的所有输入数据用于debug
//这些输入数据几乎都是从Frame类成员local_view_里拷贝的
//planning_internal::Debug 是modules\planning\proto\planning_internal.proto
//里定义的message类,Debug下面包含planning_data
//planning_data里又包含规划的相关数据,如自车位置,底盘反馈,routing响应,
//起始点,路径,速度规划,st图,sl坐标,障碍物,信号灯,前方畅通距离,场景信息等等,几
//乎所有的planning输入debug数据?
//google protobuf的用法,可以根据.proto文件生成相应的c++类,视为一种数据结构
void RecordInputDebug(planning_internal::Debug *debug);
//获取Frame类成员参考线信息类对象reference_line_info_,它是一个参考线信息的列表list
const std::list<ReferenceLineInfo> &reference_line_info() const;
//返回Frame类成员参考线信息类对象reference_line_info_的地址?
std::list<ReferenceLineInfo> *mutable_reference_line_info();
//在障碍物列表中寻找输入参数id对应的障碍物对象
Obstacle *Find(const std::string &id);
//找驾驶参考线信息
//其实这个函数就是找到Frame类数据成员reference_line_info_道路参考线列表中cost最小且可行驶的道路参考线对象
const ReferenceLineInfo *FindDriveReferenceLineInfo();
//找目标道路参考线,返回的是道路参考线信息ReferenceLineInfo类对象
//这个函数其实是找变道参考线?
const ReferenceLineInfo *FindTargetReferenceLineInfo();
//查找失败的参考线信息,返回道路参考线信息类ReferenceLineInfo类对象
//遍历道路参考线列表,找到不可驾驶的变道参考线信息类对象?
const ReferenceLineInfo *FindFailedReferenceLineInfo();
//返回Frame类的数据成员驾驶参考线信息类对象
const ReferenceLineInfo *DriveReferenceLineInfo() const;
//返回Frame类的数据成员障碍物列表obstacles_
const std::vector<const Obstacle *> obstacles() const;
/**
* 用车道的宽度创建虚拟的静态障碍物,主要是针对虚拟的停止墙
*/
//参数 参考线信息类对象reference_line_info,这里不是列表了,只是一条参考线信息
//参数 障碍物obstacle_id,引用变量&应该是用来存放构建的虚拟障碍物的Id
//参数 障碍物id
//参数 障碍物对应的frenet系的s坐标
//这个函数主要是针对虚拟静态障碍物的位置建立虚拟障碍物对象,如红绿灯停止线等
const Obstacle *CreateStopObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_s);
/**
* 用车道宽度创建虚拟静态障碍物对象,主要是用于虚拟停止墙
*/
//参数 障碍物id,车道id,车道对应的终点的frenet系s坐标lane_s
//这个函数主要是针对终点位置建立虚拟障碍物对象
const Obstacle *CreateStopObstacle(const std::string &obstacle_id,
const std::string &lane_id,
const double lane_s);
/**
* 用车道宽度创建虚拟静态障碍物对象
*/
//这个函数主要是根据3个输入参数:障碍物frenet系下的起始s坐标和终点s坐标,以及障碍物的
//id,在参考线信息类对象上构建虚拟障碍物
const Obstacle *CreateStaticObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_start_s,
const double obstacle_end_s);
//重新规划全局路径函数,输入参数是planning_context就是规划状态
//就是从自车位置出发再次routing,主要是更新routing_request的起始点为自车位置
//PlanningContext里包含routing_request
//这里找到自车最近的匹配车道上的最近点作为routing_request的起始点重新routing
//来设置PlanningContext里的rerouting对象
bool Rerouting(PlanningContext *planning_context);
//返回车辆自身状态
const common::VehicleState &vehicle_state() const;
//这个函数是是实现预测时间对齐planning起始时间?
//输入参数 规划起始时间planning_start_time
//输入参数 预测障碍物列表prediction_obstacles
static void AlignPredictionTime(
const double planning_start_time,
prediction::PredictionObstacles *prediction_obstacles);
//设置当前frame帧的规划轨迹,用ADCTrajectory类轨迹对象
void set_current_frame_planned_trajectory(
ADCTrajectory current_frame_planned_trajectory) {
current_frame_planned_trajectory_ =
std::move(current_frame_planned_trajectory);
}
//返回当前frame的规划轨迹ADCTrajectory类对象
const ADCTrajectory ¤t_frame_planned_trajectory() const {
return current_frame_planned_trajectory_;
}
//设置当前frame的离散路径 轨迹trajectory=路径path + 速度规划
void set_current_frame_planned_path(
DiscretizedPath current_frame_planned_path) {
current_frame_planned_path_ = std::move(current_frame_planned_path);
}
//返回当前frame的离散路径对象
const DiscretizedPath ¤t_frame_planned_path() const {
return current_frame_planned_path_;
}
//离目标点是否足够近?
const bool is_near_destination() const {
return is_near_destination_;
}
/**
* @brief 调整参考线优先级通过实际的道路情况
* @id_to_priority laneid和优先级的map映射关系
*/
//更新参考线优先级?输入参数是一个map,看上去像是存放一系列id和优先级数字的映射关系的
//map id_to_priority,&引用变量做参数通常代表其要被函数修改
//其实就是将 输入参数里map各个参考线id对应的优先级数字设置到
//设置到类成员reference_line_info_参考线对象列表里
void UpdateReferenceLinePriority(
const std::map<std::string, uint32_t> &id_to_priority);
//返回frame类的数据成员local_view_对象,其包含了所有的规划输入数据,是一个struct
//障碍物/底盘/交通灯/定位等信息
const LocalView &local_view() const {
return local_view_;
}
//获取Frame类数据成员障碍物列表
ThreadSafeIndexedObstacles *GetObstacleList() {
return &obstacles_;
}
//返回开放空间信息类对象?
const OpenSpaceInfo &open_space_info() const {
return open_space_info_;
}
//设置开放空间信息类对象?
OpenSpaceInfo *mutable_open_space_info() {
return &open_space_info_;
}
//根据id获取交通灯对象,输入的是交通灯的id
perception::TrafficLight GetSignal(const std::string &traffic_light_id) const;
//获取frame类的数据成员存放人机交互动作对象?
const DrivingAction &GetPadMsgDrivingAction() const {
return pad_msg_driving_action_;
}
private:
//函数名其实意思就是初始化Frame类的数据
//输入参数 车辆状态提供器类指针对象vehicle_state_provider
//输入参数: 自车信息类指针对象ego_info
common::Status InitFrameData(
const common::VehicleStateProvider *vehicle_state_provider,
const EgoInfo *ego_info);
//创建参考线信息类对象?
//参数 参考线类对象列表reference_lines
//参数 导航路径段类对象列表segments 导航route指全局路径规划
//其实这个函数就是将参数里的参考线列表reference_lines,导航路径段RouteSegments列表设
//置到Frame类数据成员reference_line_info_参考线信息类对象列表里
bool CreateReferenceLineInfo(const std::list<ReferenceLine> &reference_lines,
const std::list<hdmap::RouteSegments> &segments);
/**
* 找到一个会与ADC(Autonomous Driving Car)自车碰撞的障碍物
* @return 返回障碍物的指针,否则返回空
*/
const Obstacle *FindCollisionObstacle(const EgoInfo *ego_info) const;
//根据障碍物id和障碍物对应的边界盒对象作为参数去调用构建虚拟障碍物的函数,
//返回一个虚拟障碍物对象类
const Obstacle *CreateStaticVirtualObstacle(const std::string &id,
const common::math::Box2d &box);
//增加一个障碍物对象到类数据成员障碍物列表里
void AddObstacle(const Obstacle &obstacle);
//读交通灯函数,从数据成员local_view_读到Frame类数据成员traffic_lights_交通灯列表中
//local_view_包含规划的所有输入数据
void ReadTrafficLights();
//读取local_view_中的驾驶员交互操作行为
void ReadPadMsgDrivingAction();
//复位人机交互动作对象
void ResetPadMsgDrivingAction();
private: //Frame类的数据成员
//人机交互动作,自驾模式选择等...
static DrivingAction pad_msg_driving_action_;
//序列号?
uint32_t sequence_num_ = 0;
//LocalView类对象,包含规划所有输入数据
LocalView local_view_;
//高精度地图对象,初始化为空指针
const hdmap::HDMap *hdmap_ = nullptr;
//规划起始点
common::TrajectoryPoint planning_start_point_;
//车辆状态
common::VehicleState vehicle_state_;
//参考线列表
std::list<ReferenceLineInfo> reference_line_info_;
//是否接近目标点?
bool is_near_destination_ = false;
/**
* 车辆最终选择驾驶的道路参考线
**/
const ReferenceLineInfo *drive_reference_line_info_ = nullptr;
//障碍物列表
ThreadSafeIndexedObstacles obstacles_;
//无序map,存放id,和交通灯对象的映射关系
std::unordered_map<std::string, const perception::TrafficLight *>
traffic_lights_;
// 当前帧的规划发布轨迹,也就是输出结果
ADCTrajectory current_frame_planned_trajectory_;
//当前帧的路径点,为了将来可能的speed fallback速度规划失败?
DiscretizedPath current_frame_planned_path_;
//参考线提供器类对象
const ReferenceLineProvider *reference_line_provider_ = nullptr;
//开放空间信息类对象?非结构化道路?
OpenSpaceInfo open_space_info_;
//将来的路由点vector,用于rerouting时将自车在轨迹上最近点与之拼接
//用于将之前设定的routing请求改为从自车位置出发
std::vector<routing::LaneWaypoint> future_route_waypoints_;
//监视缓存?
common::monitor::MonitorLogBuffer monitor_logger_buffer_;
};
class FrameHistory : public IndexedQueue<uint32_t, Frame> {
public:
//frame.h里定义了一个FrameHistory类也定义了一个Frame
//可以将FrameHistory类看成是历史Frame数据的队列
//FLAGS_就是GFLAGS用法去
//modules\planning\common\planning_gflags.cc里取出
//储存的最大历史Frame的数量,默认为1
FrameHistory();
};
} // namespace planning
} // namespace apollo
frame.cc
#include "modules/planning/common/frame.h"
#include <algorithm>
#include <limits>
#include "absl/strings/str_cat.h"
#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/point_factory.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/pnc_map/path.h"
#include "modules/map/pnc_map/pnc_map.h"
#include "modules/planning/common/feature_output.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/util/util.h"
#include "modules/planning/reference_line/reference_line_provider.h"
#include "modules/routing/proto/routing.pb.h"
namespace apollo {
namespace planning {
//由modules\common\proto\error_code.proto定义的类 google protobuf用法
using apollo::common::ErrorCode;
//modules\common\status\status.h定义的Status类
//表示API调用返回状态的通用类。信息技术
//可以是成功的OK状态,也可以是带有错误消息和错误代码enum的failure状态。
using apollo::common::Status;
//modules\common\math\box2d.h定义的Box2d的二维边界盒类
using apollo::common::math::Box2d;
//modules\common\math\polygon2d.h定义的二维多边形Polygon2d类
using apollo::common::math::Polygon2d;
//可用于获取当前时间戳的单态时钟。源可以是系统(网络)时钟或模拟时钟。
//模拟时钟主要用于测试目的。
using apollo::cyber::Clock;
//modules\prediction\proto\prediction_obstacle.proto定义的message类
//预测的障碍物,google protobuf用法
using apollo::prediction::PredictionObstacles;
//modules\control\proto\pad_msg.proto定义的驾驶动作
//定义类成员驾驶员交互动作pad_msg_driving_action_为NONE
DrivingAction Frame::pad_msg_driving_action_ = DrivingAction::NONE;
//frame.h里定义了一个FrameHistory类也定义了一个Frame
//可以将FrameHistory类看成是历史Frame数据的队列
//FLAGS_就是GFLAGS用法去
//modules\planning\common\planning_gflags.cc里取出
//储存的最大历史Frame的数量,默认为1
FrameHistory::FrameHistory()
: IndexedQueue<uint32_t, Frame>(FLAGS_max_frame_history_num) {}
//Frame类的构造函数,参数是序列号
Frame::Frame(uint32_t sequence_num)
: sequence_num_(sequence_num), //用参数sequence_num去初始化Frame类成员
//modules\common\monitor_log\proto\monitor_log.proto定义的message类MonitorMessageItem,监视的msg模块是planning
monitor_logger_buffer_(common::monitor::MonitorMessageItem::PLANNING) {}
//Frame类的带参构造函数,
//参数:序列号sequence_num
//参数:local_view modules\planning\common\local_view.h定义的LocalView类
//LocalView类包含了规划模块输入所需的所有数据
//参数:planning_start_point规划轨迹起始点
//参数:车辆状态vehicle_state
//参数:reference_line_provider参考线提供器
Frame::Frame(uint32_t sequence_num, const LocalView &local_view,
const common::TrajectoryPoint &planning_start_point,
const common::VehicleState &vehicle_state,
ReferenceLineProvider *reference_line_provider)
: sequence_num_(sequence_num), //用参数初始化类成员
local_view_(local_view),//用参数初始化类成员
planning_start_point_(planning_start_point),//用参数初始化类成员
vehicle_state_(vehicle_state),//用参数初始化类成员
reference_line_provider_(reference_line_provider),//用参数初始化类成员
//表明监控的是planning模块
monitor_logger_buffer_(common::monitor::MonitorMessageItem::PLANNING) {}
//Frame类带参构造函数,跟上面的构造函数区别就是没有最后一个参数参考线提供器
//但其实调用的还是上面的构造函数,只不过最后一个参数传个nullptr空指针
Frame::Frame(uint32_t sequence_num, const LocalView &local_view,
const common::TrajectoryPoint &planning_start_point,
const common::VehicleState &vehicle_state)
: Frame(sequence_num, local_view, planning_start_point, vehicle_state,
nullptr) {}
//获取Frame类的数据成员规划起始点planning_start_point_
const common::TrajectoryPoint &Frame::PlanningStartPoint() const {
return planning_start_point_;
}
//获取Frame类的数据成员车辆状态vehicle_state_
const common::VehicleState &Frame::vehicle_state() const {
return vehicle_state_;
}
//重新规划全局路径函数,输入参数是planning_context就是规划状态
//就是从自车位置出发再次routing,主要是更新routing_request的起始点为自车位置
//PlanningContext里包含routing_request
//这里找到自车最近的匹配车道上的最近点作为routing_request的起始点重新routing
//来设置PlanningContext里的rerouting对象
bool Frame::Rerouting(PlanningContext *planning_context) {
//导航模式下不支持rerouting,默认不是导航模式
if (FLAGS_use_navigation_mode) {
AERROR << "Rerouting not supported in navigation mode";
return false;
}
//如果local_view_里的routing结果里是空指针的话报错,说明没有先前的可用的routing
if (local_view_.routing == nullptr) {
AERROR << "No previous routing available";
return false;
}
//如果hdmap_也就是空指针则报错返回
if (!hdmap_) {
AERROR << "Invalid HD Map.";
return false;
}
//planning里LocalView结构体里的routing是routing::RoutingResponse类的对象
//LocalView结构体里的routing就包含成员routing_request全局路径规划请求
//获取规划模块的输入里的全局路径规划请求放入request
auto request = local_view_.routing->routing_request();
//先清空全局路径规划请求里的header就是包含了一些时间戳等信息
request.clear_header();
//用车辆状态定义一个点工厂类东北天坐标系下的坐标点point
auto point = common::util::PointFactory::ToPointENU(vehicle_state_);
//先定义个s,l为0就是frenet系下的横纵坐标sl
double s = 0.0;
double l = 0.0;
//LaneInfoConstPtr指向modules\map\hdmap\hdmap_common.h定义的
//LaneInfo类指针对象,LaneInfo类储存了车道上的各种信息,lane id,所属road id
//lane上的散点序列,根据s插值heading,curvature,还有各种相关的路口,交通灯等信息
hdmap::LaneInfoConstPtr lane;
//这一块调用函数GetNearestLaneWithHeading()用车辆当前heading角获取距当前自车最近的
//车道,获取到的最近车道的相关信息放入lane,s,l中,这个函数属于其他模块暂不深究实现
//参数point
//参数5.0 距当前自车5.0m半径范围内寻找最近车道
//参数vehicle_state_.heading() 用当前自车的heading角去匹配车道heading
//参数M_PI / 3.0,匹配上的最近车道的heading与自车的heading角相差不能超过pi/3
//参数 引用变量lane s l用来存放找到的最近车道对象,车道上最近的s,l
//这个函数属于hdmap_的成员函数,hdmap_属于高精度地图类对象,存放地图信息
if (hdmap_->GetNearestLaneWithHeading(point, 5.0, vehicle_state_.heading(),
M_PI / 3.0, &lane, &s, &l) != 0) {
AERROR << "Failed to find nearest lane from map at position: "
<< point.DebugString() << ", heading:" << vehicle_state_.heading();
return false;
}
//清空routing request里的点
request.clear_waypoint();
//request里的新增第一个点add_waypoint
//定义一个起始点指针start_point指向这个新增点
auto *start_point = request.add_waypoint();
//设定起始点所属车道的id为上面匹配到的最近的车道的id
start_point->set_id(lane->id().id());
//设定起始点对应的s为匹配到最近车道上的最近点的s
start_point->set_s(s);
//设定起始点的XYZ坐标从point就是自车状态的位置点XYZ拷贝
start_point->mutable_pose()->CopyFrom(point);
//遍历将来的route路由车道航点集,将其塞入routing_request中
for (const auto &waypoint : future_route_waypoints_) {
// reference_line_provider_->FutureRouteWaypoints()) {
request.add_waypoint()->CopyFrom(waypoint);
}
//如果将来的航点个数小于等于1报错
if (request.waypoint_size() <= 1) {
AERROR << "Failed to find future waypoints";
return false;
}
//让指针rerouting指向规划状态planning_context里的
//成员planning_status里的rerouting重新导航请求
auto *rerouting =
planning_context->mutable_planning_status()->mutable_rerouting();
//设置rerouting重新导航请求里需要重新导航为true
rerouting->set_need_rerouting(true);
//设置rerouting重新导航请求里需要重新导航为true
*rerouting->mutable_routing_request() = request;
//打印一条log信息
monitor_logger_buffer_.INFO("Planning send Rerouting request");
//返回true
return true;
}
//获取Frame类成员参考线信息类对象reference_line_info_,它是一个参考线信息的列表list
const std::list<ReferenceLineInfo> &Frame::reference_line_info() const {
return reference_line_info_;
}
//返回Frame类成员参考线信息类对象reference_line_info_的地址?
std::list<ReferenceLineInfo> *Frame::mutable_reference_line_info() {
return &reference_line_info_;
}
//更新参考线优先级?输入参数是一个map,看上去像是存放一系列id和优先级数字的映射关系的
//map id_to_priority,&引用变量做参数通常代表其要被函数修改
//其实就是将 输入参数里map各个参考线id对应的优先级数字设置到
//设置到类成员reference_line_info_参考线对象列表里
void Frame::UpdateReferenceLinePriority(
const std::map<std::string, uint32_t> &id_to_priority) {
//遍历id,优先级的这张map里面的每一对数据pair
for (const auto &pair : id_to_priority) {
//获取这对数据pair里的id和优先级数字priority
const auto id = pair.first;
const auto priority = pair.second;
//找到这个id对应的参考线信息对象,遍历所有的参考线信息对象,find_if返回id相等的那
//一个放入ref_line_info_itr,就是pair这对数据对应的参考线信息类对象
auto ref_line_info_itr =
std::find_if(reference_line_info_.begin(), reference_line_info_.end(),
[&id](const ReferenceLineInfo &ref_line_info) {
return ref_line_info.Lanes().Id() == id;
});
//上面如果没找到id匹配的参考线信息类对象,那么ref_line_info_itr就会被赋值为参考线
//信息类对象列表里的最后一个,反之,若其为最后一个,说明没找到id匹配的
if (ref_line_info_itr != reference_line_info_.end()) {
//如果找到了匹配的id对应的参考线信息类对象
//设定其优先级为参数map里对应的优先级数字
ref_line_info_itr->SetPriority(priority);
}
}
}
//创建参考线信息类对象?
//参数 参考线类对象列表reference_lines
//参数 导航路径段类对象列表segments 导航route指全局路径规划
//其实这个函数就是将参数里的参考线列表,导航路径段列表设置
//到Frame类数据成员reference_line_info_参考线信息类对象列表里
bool Frame::CreateReferenceLineInfo(
const std::list<ReferenceLine> &reference_lines,
const std::list<hdmap::RouteSegments> &segments) {
//首先清空数据成员参考线信息类列表reference_line_info_
reference_line_info_.clear();
//获取参考线列表里的第一个对象ref_line_iter ,准备参与迭代
auto ref_line_iter = reference_lines.begin();
//获取导航路径段列表里的第一个对象segments_iter ,准备参与迭代
auto segments_iter = segments.begin();
//遍历每一条参考线列表里的每一条参考线
//这个while循环的作用就是把参数里的参考线列表以及导航路径段列表用来构造参考线信息类对
//象,挨个塞入Frame类数据成员reference_line_info_参考线信息类对象列表
while (ref_line_iter != reference_lines.end()) {
//若导航路径段因为到达终点附件而停止,
//设置数据成员标志位is_near_destination_ 足够靠近终点为true
if (segments_iter->StopForDestination()) {
is_near_destination_ = true;
}
//将当前车辆状态,规划起始点,当前遍历的参考线对象,当前遍历的导航路径段对象一起塞
//入类的数据成员reference_line_info_参考线信息类对象列表里
reference_line_info_.emplace_back(vehicle_state_, planning_start_point_,
*ref_line_iter, *segments_iter);
//迭代器的参考线对象指向列表里的下一个
++ref_line_iter;
//迭代器导航路径段对象指向列表里的下一个
++segments_iter;
}
//若Frame类数据成员参考线信息类对象列表里元素个数为2
if (reference_line_info_.size() == 2) {
//首先将车辆当前位置点xy构建一个Vec2d二维向量就是点坐标
common::math::Vec2d xy_point(vehicle_state_.x(), vehicle_state_.y());
//定义一个空的frenet系坐标点first_sl (s,l)
common::SLPoint first_sl;
//首先获取参考线信息类对象列表里的第一条参考线,将车辆当前位置点坐标xy_point
//投影到列表reference_line_info_里第一条参考线的frenet系下,投影的sl坐标
//放入first_sl
//自车XY坐标转列表第一条参考线下的frenet坐标,结果放入first_sl
if (!reference_line_info_.front().reference_line().XYToSL(xy_point,
&first_sl)) {
return false;
}
//定义一个空的frenet系坐标点second_sl (s,l)
common::SLPoint second_sl;
//获取参考线信息类对象列表里的最后一条参考线,将车辆当前位置点坐标xy_point
//投影到列表reference_line_info_里最后一条参考线的frenet系下,投影的sl坐标
//放入second_sl
//自车XY坐标转列表最后一条参考线下的frenet坐标,结果放入second_sl
//这个是在参考线信息类对象列表里元素为2,所以参考线其实就2条?
if (!reference_line_info_.back().reference_line().XYToSL(xy_point,
&second_sl)) {
return false;
}
//计算自车坐标投影到第一条参考线和投影最后一条参考线的frenet系下的l坐标的偏差
//这个是在参考线信息类对象列表里元素为2,所以参考线其实就2条?
const double offset = first_sl.l() - second_sl.l();
//设置ReferenceLineInfo类对象列表reference_line_info_的数据成员
//这个offset其实就是这两条参考线在横向上的相对偏移?
reference_line_info_.front().SetOffsetToOtherReferenceLine(offset);
reference_line_info_.back().SetOffsetToOtherReferenceLine(-offset);
}
//定义一个标志位 有有效的参考线?初始为false
bool has_valid_reference_line = false;
//遍历Frame类的数据成员reference_line_info_参考线信息类对象列表
for (auto &ref_info : reference_line_info_) {
//用障碍物列表去初始化当前遍历的参考线信息类对象
if (!ref_info.Init(obstacles())) {
AERROR << "Failed to init reference line";
} else {
//初始化成功就设置有有效的参考线标志位为true
has_valid_reference_line = true;
}
}
//返回有有效的参考线标志位
return has_valid_reference_line;
}
/**
* 用车道的宽度创建虚拟的静态障碍物,主要是针对虚拟的停止墙
*/
//参数 参考线信息类对象reference_line_info,这里不是列表了,只是一条参考线信息
//参数 障碍物obstacle_id,引用变量&应该是用来存放构建的虚拟障碍物的Id
//参数 障碍物id
//参数 障碍物对应的frenet系的s坐标
//这个函数主要是针对虚拟静态障碍物的位置建立虚拟障碍物对象,如红绿灯停止线等
const Obstacle *Frame::CreateStopObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_s) {
//如果参考线信息类对象列表为空,报错返回
if (reference_line_info == nullptr) {
AERROR << "reference_line_info nullptr";
return nullptr;
}
//获取输入参数reference_line_info参考线信息里的参考线
const auto &reference_line = reference_line_info->reference_line();
//障碍物盒对应的中心点的frenet系s坐标=障碍物s坐标 + 0.1 / 2.0
//virtual_stop_wall_length是gflags文件中定义的变量,为0.1
const double box_center_s = obstacle_s + FLAGS_virtual_stop_wall_length / 2.0;
//用参考线对象根据障碍物盒中心点s坐标获取其对应的参考点对象
auto box_center = reference_line.GetReferencePoint(box_center_s);
//获取这个障碍物盒中心点对应参考线上的参考点的Heading角
double heading = reference_line.GetReferencePoint(obstacle_s).heading();
//定义虚拟障碍物盒的宽度为4.0m,宽度对应着车道宽度方向
//长度对应着纵向
static constexpr double kStopWallWidth = 4.0;
//构建虚拟障碍物停止墙边界盒,用障碍物盒的中心点对应的参考点对象,障碍物盒的中心点对
//应的参考线上参考点对象heading角,边界盒长度0.1m,边界盒宽度4.0m
Box2d stop_wall_box{box_center, heading, FLAGS_virtual_stop_wall_length,
kStopWallWidth};
//然后通过构建的虚拟障碍物的边界盒对象stop_wall_box和障碍物id去创建虚拟障碍物
return CreateStaticVirtualObstacle(obstacle_id, stop_wall_box);
}
/**
* 用车道宽度创建虚拟静态障碍物对象,主要是用于虚拟停止墙
*/
//参数 障碍物id,车道id,车道对应的终点的frenet系s坐标lane_s
//这个函数主要是针对终点位置建立虚拟障碍物对象
const Obstacle *Frame::CreateStopObstacle(const std::string &obstacle_id,
const std::string &lane_id,
const double lane_s) {
//判断高精地图是否为空?
if (!hdmap_) {
AERROR << "Invalid HD Map.";
return nullptr;
}
//在Frame类的高精度地图对象上根据车道Id去获取车道对象
const auto lane = hdmap_->GetLaneById(hdmap::MakeMapId(lane_id));
//如果没有找到对应id的车道报错返回
if (!lane) {
AERROR << "Failed to find lane[" << lane_id << "]";
return nullptr;
}
//终点的车道s坐标就是输入参数lane_s
double dest_lane_s = std::max(0.0, lane_s);
//获取终点的车道s坐标对应的ENU大地系坐标点
auto dest_point = lane->GetSmoothPoint(dest_lane_s);
//定义两个临时变量,车道中心线左边宽度,车道中心线右边宽度,初始定义为0
double lane_left_width = 0.0;
double lane_right_width = 0.0;
//用参数lane_id对应的车道对象lane去获取终点dest_lane_s处的车道中心线左右宽度。
//获取的结果放入引用变量lane_left_width,lane_right_width
lane->GetWidth(dest_lane_s, &lane_left_width, &lane_right_width);
//构建停止墙边界盒对象stop_wall_box
//参数终点处x,y 终点处对应的车道heading,默认的虚拟墙长度0.1m,车道宽度(中心线做宽
//度 + 中心线右宽度)
Box2d stop_wall_box{{dest_point.x(), dest_point.y()},
lane->Heading(dest_lane_s),
FLAGS_virtual_stop_wall_length,
lane_left_width + lane_right_width};
//最后返回根据障碍物id和停止墙边界盒对象构建的虚拟障碍物对象。
return CreateStaticVirtualObstacle(obstacle_id, stop_wall_box);
}
/**
* 用车道宽度创建虚拟静态障碍物对象
*/
//这个函数主要是根据3个输入参数:障碍物frenet系下的起始s坐标和终点s坐标,以及障碍物的
//id,在参考线信息类对象上构建虚拟障碍物
const Obstacle *Frame::CreateStaticObstacle(
ReferenceLineInfo *const reference_line_info,
const std::string &obstacle_id, const double obstacle_start_s,
const double obstacle_end_s) {
//检查输入参数参考线信息类对象是否为空?
if (reference_line_info == nullptr) {
AERROR << "reference_line_info nullptr";
return nullptr;
}
//获取输入参数reference_line_info上的参考线reference_line
const auto &reference_line = reference_line_info->reference_line();
//这一块是将输入参数障碍物起始位置从SL坐标转化成XY坐标obstacle_start_xy
common::SLPoint sl_point;
sl_point.set_s(obstacle_start_s);
sl_point.set_l(0.0);
common::math::Vec2d obstacle_start_xy;
if (!reference_line.SLToXY(sl_point, &obstacle_start_xy)) {
AERROR << "Failed to get start_xy from sl: " << sl_point.DebugString();
return nullptr;
}
//这一块是将输入参数障碍物终点位置从SL坐标转化成XY坐标obstacle_end_s
sl_point.set_s(obstacle_end_s);
sl_point.set_l(0.0);
common::math::Vec2d obstacle_end_xy;
if (!reference_line.SLToXY(sl_point, &obstacle_end_xy)) {
AERROR << "Failed to get end_xy from sl: " << sl_point.DebugString();
return nullptr;
}
double left_lane_width = 0.0;
double right_lane_width = 0.0;
//获取障碍物起始点处对应的车道中心线左右宽度
if (!reference_line.GetLaneWidth(obstacle_start_s, &left_lane_width,
&right_lane_width)) {
AERROR << "Failed to get lane width at s[" << obstacle_start_s << "]";
return nullptr;
}
//根据障碍物起始点,终点的xy坐标构成的二维线段类对象
//以及车道在虚拟障碍物起始点处的总宽度去构建
//构建障碍物的二维障碍物边界盒对象obstacle_box
common::math::Box2d obstacle_box{
common::math::LineSegment2d(obstacle_start_xy, obstacle_end_xy),
left_lane_width + right_lane_width};
//根据障碍物id和障碍物对应的边界盒对象作为参数去调用构建虚拟障碍物的函数,
//返回一个虚拟障碍物对象类
return CreateStaticVirtualObstacle(obstacle_id, obstacle_box);
}
//这个函数还是创建虚拟障碍物对象,返回的类型为Obstacle 类
//输入参数障碍物id,以及障碍物对应的二维边界盒box
const Obstacle *Frame::CreateStaticVirtualObstacle(const std::string &id,
const Box2d &box) {
//在障碍物列表中寻找输入参数id对应的障碍物对象
const auto *object = obstacles_.Find(id);
//如果object不为空,说明找到了对应id的障碍物,障碍物还没创建就已经存在,报警告
//并返回
if (object) {
AWARN << "obstacle " << id << " already exist.";
return object;
}
//如果上面没进if也就是说该id还没有在障碍物列表中被创建
//用指针ptr指向障碍物列表新增障碍物的地址,这个新增障碍物是以id和二维边界盒为参数调用
//Obstacle类的成员函数去创建虚拟障碍物
auto *ptr =
obstacles_.Add(id, *Obstacle::CreateStaticVirtualObstacles(id, box));
//创建失败的话,报错
if (!ptr) {
AERROR << "Failed to create virtual obstacle " << id;
}
//返回创建的虚拟障碍物的指针
return ptr;
}
//Frame类的初始化函数
//输入参数 车辆状态提供器类指针对象vehicle_state_provider
//输入参数 参考线类对象列表reference_lines
//输入参数 导航路径段类对象列表segments
//输入参数: 将来的导航的车道航点序列future_route_waypoints
//输入参数: 自车信息类指针对象ego_info
//其实该函数主要就是用输入参数去初始化Frame类的数据成员
Status Frame::Init(
const common::VehicleStateProvider *vehicle_state_provider,
const std::list<ReferenceLine> &reference_lines,
const std::list<hdmap::RouteSegments> &segments,
const std::vector<routing::LaneWaypoint> &future_route_waypoints,
const EgoInfo *ego_info) {
// TODO(QiL): refactor this to avoid redundant nullptr checks in scenarios.
//用车辆状态提供器,自车信对象作为参数去初始化Frame,返回的planning状态放入status
auto status = InitFrameData(vehicle_state_provider, ego_info);
//如果初始化Frame不成功报错返回
if (!status.ok()) {
AERROR << "failed to init frame:" << status.ToString();
return status;
//用输入参数参考线类对象列表reference_lines,导航路径段类对象列表segments
//去设定Frame类的数据成员reference_line_info_参考线信息类对象列表
if (!CreateReferenceLineInfo(reference_lines, segments)) {
const std::string msg = "Failed to init reference line info.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
//设定Frame类的数据成员future_route_waypoints_ 为输入参数future_route_waypoints
//指的是将来的导航路径航点集
future_route_waypoints_ = future_route_waypoints;
//返回状态ok
return Status::OK();
}
//针对OpenSpace的规划器的初始化
//相比上面的Init函数少了结构化的车道,以及车道上的segments的相关的操作
Status Frame::InitForOpenSpace(
const common::VehicleStateProvider *vehicle_state_provider,
const EgoInfo *ego_info) {
//其实还是调用了下方的InitFrameData函数
return InitFrameData(vehicle_state_provider, ego_info);
}
//函数名其实意思就是初始化Frame类的数据
//输入参数 车辆状态提供器类指针对象vehicle_state_provider
//输入参数: 自车信息类指针对象ego_info
Status Frame::InitFrameData(
const common::VehicleStateProvider *vehicle_state_provider,
const EgoInfo *ego_info) {
//先从指定路径下获取指定格式的高精度地图类对象赋值给Frame类的数据成员hdmap_
hdmap_ = hdmap::HDMapUtil::BaseMapPtr();
CHECK_NOTNULL(hdmap_); //检查地图非空
//获取车辆的当前状态赋值给Frame类的数据成员vehicle_state_
vehicle_state_ = vehicle_state_provider->vehicle_state();
//如果车辆状态无效的话,报错返回
if (!util::IsVehicleStateValid(vehicle_state_)) {
AERROR << "Adc init point is not set";
return Status(ErrorCode::PLANNING_ERROR, "Adc init point is not set");
}
//对齐预测时间默认关闭false,输出一条debug信息
ADEBUG << "Enabled align prediction time ? : " << std::boolalpha
<< FLAGS_align_prediction_time;
//既然默认关闭,直接跳过这段if不深究
if (FLAGS_align_prediction_time) {
auto prediction = *(local_view_.prediction_obstacles);
AlignPredictionTime(vehicle_state_.timestamp(), &prediction);
local_view_.prediction_obstacles->CopyFrom(prediction);
}
//遍历预测障碍物,将数据成员local_view_预测障碍物全部读入Frame类的数据成员
//obstacles_障碍物类列表
for (auto &ptr :
Obstacle::CreateObstacles(*local_view_.prediction_obstacles)) {
AddObstacle(*ptr);
}
//如果规划起始点的速度<0.001,其实是说明车辆静止?
if (planning_start_point_.v() < 1e-3) {
//根据自车信息类对象作为参数去找会产生碰撞的障碍物对象
const auto *collision_obstacle = FindCollisionObstacle(ego_info);
//如果collision_obstacle 不为空,说明找到了碰撞障碍物
if (collision_obstacle != nullptr) {
//如果有碰撞障碍物,就打印错误信息
const std::string msg = absl::StrCat(
"Found collision with obstacle: ", collision_obstacle->Id());
AERROR << msg;
monitor_logger_buffer_.ERROR(msg);
return Status(ErrorCode::PLANNING_ERROR, msg);
}
}
//把Frame类数据成员local_view_中的交通灯信息读取到Frame类数据成员traffic_lights_中
ReadTrafficLights();
//读取local_view_中的驾驶员交互操作行为
ReadPadMsgDrivingAction();
//返回状态ok
return Status::OK();
}
//该函数就是将Frame类对象数据成员中的障碍物列表去和自车信息作碰撞检测,返回会产生碰撞的
//障碍物对象
const Obstacle *Frame::FindCollisionObstacle(const EgoInfo *ego_info) const {
//如果障碍物列表为空,直接返回空指针障碍物对象
if (obstacles_.Items().empty()) {
return nullptr;
}
//自车的多边形adc_polygon实际上就是自车边界盒构成的多边形对象
const auto &adc_polygon = Polygon2d(ego_info->ego_box());
//遍历Frame类数据成员障碍物列表obstacles_
for (const auto &obstacle : obstacles_.Items()) {
//如果障碍物是虚拟的,则直接进入下一个障碍物的循环
if (obstacle->IsVirtual()) {
continue;
}
//当前遍历的障碍物的多边形就是感知障碍物的边界盒多边形
const auto &obstacle_polygon = obstacle->PerceptionPolygon();
//如果障碍物多边形和自车多边形有overlap重叠,
//modules\common\math\polygon2d.cc中定义了HasOverlap()函数,属于Polygon2d
//类的成员函数,可以快速判断两个多边形是否有重叠
if (obstacle_polygon.HasOverlap(adc_polygon)) {
//如果自车和当前遍历的障碍物对象有重叠的话,就返回当前遍历的障碍物对象
return obstacle;
}
}
//遍历完Frame类成员障碍物列表都没有返回的话,说明确实没有会碰撞的障碍物
//返回空指针
return nullptr;
}
//返回Frame类数据成员序列号,这个序列号是干什么的?
uint32_t Frame::SequenceNum() const { return sequence_num_; }
//获取debug string打印序列号字符串,所以序列号是类似于id一样的识别号代表特定一帧frame
//数据?
std::string Frame::DebugString() const {
return absl::StrCat("Frame: ", sequence_num_);
}
//其实这个函数就记录planning的所有输入数据用于debug
//这些输入数据几乎都是从Frame类成员local_view_里拷贝的
//planning_internal::Debug 是modules\planning\proto\planning_internal.proto
//里定义的message类,Debug下面包含planning_data
//planning_data里又包含规划的相关数据,如自车位置,底盘反馈,routing响应,
//起始点,路径,速度规划,st图,sl坐标,障碍物,信号灯,前方畅通距离,场景信息等等,几
//乎所有的planning输入debug数据?
//google protobuf的用法,可以根据.proto文件生成相应的c++类,视为一种数据结构
void Frame::RecordInputDebug(planning_internal::Debug *debug) {
//如果debug为空,直接返回
if (!debug) {
ADEBUG << "Skip record input into debug";
return;
}
//用指针planning_debug_data指向输入参数debug里的planning_data
//mutable可以修改被const修饰的变量或成员
auto *planning_debug_data = debug->mutable_planning_data();
//用指针adc_position指向输入参数debug里的adc_position
auto *adc_position = planning_debug_data->mutable_adc_position();
//通过指针adc_position设定debug里的planning_debug_data里的自车位置
//从local_view_进行拷贝定位信息
adc_position->CopyFrom(*local_view_.localization_estimate);
//同理设定输入参数debug里的planning_debug_data里的chassis底盘信息
//从local_view_进行拷贝底盘信息
auto debug_chassis = planning_debug_data->mutable_chassis();
debug_chassis->CopyFrom(*local_view_.chassis);
//如果不用导航模式的话,设定输入参数debug里的planning_debug_data里的routing全局路
//径规划信息,同样是从local_view_拷贝
//导航模式就不需要routing了,我理解导航模式就是循迹模式?导航模式通常不用,不重要
if (!FLAGS_use_navigation_mode) {
auto debug_routing = planning_debug_data->mutable_routing();
debug_routing->CopyFrom(*local_view_.routing);
}
//从local_view_里拷贝预测障碍物列表的header到输入参数debug里的planning_debug_data
//里的prediction_header
planning_debug_data->mutable_prediction_header()->CopyFrom(
local_view_.prediction_obstacles->header());
/*
auto relative_map = AdapterManager::GetRelativeMap();
if (!relative_map->Empty()) {
planning_debug_data->mutable_relative_map()->mutable_header()->CopyFrom(
relative_map->GetLatestObserved().header());
}
*/
}
//这个函数是是实现预测时间对齐planning起始时间?
//输入参数 规划起始时间planning_start_time
//输入参数 预测障碍物列表prediction_obstacles
void Frame::AlignPredictionTime(const double planning_start_time,
PredictionObstacles *prediction_obstacles) {
//如果预测障碍物列表为空 或 预测障碍物列表没有header 或预测障碍物列表的header里
//没有时间戳,只要任意一个发生就直接返回
if (!prediction_obstacles || !prediction_obstacles->has_header() ||
!prediction_obstacles->header().has_timestamp_sec()) {
return;
}
//获取输入参数预测障碍物列表的header时间戳prediction_header_time
double prediction_header_time =
prediction_obstacles->header().timestamp_sec();
//遍历预测障碍物列表
for (auto &obstacle : *prediction_obstacles->mutable_prediction_obstacle()) {
//遍历障碍物轨迹
for (auto &trajectory : *obstacle.mutable_trajectory()) {
//遍历轨迹点
for (auto &point : *trajectory.mutable_trajectory_point()) {
//修改预测障碍物轨迹点的相对时间为 预测头部时间 + 预测轨迹点相对时间 - 规划起
//始时间,其实就是为了将预测障碍物列表的header时间与planning的起始时间对齐
point.set_relative_time(prediction_header_time + point.relative_time() -
planning_start_time);
}
//如果障碍物的预测轨迹点非空 且 起始轨迹点的相对时间<0
if (!trajectory.trajectory_point().empty() &&
trajectory.trajectory_point().begin()->relative_time() < 0) {
auto it = trajectory.trajectory_point().begin();
//如果预测障碍物列表相对header时间小于0,则遍历找到预测障碍物轨迹上相对时间不
//小于0的轨迹点
while (it != trajectory.trajectory_point().end() &&
it->relative_time() < 0) {
++it;
}
//清除掉这个预测障碍物相对时间<0的部分
trajectory.mutable_trajectory_point()->erase(
trajectory.trajectory_point().begin(), it);
}
}
}
}
//根据障碍物id去Frame类成员障碍物列表obstacles_里找到id匹配的障碍物对象,将障碍物
//对象返回
Obstacle *Frame::Find(const std::string &id) { return obstacles_.Find(id); }
//增加一个障碍物对象到类数据成员障碍物列表里
void Frame::AddObstacle(const Obstacle &obstacle) {
obstacles_.Add(obstacle.Id(), obstacle);
}
//读交通灯函数,从数据成员local_view_读到Frame类数据成员traffic_lights_交通灯列表中
//local_view_包含规划的所有输入数据
void Frame::ReadTrafficLights() {
//首先清空之前储存的数据
traffic_lights_.clear();
//从local_view_里读取到交通灯信息,local_view_就是存放planning的所有输入信息的一个变量
const auto traffic_light_detection = local_view_.traffic_light;
//如果读到的交通灯信息为空则返回
if (traffic_light_detection == nullptr) {
return;
}
//计算延迟,用交通灯检测信息的时间戳-当前时间
const double delay = traffic_light_detection->header().timestamp_sec() -
Clock::NowInSeconds();
//如果延迟大于signal_expire_time_sec,是gflag变量,默认为5s则输出debug信息
if (delay > FLAGS_signal_expire_time_sec) {
ADEBUG << "traffic signals msg is expired, delay = " << delay
<< " seconds.";
return;
}
//遍历交通灯信息,将所有交通灯对象及其id塞到frame类的数据成员traffic_lights_中
for (const auto &traffic_light : traffic_light_detection->traffic_light()) {
traffic_lights_[traffic_light.id()] = &traffic_light;
}
}
//根据id获取交通灯对象,输入的是交通灯的id
perception::TrafficLight Frame::GetSignal(
const std::string &traffic_light_id) const {
//去类数据成员交通灯列表traffic_lights_找这个id
const auto *result =
apollo::common::util::FindPtrOrNull(traffic_lights_, traffic_light_id);
//如果为空,设置一些默认信息并返回
if (result == nullptr) {
perception::TrafficLight traffic_light;
traffic_light.set_id(traffic_light_id);
traffic_light.set_color(perception::TrafficLight::UNKNOWN);
traffic_light.set_confidence(0.0);
traffic_light.set_tracking_time(0.0);
return traffic_light;
}
//不为空返回这个交通灯对象
return *result;
}
//这个是读取人机交互动作?就是STOP/START/RESET,设置驾驶模式一类
void Frame::ReadPadMsgDrivingAction() {
if (local_view_.pad_msg) {
if (local_view_.pad_msg->has_action()) {
pad_msg_driving_action_ = local_view_.pad_msg->action();
}
}
}
//复位人机交互动作对象
void Frame::ResetPadMsgDrivingAction() {
pad_msg_driving_action_ = DrivingAction::NONE;
}
//找驾驶参考线信息
//其实这个函数就是找到Frame类数据成员reference_line_info_道路参考线列表中cost最小且可行驶的道路参考线对象
const ReferenceLineInfo *Frame::FindDriveReferenceLineInfo() {
//初始化最小的cost为无穷大,就是double能表示的最大数
double min_cost = std::numeric_limits<double>::infinity();
//初始定义Frame类的数据成员驾驶参考线drive_reference_line_info_信息对象为空指针
drive_reference_line_info_ = nullptr;
//遍历Frame类里的数据成员的道路参考线列表reference_line_info_
//其实这个函数就是找到Frame类数据成员reference_line_info_道路参考线列表中cost最小且可行驶的道路参考线
for (const auto &reference_line_info : reference_line_info_) {
//如果遍历的这条道路参考线可驾驶,且cost小于最小cost(初始定义为无穷大)
//那么就更新最小的cost,同时更新驾驶道路参考线对象drive_reference_line_info_
//更新为这条cost更小的道路参考线
if (reference_line_info.IsDrivable() &&
reference_line_info.Cost() < min_cost) {
drive_reference_line_info_ = &reference_line_info;
min_cost = reference_line_info.Cost();
}
}
return drive_reference_line_info_;
}
//找目标道路参考线,返回的是道路参考线信息ReferenceLineInfo类对象
//这个函数其实是找变道参考线?
const ReferenceLineInfo *Frame::FindTargetReferenceLineInfo() {
//初始化定义输出结果的道路参考线对象为空
const ReferenceLineInfo *target_reference_line_info = nullptr;
//遍历Frame类的数据成员reference_line_info_道路参考线列表
for (const auto &reference_line_info : reference_line_info_) {
//如果找到变道路径参考线就直接返回,所以这个函数其实是找变道参考线?
if (reference_line_info.IsChangeLanePath()) {
return &reference_line_info;
}
target_reference_line_info = &reference_line_info;
}
return target_reference_line_info;
}
//查找失败的参考线信息,返回道路参考线信息类ReferenceLineInfo类对象
//遍历道路参考线列表,找到不可驾驶的变道参考线信息类对象?
const ReferenceLineInfo *Frame::FindFailedReferenceLineInfo() {
//遍历道路参考线列表,找到不可驾驶的变道参考线信息类对象?
for (const auto &reference_line_info : reference_line_info_) {
// Find the unsuccessful lane-change path
if (!reference_line_info.IsDrivable() &&
reference_line_info.IsChangeLanePath()) {
return &reference_line_info;
}
}
return nullptr;
}
//返回Frame类的数据成员驾驶参考线信息类对象
const ReferenceLineInfo *Frame::DriveReferenceLineInfo() const {
return drive_reference_line_info_;
}
//返回Frame类的数据成员障碍物列表obstacles_
const std::vector<const Obstacle *> Frame::obstacles() const {
return obstacles_.Items();
}
} // namespace planning
} // namespace apollo
frame_test.cc
Frame类的测试代码,了解Frame类的使用和功能。
测试里好像都是测试将预测障碍物的对齐planning起始时间,但其实默认关闭,不细究该部分。
#include "modules/planning/common/frame.h"
#include "cyber/common/file.h"
#include "gtest/gtest.h"
#include "modules/common/util/util.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
namespace apollo {
namespace planning {
class FrameTest : public ::testing::Test {
public:
virtual void SetUp() {
ASSERT_TRUE(cyber::common::GetProtoFromFile(
"/apollo/modules/planning/testdata/common/sample_prediction.pb.txt",
&prediction_obstacles_));
}
protected:
prediction::PredictionObstacles prediction_obstacles_;
};
TEST_F(FrameTest, AlignPredictionTime) {
int first_traj_size = prediction_obstacles_.prediction_obstacle(0)
.trajectory(0)
.trajectory_point_size();
double origin_pred_time = prediction_obstacles_.header().timestamp_sec();
Frame::AlignPredictionTime(origin_pred_time + 0.1, &prediction_obstacles_);
ASSERT_EQ(first_traj_size - 1, prediction_obstacles_.prediction_obstacle(0)
.trajectory(0)
.trajectory_point_size());
Frame::AlignPredictionTime(origin_pred_time + 0.5, &prediction_obstacles_);
ASSERT_EQ(first_traj_size - 3, prediction_obstacles_.prediction_obstacle(0)
.trajectory(0)
.trajectory_point_size());
Frame::AlignPredictionTime(origin_pred_time + 12.0, &prediction_obstacles_);
ASSERT_EQ(0, prediction_obstacles_.prediction_obstacle(0)
.trajectory(0)
.trajectory_point_size());
}
} // namespace planning
} // namespace apollo