概述
common.cc/.h是planning模块下的common/util路径下
根据路径和命名也可以看出
common.cc其实就是实现了一些planning中会用到的一些通用的辅助工具,util这个文件夹在很多工程中都可以看到,都是存放一些辅助功能的实现代码。
从代码来看,common.cc主要是实现:
第一个函数
根据停止线的Frenet系的纵坐标s构建PathDecision路径决策类对象(停车决策),并设置相关的停车原因,停车点xyz坐标,停车处heading角等信息。
第二个函数(重载第一个函数)
根据障碍物所处的车道id以及在障碍物在车道中的Frenet系的纵坐标s构建PathDecision路径决策类对象(停车决策),并设置相关的停车原因,停车点xyz坐标,停车处heading角等信息。
主要就是实现针对停止线/车道中的障碍物设定路径决策对象。
common.h
#pragma once
#include <string>
#include <vector>
#include "modules/planning/common/frame.h"
#include "modules/planning/common/reference_line_info.h"
namespace apollo {
namespace planning {
namespace util { //命名空间 apollo::planning::util::
/*
* @brief:建立虚拟的障碍物或这虚拟墙,并且增加STOP停车决策
*/
//参数stop_wall_id障碍物ID,是字符串类
//参数stop_line_s是Frenet坐标系下停止线的纵坐标s
//参数安全距离stop_distance
//参数stop_reason_code是StopReasonCode类型变量,停车原因代码
//参数wait_for_obstacles是一个string类型的vector,猜测其是一系列障碍物ID?
//参数decision_tag是string类型,猜测就是决策的tag,也就是表明是停车/超车/减速避让等
//参数Frame类 frame Frame里面保存了一次规划周期的所有数据,后续的博客将会解析到
//参数ReferenceLineInfo类 reference_line_info ReferenceLineInfo类提供平滑的参考
//线,通常是道路中心线
int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
const double stop_distance,
const StopReasonCode& stop_reason_code,
const std::vector<std::string>& wait_for_obstacles,
const std::string& decision_tag, Frame* const frame,
ReferenceLineInfo* const reference_line_info);
//函数重载,建立stop停车决策,参数几乎同上,唯一的不同是将参数stop_line_s停止线位置换
//成了lane_id车道Id和对应的车道的障碍物相对与车道的纵向位置lane_s
int BuildStopDecision(const std::string& stop_wall_id,
const std::string& lane_id, const double lane_s,
const double stop_distance,
const StopReasonCode& stop_reason_code,
const std::vector<std::string>& wait_for_obstacles,
const std::string& decision_tag, Frame* const frame,
ReferenceLineInfo* const reference_line_info);
} // namespace util
} // namespace planning
} // namespace apollo
common.cc
#include "modules/planning/common/util/common.h"
namespace apollo {
namespace planning {
namespace util {
//用到apollo::common::util::命名空间下的另一个函数WithinBound,判断一个value是否在上下边界之间,返回true or false
using apollo::common::util::WithinBound;
/*
* @brief:建立虚拟的障碍物或这虚拟墙,并且增加STOP停车决策
*/
//参数stop_wall_id障碍物ID,是字符串类
//参数stop_line_s是Frenet坐标系下停止线的纵坐标s
//参数安全距离stop_distance
//参数stop_reason_code是StopReasonCode类型变量,停车原因代码
//参数wait_for_obstacles是一个string类型的vector,猜测其是一系列障碍物ID?
//参数decision_tag是string类型,猜测就是决策的tag,也就是表明是停车/超车/减速避让等
//参数Frame类 frame Frame里面保存了一次规划周期的所有数据,后续的博客将会解析到
//参数ReferenceLineInfo类 reference_line_info ReferenceLineInfo类提供平滑的参考
//线,通常是道路中心线
int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
const double stop_distance,
const StopReasonCode& stop_reason_code,
const std::vector<std::string>& wait_for_obstacles,
const std::string& decision_tag, Frame* const frame,
ReferenceLineInfo* const reference_line_info) {
CHECK_NOTNULL(frame);//判断frame不为NULL空,否则报错
CHECK_NOTNULL(reference_line_info);//判断reference_line_info不为NULL空,否则报错
// check
//获取reference_line_info中的参考线数据放入reference_line
const auto& reference_line = reference_line_info->reference_line();
//判断停止线的纵向位置s是否处于 区间[0 参考线总长度],否则报错
if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line";
return 0;
}
// create virtual stop wall
// 创建虚拟的停止墙
//可以看出CreateStopObstacle是Frame类的成员函数
//参数reference_line_info参考线信息,stop_wall_id虚拟墙障碍物id
//stop_line_s是停止线Frenet纵向位置,涉及Frame部分后续会有专门博客讲解
const auto* obstacle =
frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s);
//如果obstacle为空,则报错
if (!obstacle) {
AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
return -1;
}
//这里又调用了ReferenceLineInfo类的成员函数AddObstacle()其实就是在参考线上增加
//了一个障碍物,并返回一个障碍物对象
const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
if (!stop_wall) {
AERROR << "Failed to add obstacle[" << stop_wall_id << "]";
return -1;
}
// build stop decision
// 建立停车决策
// 停止位置stop_s = 停止线位置 - 安全距离stop_distance,就是留一个安全距离作为缓冲
// 不能刚好停在停止线那,而是把期望停止点往回挪一个安全距离
const double stop_s = stop_line_s - stop_distance;
//参考线reference_line调用成员函数GetReferencePoint()查询参考线上停止点信息
const auto& stop_point = reference_line.GetReferencePoint(stop_s);
//查询停止点在参考线上的heading角
const double stop_heading =
reference_line.GetReferencePoint(stop_s).heading();
//ObjectDecisionType是个找不到的类,是.proto文件里的message生成的类
//这是google protobuf库的用法,message可以表示一种数据结构
//message ObjectDecisionType定义位于modules\planning\proto\decision.proto
//这个proto里储存者多种针对object物体的行为决策类型
//例如:ignore/stop/follow/yield/overtake/nudge/side_pass
//忽略/停止/跟随/减速避让(车,人)/超车/接近(催促?)/绕行?
ObjectDecisionType stop; //定义一个物体决策类型对象stop
auto* stop_decision = stop.mutable_stop(); //将指针stop_decision指向物体决策类型
//对象stop里的stop
//设置物体决策类型对象stop里的stop决策的停车原因
stop_decision->set_reason_code(stop_reason_code);
//设置物体决策类型对象stop里的stop决策的停车安全距离
stop_decision->set_distance_s(-stop_distance);
//设置物体决策类型对象stop里的stop决策的停止点的heading
stop_decision->set_stop_heading(stop_heading);
//设置物体决策类型对象stop里的stop决策的停止点x坐标
stop_decision->mutable_stop_point()->set_x(stop_point.x());
//设置物体决策类型对象stop里的stop决策的停止点y坐标
stop_decision->mutable_stop_point()->set_y(stop_point.y());
//设置物体决策类型对象stop里的stop决策的停止点z坐标
stop_decision->mutable_stop_point()->set_z(0.0);
//将一些列需要等待的障碍物加到设置物体决策类型对象stop里的stop决策的
//加到这一项wait_for_obstacle,这是protobuf库的用法
//add_就可以增加一个repeated元素
for (size_t i = 0; i < wait_for_obstacles.size(); ++i) {
stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]);
}
//路径决策path_decision = 参考线调用成员函数path_decision()返回其数据成员路径决策
//路径决策其实是隐藏在ReferenceLineInfo类里的吗?
auto* path_decision = reference_line_info->path_decision();
//路径决策path_decision,纵向决策增加一个stop
path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);
return 0;
}
//函数重载,建立stop停车决策,参数几乎同上,唯一的不同是将参数stop_line_s停止线位置换
//成了lane_id车道Id和对应的车道的障碍物相对与车道的纵向位置lane_s
int BuildStopDecision(const std::string& stop_wall_id,
const std::string& lane_id, const double lane_s,
const double stop_distance,
const StopReasonCode& stop_reason_code,
const std::vector<std::string>& wait_for_obstacles,
const std::string& decision_tag, Frame* const frame,
ReferenceLineInfo* const reference_line_info) {
//检查frame,reference_line_info是否为空,否则报错
CHECK_NOTNULL(frame);
CHECK_NOTNULL(reference_line_info);
//获取reference_line_info中的参考线
const auto& reference_line = reference_line_info->reference_line();
// create virtual stop wall
//创建虚拟墙障碍物对象,这个CreateStopObstacle()函数也是一个重载函数
//其参数也变成了停止墙id stop_wall_id,车道id lane_id,停止点的在车道上对应的Frenet
//纵向坐标lane_s
const auto* obstacle =
frame->CreateStopObstacle(stop_wall_id, lane_id, lane_s);
if (!obstacle) { //检查障碍物对象是否创建成功
AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
return -1;
}
//在参考线信息上增加障碍物
const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
if (!stop_wall) {
AERROR << "Failed to create obstacle for: " << stop_wall_id;
return -1;
}
//停止墙盒由障碍物对象调用其成员函数PerceptionBoundingBox()返回其障碍物边界盒
const auto& stop_wall_box = stop_wall->PerceptionBoundingBox();
//判断障碍物边界盒的中心在车道上?,不不在的话报debug信息并直接返回
if (!reference_line.IsOnLane(stop_wall_box.center())) {
ADEBUG << "stop point is not on lane. SKIP STOP decision";
return 0;
}
// build stop decision
//建立停止决策
//停止点就位于参考线上查找停止墙边界盒的起始Frenet纵向坐标start_s - 停车安全距离
//stop_distance
auto stop_point = reference_line.GetReferencePoint(
stop_wall->PerceptionSLBoundary().start_s() - stop_distance);
//往物体决策类型对象stop里写入相关的信息
//做出停车决策的原因,安全距离,停止点期望的x,y,z坐标及heading角
ObjectDecisionType stop;
auto* stop_decision = stop.mutable_stop();
stop_decision->set_reason_code(stop_reason_code);
stop_decision->set_distance_s(-stop_distance);
stop_decision->set_stop_heading(stop_point.heading());
stop_decision->mutable_stop_point()->set_x(stop_point.x());
stop_decision->mutable_stop_point()->set_y(stop_point.y());
stop_decision->mutable_stop_point()->set_z(0.0);
//路径决策path_decision = 参考线调用成员函数path_decision()返回其数据成员路径决策
//路径决策其实是隐藏在ReferenceLineInfo类里的吗?
auto* path_decision = reference_line_info->path_decision();
//路径决策path_decision,纵向决策增加一个stop
path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);
return 0;
}
} // namespace util
} // namespace planning
} // namespace apollo