百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\Obstacle类代码详解
2024-05-17 10:38:37
概述
Obstacle类是apollo planning模块下modules\planning\common\obstacle.cc/.h实现
从类名来看，应该是障碍物类，将一个障碍物的所有相关信息封装成这个类。
注：
/nudge在apollo里代表横向上轻轻绕一下避开？
sl_boundary_理解为障碍物的SL边界，其实就是边界盒坐标转化为SL坐标形式？
ObjectDecisionType modules\planning\proto\decision.proto中定义的message类，针对该障碍物对象决策：停车/绕行/减速避让等
从代码来看Obstacle类主要是实现：
obstacle.h
#pragma once#include <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/planning/common/indexed_list.h"
#include "modules/planning/common/speed/st_boundary.h"
#include "modules/planning/proto/decision.pb.h"
#include "modules/planning/proto/sl_boundary.pb.h"
#include "modules/planning/reference_line/reference_line.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"namespace apollo {namespace planning {/*** @class Obstacle* @brief 这是一个连接障碍物路径及其所在path属性的类* 一个障碍物的路径属性包括frenet系下s,l的值* 障碍物的决策也属于一个路径path的属性* 决策分为两种：横向决策和纵向决策* 横向决策包括：忽略ignore/轻微绕行nudge* 横向决策安全优先级:nudge > ignore* 纵向决策包括：停止stop/减速避让yield/跟车follow/超车overtake/忽略ignore* 纵向决策的安全优先级：stop > yield >= follow > overtake > ignore* ignore决策同时属于纵向和横向决策，其优先级最低*/
class Obstacle {public:Obstacle() = default;//障碍物类带参构造函数，输入参数id,感知障碍物类对象，障碍物优先级，是否为静态障碍物去构造一个障碍物实例对象Obstacle(const std::string& id,const perception::PerceptionObstacle& perception_obstacle,const prediction::ObstaclePriority::Priority& obstacle_priority,const bool is_static);//Obstacle类的带参构造函数重载，输入参数不同
//输入参数跟上面差不多，就多了一个预测轨迹类对象trajectory Obstacle(const std::string& id,const perception::PerceptionObstacle& perception_obstacle,const prediction::Trajectory& trajectory,const prediction::ObstaclePriority::Priority& obstacle_priority,const bool is_static);//设置/获取障碍物id_，也就是其类成员const std::string& Id() const { return id_; }void SetId(const std::string& id) { id_ = id; }//获取障碍物的速度speed_，也就是其数据成员double speed() const { return speed_; }//获取数据成员感知障碍物id perception_id_int32_t PerceptionId() const { return perception_id_; }//返回障碍物是否为静止的标志位，类数据成员is_static_bool IsStatic() const { return is_static_; }//返回障碍物是否为虚拟的标志位，类数据成员is_virtual_ bool IsVirtual() const { return is_virtual_; }//根据时间查询预测轨迹点
//输入参数是相对时间relative_timecommon::TrajectoryPoint GetPointAtTime(const double time) const;//获取障碍物的2维边界盒，输入参数是预测轨迹点？其实就是轨迹点作为几何中心点其x,y,theta信息加上数据成员感知障碍物的长宽构建二维边界盒common::math::Box2d GetBoundingBox(const common::TrajectoryPoint& point) const;//返回感知障碍物的边界盒，类数据成员perception_bounding_box_const common::math::Box2d& PerceptionBoundingBox() const {return perception_bounding_box_;}//返回感知障碍物的多边形，类的数据成员perception_polygon_const common::math::Polygon2d& PerceptionPolygon() const {return perception_polygon_;}//获取障碍物的预测轨迹，返回类的数据成员trajectory_const prediction::Trajectory& Trajectory() const { return trajectory_; }common::TrajectoryPoint* AddTrajectoryPoint() {return trajectory_.add_trajectory_point();}//返回障碍物是否有轨迹？bool HasTrajectory() const {return !(trajectory_.trajectory_point().empty());}//返回感知障碍物对象，属于Obstacle类成员const perception::PerceptionObstacle& Perception() const {return perception_obstacle_;}/*** @brief 这是一个辅助函数可以创建障碍物来自预测数据* 预测原始对于每个障碍物可能有多个轨迹？* 这个函数会针对每个轨迹创建一个障碍物？* @param predictions 预测结果* @return obstacles 障碍物对象列表*/
//创建障碍物类对象指针列表，输入参数是预测障碍物列表
//其实就是从预测障碍物对象中提取出有效的障碍物和有效的轨迹等信息，塞入障碍物类对象指针列表返回static std::list<std::unique_ptr<Obstacle>> CreateObstacles(const prediction::PredictionObstacles& predictions);//创建虚拟障碍物函数
//输入参数id，二维障碍物边界盒static std::unique_ptr<Obstacle> CreateStaticVirtualObstacles(const std::string& id, const common::math::Box2d& obstacle_box);//判断是否为有效的感知障碍物，输入参数是感知障碍物类对象static bool IsValidPerceptionObstacle(const perception::PerceptionObstacle& obstacle);//判断是否为有效的轨迹点，输入参数是一个轨迹点类型对象
//只要轨迹点没有路径点 或 x/y/z/kappa/s/dkappa/ddkappa/v/a/relative_time里只要有
//一个为nan(not a number)就说明这个轨迹点是无效的，否则有效static bool IsValidTrajectoryPoint(const common::TrajectoryPoint& point);//内联函数，返回障碍物是否为注意等级的障碍物？inline bool IsCautionLevelObstacle() const {return is_caution_level_obstacle_;}// const Obstacle* obstacle() const;/*** return 融合的横向决策* Lateral decision is one of {Nudge, Ignore}**/
//返回Obstacle类数据成员，针对这个障碍物的横向决策const ObjectDecisionType& LateralDecision() const;/*** @brief return 融合的纵向决策* Longitudinal decision is one of {Stop, Yield, Follow, Overtake, Ignore}**/const ObjectDecisionType& LongitudinalDecision() const;//返回debug字符串std::string DebugString() const;//获取该感知障碍物的sl边界盒？是感知障碍物边界在参考线上的投影？const SLBoundary& PerceptionSLBoundary() const;//获取数据成员 参考线ST边界const STBoundary& reference_line_st_boundary() const;//获取数据成员 路径ST边界，是指该障碍物所占据的的路径边界？const STBoundary& path_st_boundary() const;//返回数据成员，决策标签？一个字符串的vector,decider_tags_const std::vector<std::string>& decider_tags() const;//返回目标决策类型列表？const std::vector<ObjectDecisionType>& decisions() const;//针对该障碍物增加一个纵向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标纵向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个void AddLongitudinalDecision(const std::string& decider_tag,const ObjectDecisionType& decision);//针对该障碍物增加一个横向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标横向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个void AddLateralDecision(const std::string& decider_tag,const ObjectDecisionType& decision);//检查针对该障碍物是否有横向决策？bool HasLateralDecision() const;//设定路径的ST边界，也就是针对该障碍物的ST边界？void set_path_st_boundary(const STBoundary& boundary);//返回数据成员 路径ST边界被初始化？bool is_path_st_boundary_initialized() {return path_st_boundary_initialized_;}//设置ST边界类型？void SetStBoundaryType(const STBoundary::BoundaryType type);//擦除类里储存的路径ST边界void EraseStBoundary();//设定道路参考线的ST边界void SetReferenceLineStBoundary(const STBoundary& boundary);//设定道路参考线ST边界类型void SetReferenceLineStBoundaryType(const STBoundary::BoundaryType type);//擦除类里储存的道路参考线ST边界void EraseReferenceLineStBoundary();//检查针对该障碍物是否有纵向决策？bool HasLongitudinalDecision() const;//针对该障碍物的决策是否不可忽略bool HasNonIgnoreDecision() const;/*** @brief* 用自车的最小转弯半径去计算到该障碍物的停止距离*/double MinRadiusStopDistance(const common::VehicleParam& vehicle_param) const;/*** @brief* 检查该障碍物是否可以被安全的忽略* 目标可以被忽略仅当纵向和横向决策都是可以忽略的时候*/bool IsIgnore() const;bool IsLongitudinalIgnore() const;bool IsLateralIgnore() const;//针对障碍物对象建立自车参考线的ST边界，输入参数是道路参考线类对象，以及adc自车的起始s坐标，将障碍物投影到自车的ST图上void BuildReferenceLineStBoundary(const ReferenceLine& reference_line,const double adc_start_s);//设置障碍物的SL边界也就是Frenet系的横纵坐标的边界类对象sl_boundary
//输入参数就是SLBoundary SL边界类对象
//就是将参数拷贝给类数据成员sl_boundary_ 其实就是障碍物边界点相对于道路参考线的SL坐标void SetPerceptionSlBoundary(const SLBoundary& sl_boundary);/*** @brief * 是否为纵向决策，有ignore，有stop/yield/follow/overtake都是**/static bool IsLongitudinalDecision(const ObjectDecisionType& decision);//nudge在apollo里代表横向上轻轻绕一下避开？
//判断目标决策类型对象是横向决策？如果有ignore或nudge就认为是static bool IsLateralDecision(const ObjectDecisionType& decision);//设置该障碍物为阻塞障碍物？void SetBlockingObstacle(bool blocking) { is_blocking_obstacle_ = blocking; }//返回该障碍物是否为阻塞自车的障碍物？bool IsBlockingObstacle() const { return is_blocking_obstacle_; }/** @brief IsLaneBlocking is only meaningful when IsStatic() == true.* 只有当障碍物为静态时，判断车道是否被阻塞*/bool IsLaneBlocking() const { return is_lane_blocking_; }void CheckLaneBlocking(const ReferenceLine& reference_line);bool IsLaneChangeBlocking() const { return is_lane_change_blocking_; }void SetLaneChangeBlocking(const bool is_distance_clear);private:FRIEND_TEST(MergeLongitudinalDecision, AllDecisions);static ObjectDecisionType MergeLongitudinalDecision(const ObjectDecisionType& lhs, const ObjectDecisionType& rhs);FRIEND_TEST(MergeLateralDecision, AllDecisions);static ObjectDecisionType MergeLateralDecision(const ObjectDecisionType& lhs,const ObjectDecisionType& rhs);//建立轨迹的ST边界
//输入参数 参考线类对象，自车起始的frenet系纵坐标s，第三个参数用以存放得到的结果
//这个函数的实现代码没太看明白，大致就是根据参考线，自车的纵向坐标s，用动态感知障碍物对象及其预测轨迹去修改之前的自车参考线ST边界，起始就是用该障碍物信息去修正ST图中搜索的可行域？将动态障碍物投影到自车的ST图上。bool BuildTrajectoryStBoundary(const ReferenceLine& reference_line,const double adc_start_s,STBoundary* const st_boundary);//是否为有效的障碍物，主要是看感知障碍物的长宽是否为nan(not a number)或者过于小了，是的话就说明是无效的障碍物bool IsValidObstacle(const perception::PerceptionObstacle& perception_obstacle);private:std::string id_;int32_t perception_id_ = 0;bool is_static_ = false;bool is_virtual_ = false;double speed_ = 0.0;bool path_st_boundary_initialized_ = false;prediction::Trajectory trajectory_;perception::PerceptionObstacle perception_obstacle_;common::math::Box2d perception_bounding_box_;common::math::Polygon2d perception_polygon_;std::vector<ObjectDecisionType> decisions_;std::vector<std::string> decider_tags_;SLBoundary sl_boundary_; //其实就是障碍物边界点相对于道路参考线的SL坐标STBoundary reference_line_st_boundary_;STBoundary path_st_boundary_;ObjectDecisionType lateral_decision_;ObjectDecisionType longitudinal_decision_;// for keep_clear usage onlybool is_blocking_obstacle_ = false;bool is_lane_blocking_ = false;bool is_lane_change_blocking_ = false;bool is_caution_level_obstacle_ = false;double min_radius_stop_distance_ = -1.0;struct ObjectTagCaseHash {size_t operator()(const planning::ObjectDecisionType::ObjectTagCase tag) const {return static_cast<size_t>(tag);}};static const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,ObjectTagCaseHash>s_lateral_decision_safety_sorter_;static const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,ObjectTagCaseHash>s_longitudinal_decision_safety_sorter_;
};typedef IndexedList<std::string, Obstacle> IndexedObstacles;
typedef ThreadSafeIndexedList<std::string, Obstacle> ThreadSafeIndexedObstacles;} // namespace planning
} // namespace apolloobstacle.cc
#include "modules/planning/common/obstacle.h"#include <algorithm>
#include <utility>#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/util/map_util.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed/st_boundary.h"namespace apollo {namespace planning {using apollo::common::VehicleConfigHelper;
using apollo::common::util::FindOrDie;
using apollo::perception::PerceptionObstacle;
using apollo::prediction::ObstaclePriority;namespace { //又定义了个namespace防止其他程序引用该类时出现重名重复定义现象。
//定义了3个常量
//ST边界s的差值阈值 0.2m?
const double kStBoundaryDeltaS = 0.2; // meters
//ST边界的逃离s的差值 1.0m?
const double kStBoundarySparseDeltaS = 1.0; // meters
//ST边界的时间差值阈值0.05s
const double kStBoundaryDeltaT = 0.05; // seconds
} // namespace//定义了一个无序map s_longitudinal_decision_safety_sorter_，存放目标场景标签和纵向距离的映射？
//这个map的名字字面意义来看，纵向决策安全分类？
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,Obstacle::ObjectTagCaseHash>Obstacle::s_longitudinal_decision_safety_sorter_ = {{ObjectDecisionType::kIgnore, 0}, //忽略？{ObjectDecisionType::kOvertake, 100}, //超车{ObjectDecisionType::kFollow, 300}, //跟车{ObjectDecisionType::kYield, 400}, //避让{ObjectDecisionType::kStop, 500}}; //停车//定义了一个无序map s_lateral_decision_safety_sorter_，存放目标场景标签和横向距离的映射？
//这个map的名字字面意义来看，横向决策安全分类？
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,Obstacle::ObjectTagCaseHash>Obstacle::s_lateral_decision_safety_sorter_ = {//横向决策忽略，Nudge是靠边避让？{ObjectDecisionType::kIgnore, 0}, {ObjectDecisionType::kNudge, 100}};//Obstacle障碍物类的带参构造函数
//输入参数：障碍物id，感知障碍物类对象，障碍物优先级类对象，是否为静态障碍物？
Obstacle::Obstacle(const std::string& id,const PerceptionObstacle& perception_obstacle,const ObstaclePriority::Priority& obstacle_priority,const bool is_static)//用这些输入参数去初始化Obstacle类自己的数据成员，赋值数据成员id_: id_(id), //赋值数据成员感知障碍物perception_id_perception_id_(perception_obstacle.id()),//赋值数据成员感知障碍物对象perception_obstacle_perception_obstacle_(perception_obstacle),//赋值数据成员感知障碍物的边界盒perception_bounding_box_perception_bounding_box_({perception_obstacle_.position().x(),perception_obstacle_.position().y()},perception_obstacle_.theta(),perception_obstacle_.length(),perception_obstacle_.width()) {//将这个障碍物的注意等级先初始设定为CAUTION注意is_caution_level_obstacle_ = (obstacle_priority == ObstaclePriority::CAUTION);//定义一个2维多边形点vectorstd::vector<common::math::Vec2d> polygon_points;//FLAGS_use_navigation_mode是GFLAGS库的用法，FLAGS_代表去相应的XXX.gflags.cc里取//出use_navigation_mode的值，默认为false，或者多边形点的数量小于等于2//如果用导航模式或感知障碍物的边界多边形点的数目小于等于2，就获取感知障碍物边界盒的所有顶//点，顶点就是根据感知的长宽，几何中心，heading角计算而来if (FLAGS_use_navigation_mode ||perception_obstacle.polygon_point_size() <= 2) {perception_bounding_box_.GetAllCorners(&polygon_points);} else {//既不使用导航模式，感知障碍物的边界多边形点数目又大于2的话//把感知障碍物的多边形点放入vector polygon_pointsACHECK(perception_obstacle.polygon_point_size() > 2)<< "object " << id << "has less than 3 polygon points";for (const auto& point : perception_obstacle.polygon_point()) {polygon_points.emplace_back(point.x(), point.y());}}//计算感知障碍物多边形点集的凸包，得到的多边形结果存放在引用变量perception_polygon_感知多边形里ACHECK(common::math::Polygon2d::ComputeConvexHull(polygon_points,&perception_polygon_))<< "object[" << id << "] polygon is not a valid convex hull.\n"<< perception_obstacle.DebugString();//判断是否为静态 = 是否为静态 或 障碍物优先级为可忽略，就是障碍物可忽略的话也可直接视为静态障碍物is_static_ = (is_static || obstacle_priority == ObstaclePriority::IGNORE);//是否为虚拟障碍物，若感知障碍物id<0的话则为虚拟障碍物is_virtual_ = (perception_obstacle.id() < 0);//std::hypot 计算 x 和 y 的平方和的平方根,其实就是计算下感知障碍物速度X方向，Y方向//的矢量和speed_ = std::hypot(perception_obstacle.velocity().x(),perception_obstacle.velocity().y());
}//Obstacle类的带参构造函数重载，输入参数不同
//输入参数跟上面差不多，就多了一个预测轨迹类对象trajectory
Obstacle::Obstacle(const std::string& id,const PerceptionObstacle& perception_obstacle,const prediction::Trajectory& trajectory,const ObstaclePriority::Priority& obstacle_priority,const bool is_static)//输入参数除预测轨迹，其他参数直接调用上面的构造函数初始化数据成员: Obstacle(id, perception_obstacle, obstacle_priority, is_static) {//将输入参数预测轨迹trajectory赋值给数据成员trajectory_trajectory_ = trajectory;//取出预测轨迹的轨迹点trajectory_pointsauto& trajectory_points = *trajectory_.mutable_trajectory_point();//初始定义了一个累计纵向距离s cumulative_sdouble cumulative_s = 0.0;//如果轨迹点的数目大于0 轨迹点的第一个点的s设置为0，其实就//相对第一个轨迹点的相对纵坐标sif (trajectory_points.size() > 0) {trajectory_points[0].mutable_path_point()->set_s(0.0);}//从预测轨迹点的第二个点开始遍历设置相对纵坐标s，第一个点上面已经设置了for (int i = 1; i < trajectory_points.size(); ++i) {//前继点，第i-1个预测轨迹点prevconst auto& prev = trajectory_points[i - 1];//当前点，第i个预测轨迹点curconst auto& cur = trajectory_points[i];//如果前继点的相对时间 >= 当前点相对时间，报错if (prev.relative_time() >= cur.relative_time()) {AERROR << "prediction time is not increasing."<< "current point: " << cur.ShortDebugString()<< "previous point: " << prev.ShortDebugString();}//累计的纵向位置s=上次累计的纵向位置s + 第i-1个点到第i个点的距离cumulative_s +=common::util::DistanceXY(prev.path_point(), cur.path_point());//设定第i个点的相对起点的纵坐标strajectory_points[i].mutable_path_point()->set_s(cumulative_s);}
}//根据时间查询预测轨迹点
//输入参数是相对时间relative_time
common::TrajectoryPoint Obstacle::GetPointAtTime(const double relative_time) const {//首先去数据成员预测轨迹trajectory_上取出所有的预测轨迹点const auto& points = trajectory_.trajectory_point();//如果预测轨迹点个数小于2，那么轨迹点point的s,t,v,a直接设置为0，x,y,z,theta直接设置为当前值返回这个轨迹点if (points.size() < 2) {common::TrajectoryPoint point;point.mutable_path_point()->set_x(perception_obstacle_.position().x());point.mutable_path_point()->set_y(perception_obstacle_.position().y());point.mutable_path_point()->set_z(perception_obstacle_.position().z());point.mutable_path_point()->set_theta(perception_obstacle_.theta());point.mutable_path_point()->set_s(0.0);point.mutable_path_point()->set_kappa(0.0);point.mutable_path_point()->set_dkappa(0.0);point.mutable_path_point()->set_ddkappa(0.0);point.set_v(0.0);point.set_a(0.0);point.set_relative_time(0.0);return point;} else {//如果该障碍物的预测轨迹点数目 >= 2//这是函数内部又定义了个比较函数comp，比较函数的参数有轨迹点p，和时间time//若p的相对时间小于time就返回pauto comp = [](const common::TrajectoryPoint p, const double time) {return p.relative_time() < time;};//std::lower_bound的用法就是遍历预测轨迹点，lower_bound代表反向遍历，找到第一个小于给定时间的轨迹点//那正向遍历和反向遍历结果就不一样，正向遍历upper_bound就是找到轨迹点中的第一个点了，反向遍历lower_bound就是找到轨迹点中相对时间刚好小于给定时间的点，下一个点相对时间就要大于给定时间了//那么预测轨迹点里相对时间刚好小于输入参数给定相对时间relative_time的轨迹点复制给it_lowerauto it_lower =std::lower_bound(points.begin(), points.end(), relative_time, comp);//如果it_lower是预测轨迹里的第一个点/最后一个点就直接返回之if (it_lower == points.begin()) {return *points.begin();} else if (it_lower == points.end()) {return *points.rbegin();}//如果it_lower不是预测轨迹里的第一个点/最后一个点就用it_lower和其上一个点线性插值得到relative_time对应的轨迹点信息并返回return common::math::InterpolateUsingLinearApproximation(*(it_lower - 1), *it_lower, relative_time);}
}//获取障碍物的2维边界盒，输入参数是预测轨迹点？其实就是轨迹点作为几何中心点其x,y,theta信息加上数据成员感知障碍物的长宽构建二维边界盒
common::math::Box2d Obstacle::GetBoundingBox(const common::TrajectoryPoint& point) const {return common::math::Box2d({point.path_point().x(), point.path_point().y()},point.path_point().theta(),perception_obstacle_.length(),perception_obstacle_.width());
}//判断是否为有效的感知障碍物，输入参数是感知障碍物类对象
bool Obstacle::IsValidPerceptionObstacle(const PerceptionObstacle& obstacle) {//如果感知障碍物的长宽高<=0自然是无效的感知障碍物，直接报错返回if (obstacle.length() <= 0.0) {AERROR << "invalid obstacle length:" << obstacle.length();return false;}if (obstacle.width() <= 0.0) {AERROR << "invalid obstacle width:" << obstacle.width();return false;}if (obstacle.height() <= 0.0) {AERROR << "invalid obstacle height:" << obstacle.height();return false;}//如果感知障碍物有速度if (obstacle.has_velocity()) {//如果感知障碍物的X方向速度 或 Y方向速度只要有一个为nan(not a number)则直接返回falseif (std::isnan(obstacle.velocity().x()) ||std::isnan(obstacle.velocity().y())) {AERROR << "invalid obstacle velocity:"<< obstacle.velocity().DebugString();return false;}}//上面都不满足没有返回，那么遍历感知障碍物的多边形点，只要有一个点的x或y坐标为nan(not a number)就直接返回falsefor (auto pt : obstacle.polygon_point()) {if (std::isnan(pt.x()) || std::isnan(pt.y())) {AERROR << "invalid obstacle polygon point:" << pt.DebugString();return false;}}//上面情况都不满足，说明感知障碍物是个有效的障碍物return true;
}//创建障碍物类对象指针列表，输入参数是预测障碍物列表
//其实就是从预测障碍物对象中提取出有效的障碍物和有效的轨迹等信息，塞入障碍物类对下个指针列表返回
std::list<std::unique_ptr<Obstacle>> Obstacle::CreateObstacles(const prediction::PredictionObstacles& predictions) {//先定义一个空的障碍物类指针列表准备用来存放结果返回std::list<std::unique_ptr<Obstacle>> obstacles;//遍历预测障碍物列表for (const auto& prediction_obstacle : predictions.prediction_obstacle()) {//判断预测障碍物的成员感知障碍物是否是有效的感知障碍物，否则报错并跳到下一个障碍物if (!IsValidPerceptionObstacle(prediction_obstacle.perception_obstacle())) {AERROR << "Invalid perception obstacle: "<< prediction_obstacle.perception_obstacle().DebugString();continue;}//将第i个被遍历的预测障碍物的id，轨迹，优先级，是否为静态障碍物作为参数调用Obstacle类的构造函数，获得的对象塞到障碍物类对象指针列表obstaclesconst auto perception_id =std::to_string(prediction_obstacle.perception_obstacle().id());if (prediction_obstacle.trajectory().empty()) {obstacles.emplace_back(new Obstacle(perception_id, prediction_obstacle.perception_obstacle(),prediction_obstacle.priority().priority(),prediction_obstacle.is_static()));continue;}//遍历第i个预测障碍物的预测轨迹int trajectory_index = 0;for (const auto& trajectory : prediction_obstacle.trajectory()) {bool is_valid_trajectory = true;//遍历预测轨迹点for (const auto& point : trajectory.trajectory_point()) {//若第i个障碍物的预测轨迹点上有一个无效点，就设置is_valid_trajectory为无效轨迹，并且直接跳出针对第i个预测障碍物预测轨迹点的遍历循环if (!IsValidTrajectoryPoint(point)) {AERROR << "obj:" << perception_id<< " TrajectoryPoint: " << trajectory.ShortDebugString()<< " is NOT valid.";is_valid_trajectory = false;break;}}//若第i个预测障碍物的轨迹不是有效的轨迹，则直接跳到下一个障碍物if (!is_valid_trajectory) {continue;}//若执行到这里说明第i个障碍物有效，且其预测轨迹是有效的//那么就将其id,优先级，预测轨迹，是否为静态障碍物的信息截出来拿去调用Obstacle类的构造函数，得到的障碍物对象塞入障碍物对象列表obstacles待返回const std::string obstacle_id =absl::StrCat(perception_id, "_", trajectory_index);obstacles.emplace_back(new Obstacle(obstacle_id, prediction_obstacle.perception_obstacle(),trajectory, prediction_obstacle.priority().priority(),prediction_obstacle.is_static()));++trajectory_index;}}//返回障碍物列表return obstacles;
}//创建虚拟障碍物函数
//输入参数id，二维障碍物边界盒
std::unique_ptr<Obstacle> Obstacle::CreateStaticVirtualObstacles(const std::string& id, const common::math::Box2d& obstacle_box) {//创建一个虚拟的感知障碍物对象perception::PerceptionObstacle perception_obstacle;//id需要一个整数，虚拟障碍物是个负idsize_t negative_id = std::hash<std::string>{}(id);// set the first bit to 1 so negative_id became negative number//size_t是32位？这里将1左移31位，将32位最高位置1代表负数？然后虚拟障碍物id与之按位或negative_id |= (0x1 << 31);//感知障碍物设置id，几何中心x,y,heading,vx=0,vy=0,长宽高，类型为未知不可移动perception_obstacle.set_id(static_cast<int32_t>(negative_id));perception_obstacle.mutable_position()->set_x(obstacle_box.center().x());perception_obstacle.mutable_position()->set_y(obstacle_box.center().y());perception_obstacle.set_theta(obstacle_box.heading());perception_obstacle.mutable_velocity()->set_x(0);perception_obstacle.mutable_velocity()->set_y(0);perception_obstacle.set_length(obstacle_box.length());perception_obstacle.set_width(obstacle_box.width());perception_obstacle.set_height(FLAGS_virtual_stop_wall_height);perception_obstacle.set_type(perception::PerceptionObstacle::UNKNOWN_UNMOVABLE);//设置感知障碍物的跟踪时间1.0sperception_obstacle.set_tracking_time(1.0);//定义顶点坐标vectorstd::vector<common::math::Vec2d> corner_points;//获取输入参数二维边界盒的所有顶点放入corner_pointsobstacle_box.GetAllCorners(&corner_points);//遍历二维边界盒的所有顶点，将其顶点拷贝给感知障碍物对象的多边形点for (const auto& corner_point : corner_points) {auto* point = perception_obstacle.add_polygon_point();point->set_x(corner_point.x());point->set_y(corner_point.y());}//通过感知障碍物id,感知障碍物对象，障碍物优先级正常，true代表为静态障碍物//通过这些信息调用带参构造函数构建一个障碍物对象指针auto* obstacle =new Obstacle(id, perception_obstacle, ObstaclePriority::NORMAL, true);//设置其属性为虚拟障碍物obstacle->is_virtual_ = true;//将这个障碍物指针对象返回return std::unique_ptr<Obstacle>(obstacle);
}//判断是否为有效的轨迹点，输入参数是一个轨迹点类型对象
//只要轨迹点没有路径点 或 x/y/z/kappa/s/dkappa/ddkappa/v/a/relative_time里只要有
//一个为nan(not a number)就说明这个轨迹点是无效的，否则有效
bool Obstacle::IsValidTrajectoryPoint(const common::TrajectoryPoint& point) {return !((!point.has_path_point()) || std::isnan(point.path_point().x()) ||std::isnan(point.path_point().y()) ||std::isnan(point.path_point().z()) ||std::isnan(point.path_point().kappa()) ||std::isnan(point.path_point().s()) ||std::isnan(point.path_point().dkappa()) ||std::isnan(point.path_point().ddkappa()) || std::isnan(point.v()) ||std::isnan(point.a()) || std::isnan(point.relative_time()));
}//设置障碍物的SL边界也就是Frenet系的横纵坐标的边界类对象sl_boundary
//输入参数就是SLBoundary SL边界类对象
//就是将参数拷贝给类数据成员sl_boundary_ 其实就是障碍物边界点相对于道路参考线的SL坐标
void Obstacle::SetPerceptionSlBoundary(const SLBoundary& sl_boundary) {sl_boundary_ = sl_boundary;
}//使用自车的最小转弯半径计算停车距离？输入参数是车辆参数类对象
//计算的原理大致如博客末尾附图
double Obstacle::MinRadiusStopDistance(const common::VehicleParam& vehicle_param) const {//如果针对这个障碍物的最小转弯半径的停车距离>0则直接返回？是说明已经设置了吗？if (min_radius_stop_distance_ > 0) {return min_radius_stop_distance_;}//定义一个停止距离的缓冲区0.5mstatic constexpr double stop_distance_buffer = 0.5;//获取车辆参数的最小转弯半径const double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();//这个是计算出自车到达横向边界上再加上自车宽度一半的横向偏移？//sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标double lateral_diff =vehicle_param.width() / 2.0 + std::max(std::fabs(sl_boundary_.start_l()),std::fabs(sl_boundary_.end_l()));//又定义了一个很小的正常数const double kEpison = 1e-5;lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison);//计算的原理大致如博客末尾附图，利用勾股定理计算计算最小转弯半径下的停车距离double stop_distance =std::sqrt(std::fabs(min_turn_radius * min_turn_radius -(min_turn_radius - lateral_diff) *(min_turn_radius - lateral_diff))) +stop_distance_buffer;stop_distance -= vehicle_param.front_edge_to_center();//对这个停车距离再进行限幅并返回stop_distance = std::min(stop_distance, FLAGS_max_stop_distance_obstacle);stop_distance = std::max(stop_distance, FLAGS_min_stop_distance_obstacle);return stop_distance;
}//针对障碍物对象建立自车参考线的ST边界，输入参数是道路参考线类对象，以及adc自车的起始s坐标，将障碍物投影到自车的ST图上
//文字不足以描述，详细原理见文末附图
void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,const double adc_start_s) {//获取ADC 自动驾驶车辆的物理参数配置const auto& adc_param =VehicleConfigHelper::Instance()->GetConfig().vehicle_param();//获取车辆物理参数中的宽度const double adc_width = adc_param.width();//如果障碍物类成员表明其是静态障碍物或障碍物预测轨迹点为空，其实就是个静止障碍物？if (is_static_ || trajectory_.trajectory_point().empty()) {//定义一个存放STPoint点对的vetor point_pairsstd::vector<std::pair<STPoint, STPoint>> point_pairs;//sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标//获取障碍物在参考线上对应的起始s坐标，其实就是障碍物的尾部边界点？double start_s = sl_boundary_.start_s();//获取障碍物在参考线上对应的终点s坐标，其实就是障碍物的头部边界点？double end_s = sl_boundary_.end_s();//如果终点s-起点s<阈值0.2m，那么终点s=起点s+0.2mif (end_s - start_s < kStBoundaryDeltaS) {end_s = start_s + kStBoundaryDeltaS;}//参考线道路被阻塞？输入参数是感知障碍物边界盒，自车的宽度//检查是否有边界盒堵塞了路面。标准是检查路面上的剩余空间是否大于自车宽度。if (!reference_line.IsBlockRoad(perception_bounding_box_, adc_width)) {return;}//往ST边界里塞ST点对，起点t 0.0//S分别塞入(障碍物SL边界起始点s-自车的起始s，0.0) (SL边界终点s-自车的起始s)//其实就是这个障碍物的S边界上下限距离自车起始点s的距离//相对时间0对应的S边界就是自车到障碍物头部和尾部纵向位置的差值？这个边界其实还是//障碍物的头部尾部的相对纵向位置s投影到自车参考线上//其实就是把障碍物投影到自车的ST图上？//相对时间0也就是当下，直接把现在障碍物头部尾部所处位置直接放入ST图point_pairs.emplace_back(STPoint(start_s - adc_start_s, 0.0),STPoint(end_s - adc_start_s, 0.0)); //因为这是一个静态障碍物(在这个大if里说明是静态障碍物) ，障碍物会阻塞参考线//s轴上相对自车位置的s坐标8s(最大ST规划时间？)//认为这8s该障碍物头部尾部会一直占据start_s到end_s位置 point_pairs.emplace_back(STPoint(start_s - adc_start_s, FLAGS_st_max_t),STPoint(end_s - adc_start_s, FLAGS_st_max_t));//障碍物类成员参考线ST边界就为这个障碍物相对纵向位置占据8sreference_line_st_boundary_ = STBoundary(point_pairs);} else { //不是静态障碍物的话，调用建立轨迹ST边界函数，输入参数是道路参考线，自车的起始s，求得的st边界放入reference_line_st_boundary_，起始就是对于这个障碍物对象将其的影响考虑成对参考线ST边界的影响上，其实就是将动态障碍物映射到ST图上？if (BuildTrajectoryStBoundary(reference_line, adc_start_s,&reference_line_st_boundary_)) {ADEBUG << "Found st_boundary for obstacle " << id_;ADEBUG << "st_boundary: min_t = " << reference_line_st_boundary_.min_t()<< ", max_t = " << reference_line_st_boundary_.max_t()<< ", min_s = " << reference_line_st_boundary_.min_s()<< ", max_s = " << reference_line_st_boundary_.max_s();} else {ADEBUG << "No st_boundary for obstacle " << id_;}}
}//建立轨迹的ST边界
//输入参数 参考线类对象，自车起始的frenet系纵坐标s，第三个参数用以存放得到的结果
//这个函数的实现代码没太看明白，大致就是根据参考线，自车的纵向坐标s，用动态感知障碍物对象及其预测轨迹去修改之前的自车参考线ST边界，起始就是用该障碍物信息去修正ST图中搜索的可行域？将动态障碍物投影到自车的ST图上。
bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,const double adc_start_s,STBoundary* const st_boundary) {//如果不是有效的感知障碍物直接返回falseif (!IsValidObstacle(perception_obstacle_)) {AERROR << "Fail to build trajectory st boundary because object is not ""valid. PerceptionObstacle: "<< perception_obstacle_.DebugString();return false;}//获取这个障碍物的长宽，以及预测轨迹点const double object_width = perception_obstacle_.width();const double object_length = perception_obstacle_.length();const auto& trajectory_points = trajectory_.trajectory_point();//如果预测轨迹点为空直接返回falseif (trajectory_points.empty()) {AWARN << "object " << id_ << " has no trajectory points";return false;}//获取自车的物理参数，长度，长度一半，宽度const auto& adc_param =VehicleConfigHelper::Instance()->GetConfig().vehicle_param();const double adc_length = adc_param.length();const double adc_half_length = adc_length / 2.0;const double adc_width = adc_param.width();//这里创建了两个边界盒对象最小盒，最大盒？没看到在哪里用？//Box2d构造函数，center中心点，heading,length,widthcommon::math::Box2d min_box({0, 0}, 1.0, 1.0, 1.0);common::math::Box2d max_box({0, 0}, 1.0, 1.0, 1.0);//ST点，就是纵向位置s和时间t构成的点(s,t)//又创建了一个ST点对的vector polygon_points,多边形点？std::vector<std::pair<STPoint, STPoint>> polygon_points;//又创建了一个SL边界类对象，last_sl_boundary上一次的sl边界？SLBoundary last_sl_boundary;//初始定义了上次的索引为0 last_indexint last_index = 0;//从障碍物预测轨迹的第二个点开始遍历for (int i = 1; i < trajectory_points.size(); ++i) {ADEBUG << "last_sl_boundary: " << last_sl_boundary.ShortDebugString();//traj轨迹 = path路径 + 速度规划//障碍物第i-1个预测轨迹点const auto& first_traj_point = trajectory_points[i - 1];//障碍物第i个预测轨迹点const auto& second_traj_point = trajectory_points[i];//障碍物第i-1个预测轨迹点上的路径点const auto& first_point = first_traj_point.path_point();//障碍物第i个预测轨迹点上的路径点const auto& second_point = second_traj_point.path_point();//定义了一个物体移动边界盒长度 = 物体本身长度 + 两个轨迹点之间的长度，又准备搞障碍物第//i个轨迹点的边界盒了double object_moving_box_length =object_length + common::util::DistanceXY(first_point, second_point);//定义该障碍物(Obstacle类实例)第i个轨迹点的边界盒的中心，就是第i个，i-1轨迹点的中点common::math::Vec2d center((first_point.x() + second_point.x()) / 2.0,(first_point.y() + second_point.y()) / 2.0);//构建一个2维边界盒对象 object_moving_box物体移动边界盒，center,heading,length,width,headingcommon::math::Box2d object_moving_box(center, first_point.theta(), object_moving_box_length, object_width);//又定义了SL边界类对象 object_boundary 目标边界？SLBoundary object_boundary;//Apollo原注释//注意：这种方法在参考线不是直的时候有误差，需要双回路来cover所有的corner cases。//粗糙的跳过距离上次sl边界盒过近的点？//这里就是计算障碍物第i预测轨迹点和第i-1个点之间的距离//上一个点的索引并不是单纯的i-1而是由last_index控制，如果可以忽略的情况下last_index并不会递增const double distance_xy =common::util::DistanceXY(trajectory_points[last_index].path_point(),trajectory_points[i].path_point());//大致意思是障碍物从i-1个点挪动到第i个轨迹点，横向上变化量假设最恶劣变化distance_xy两点间直线距离，都没有超过上一次的SL边界，就直接跳到下一个轨迹点？这块原理不是特别清楚？if (last_sl_boundary.start_l() > distance_xy ||last_sl_boundary.end_l() < -distance_xy) {continue;}//上一次SL边界的纵向中点const double mid_s =(last_sl_boundary.start_s() + last_sl_boundary.end_s()) / 2.0;const double start_s = std::fmax(0.0, mid_s - 2.0 * distance_xy);const double end_s = (i == 1) ? reference_line.Length(): std::fmin(reference_line.Length(),mid_s + 2.0 * distance_xy);if (!reference_line.GetApproximateSLBoundary(object_moving_box, start_s,end_s, &object_boundary)) {AERROR << "failed to calculate boundary";return false;}// update history recordlast_sl_boundary = object_boundary;last_index = i;//跳过障碍物，如果它在道路参考线的一边的话，在横向上相对参考线的偏移大于//自车车宽一半+障碍物车长*0.4那么就忽略这个轨迹点static constexpr double kSkipLDistanceFactor = 0.4;const double skip_l_distance =(object_boundary.end_s() - object_boundary.start_s()) *kSkipLDistanceFactor +adc_width / 2.0;
//跳过障碍物，如果它在道路参考线的一边的话，在横向上相对参考线的偏移大于//自车车宽一半+障碍物车长*0.4那么就忽略这个轨迹点if (!IsCautionLevelObstacle() &&(std::fmin(object_boundary.start_l(), object_boundary.end_l()) >skip_l_distance ||std::fmax(object_boundary.start_l(), object_boundary.end_l()) <-skip_l_distance)) {continue;}//如果障碍物的level不是Caution需要注意这个级别或者障碍物头部都在自车后方，这个轨迹点也可忽略if (!IsCautionLevelObstacle() && object_boundary.end_s() < 0) {// skip if behind reference linecontinue;}static constexpr double kSparseMappingS = 20.0;const double st_boundary_delta_s =(std::fabs(object_boundary.start_s() - adc_start_s) > kSparseMappingS)? kStBoundarySparseDeltaS: kStBoundaryDeltaS;const double object_s_diff =object_boundary.end_s() - object_boundary.start_s();if (object_s_diff < st_boundary_delta_s) {continue;}const double delta_t =second_traj_point.relative_time() - first_traj_point.relative_time();double low_s = std::max(object_boundary.start_s() - adc_half_length, 0.0);bool has_low = false;double high_s =std::min(object_boundary.end_s() + adc_half_length, FLAGS_st_max_s);bool has_high = false;while (low_s + st_boundary_delta_s < high_s && !(has_low && has_high)) {if (!has_low) {auto low_ref = reference_line.GetReferencePoint(low_s);has_low = object_moving_box.HasOverlap({low_ref, low_ref.heading(), adc_length,adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});low_s += st_boundary_delta_s;}if (!has_high) {auto high_ref = reference_line.GetReferencePoint(high_s);has_high = object_moving_box.HasOverlap({high_ref, high_ref.heading(), adc_length,adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});high_s -= st_boundary_delta_s;}}if (has_low && has_high) {low_s -= st_boundary_delta_s;high_s += st_boundary_delta_s;double low_t =(first_traj_point.relative_time() +std::fabs((low_s - object_boundary.start_s()) / object_s_diff) *delta_t);polygon_points.emplace_back(std::make_pair(STPoint{low_s - adc_start_s, low_t},STPoint{high_s - adc_start_s, low_t}));double high_t =(first_traj_point.relative_time() +std::fabs((high_s - object_boundary.start_s()) / object_s_diff) *delta_t);if (high_t - low_t > 0.05) {polygon_points.emplace_back(std::make_pair(STPoint{low_s - adc_start_s, high_t},STPoint{high_s - adc_start_s, high_t}));}}}if (!polygon_points.empty()) {std::sort(polygon_points.begin(), polygon_points.end(),[](const std::pair<STPoint, STPoint>& a,const std::pair<STPoint, STPoint>& b) {return a.first.t() < b.first.t();});auto last = std::unique(polygon_points.begin(), polygon_points.end(),[](const std::pair<STPoint, STPoint>& a,const std::pair<STPoint, STPoint>& b) {return std::fabs(a.first.t() - b.first.t()) <kStBoundaryDeltaT;});polygon_points.erase(last, polygon_points.end());if (polygon_points.size() > 2) {*st_boundary = STBoundary(polygon_points);}} else {return false;}return true;
}//获取数据成员 参考线ST边界
const STBoundary& Obstacle::reference_line_st_boundary() const {return reference_line_st_boundary_;
}
//获取数据成员 路径ST边界
const STBoundary& Obstacle::path_st_boundary() const {return path_st_boundary_;
}//返回数据成员，决策标签？一个字符串的vector,decider_tags_
const std::vector<std::string>& Obstacle::decider_tags() const {return decider_tags_;
}//返回目标决策类型列表？
const std::vector<ObjectDecisionType>& Obstacle::decisions() const {return decisions_;
}//nudge在apollo里代表横向上轻轻绕一下避开？
//判断目标决策类型对象是横向决策？如果有ignore或nudge就认为是
bool Obstacle::IsLateralDecision(const ObjectDecisionType& decision) {return decision.has_ignore() || decision.has_nudge();
}//是否为纵向决策
//有ignore，有stop/yield/follow/overtake都是
bool Obstacle::IsLongitudinalDecision(const ObjectDecisionType& decision) {return decision.has_ignore() || decision.has_stop() || decision.has_yield() ||decision.has_follow() || decision.has_overtake();
}//融合两个纵向决策类型对象，保留更保守的那个
//停车/跟车/减速避让都保留纵向距离更大的那个
//超车保留纵向距离更大的那个
ObjectDecisionType Obstacle::MergeLongitudinalDecision(const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return rhs;}if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return lhs;}//portobuf库FindOrDie：查找map容器中指定key的value值地址，否则抛出FatalException异常或终止进程const auto lhs_val =FindOrDie(s_longitudinal_decision_safety_sorter_, lhs.object_tag_case());const auto rhs_val =FindOrDie(s_longitudinal_decision_safety_sorter_, rhs.object_tag_case());if (lhs_val < rhs_val) {return rhs;} else if (lhs_val > rhs_val) {return lhs;} else {if (lhs.has_ignore()) {return rhs;} else if (lhs.has_stop()) {return lhs.stop().distance_s() < rhs.stop().distance_s() ? lhs : rhs;} else if (lhs.has_yield()) {return lhs.yield().distance_s() < rhs.yield().distance_s() ? lhs : rhs;} else if (lhs.has_follow()) {return lhs.follow().distance_s() < rhs.follow().distance_s() ? lhs : rhs;} else if (lhs.has_overtake()) {return lhs.overtake().distance_s() > rhs.overtake().distance_s() ? lhs: rhs;} else {DCHECK(false) << "Unknown decision";}}return lhs; // stop compiler complaining
}//返回Obstacle类数据成员，针对这个障碍物的纵向决策
const ObjectDecisionType& Obstacle::LongitudinalDecision() const {return longitudinal_decision_;
}
//返回Obstacle类数据成员，针对这个障碍物的横向决策
const ObjectDecisionType& Obstacle::LateralDecision() const {return lateral_decision_;
}//障碍物可被忽略？调用函数判断横/纵向均可被忽略？
bool Obstacle::IsIgnore() const {return IsLongitudinalIgnore() && IsLateralIgnore();
}//判断该障碍物纵向决策是否为可忽略？
bool Obstacle::IsLongitudinalIgnore() const {return longitudinal_decision_.has_ignore();
}//判断该障碍物横向决策是否为可忽略？
bool Obstacle::IsLateralIgnore() const {return lateral_decision_.has_ignore();
}//融合两个横向的目标决策类型对象，保留避让nudge距离大的那个横向目标决策类型对象并返回
ObjectDecisionType Obstacle::MergeLateralDecision(const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return rhs;}if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return lhs;}const auto lhs_val =FindOrDie(s_lateral_decision_safety_sorter_, lhs.object_tag_case());const auto rhs_val =FindOrDie(s_lateral_decision_safety_sorter_, rhs.object_tag_case());if (lhs_val < rhs_val) {return rhs;} else if (lhs_val > rhs_val) {return lhs;} else {if (lhs.has_ignore()) {return rhs;} else if (lhs.has_nudge()) {DCHECK(lhs.nudge().type() == rhs.nudge().type())<< "could not merge left nudge and right nudge";return std::fabs(lhs.nudge().distance_l()) >std::fabs(rhs.nudge().distance_l())? lhs: rhs;}}DCHECK(false) << "Does not have rule to merge decision: "<< lhs.ShortDebugString()<< " and decision: " << rhs.ShortDebugString();return lhs;
}//检查针对该障碍物是否有横向决策？
bool Obstacle::HasLateralDecision() const {return lateral_decision_.object_tag_case() !=ObjectDecisionType::OBJECT_TAG_NOT_SET;
}//检查针对该障碍物是否有纵向决策？
bool Obstacle::HasLongitudinalDecision() const {return longitudinal_decision_.object_tag_case() !=ObjectDecisionType::OBJECT_TAG_NOT_SET;
}//针对该障碍物的决策是否不可忽略
bool Obstacle::HasNonIgnoreDecision() const {return (HasLateralDecision() && !IsLateralIgnore()) ||(HasLongitudinalDecision() && !IsLongitudinalIgnore());
}//针对该障碍物增加一个纵向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标纵向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个
void Obstacle::AddLongitudinalDecision(const std::string& decider_tag,const ObjectDecisionType& decision) {DCHECK(IsLongitudinalDecision(decision))<< "Decision: " << decision.ShortDebugString()<< " is not a longitudinal decision";longitudinal_decision_ =MergeLongitudinalDecision(longitudinal_decision_, decision);ADEBUG << decider_tag << " added obstacle " << Id()<< " longitudinal decision: " << decision.ShortDebugString()<< ". The merged decision is: "<< longitudinal_decision_.ShortDebugString();decisions_.push_back(decision);decider_tags_.push_back(decider_tag);
}//针对该障碍物增加一个横向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标横向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个
void Obstacle::AddLateralDecision(const std::string& decider_tag,const ObjectDecisionType& decision) {DCHECK(IsLateralDecision(decision))<< "Decision: " << decision.ShortDebugString()<< " is not a lateral decision";lateral_decision_ = MergeLateralDecision(lateral_decision_, decision);ADEBUG << decider_tag << " added obstacle " << Id()<< " a lateral decision: " << decision.ShortDebugString()<< ". The merged decision is: "<< lateral_decision_.ShortDebugString();decisions_.push_back(decision);decider_tags_.push_back(decider_tag);
}//返回debug字符串
std::string Obstacle::DebugString() const {std::stringstream ss;ss << "Obstacle id: " << id_;for (size_t i = 0; i < decisions_.size(); ++i) {ss << " decision: " << decisions_[i].DebugString() << ", made by "<< decider_tags_[i];}if (lateral_decision_.object_tag_case() !=ObjectDecisionType::OBJECT_TAG_NOT_SET) {ss << "lateral decision: " << lateral_decision_.ShortDebugString();}if (longitudinal_decision_.object_tag_case() !=ObjectDecisionType::OBJECT_TAG_NOT_SET) {ss << "longitudinal decision: "<< longitudinal_decision_.ShortDebugString();}return ss.str();
}//获取针对该感知障碍物的SL边界盒？
//sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标
const SLBoundary& Obstacle::PerceptionSLBoundary() const {return sl_boundary_;
}//设定路径的ST边界，也就是针对该障碍物的ST边界？
void Obstacle::set_path_st_boundary(const STBoundary& boundary) {path_st_boundary_ = boundary;path_st_boundary_initialized_ = true;
}//设定ST边界类型，设置为输入的类型
void Obstacle::SetStBoundaryType(const STBoundary::BoundaryType type) {path_st_boundary_.SetBoundaryType(type);
}//擦除类里储存的路径ST边界
void Obstacle::EraseStBoundary() { path_st_boundary_ = STBoundary(); }//设定道路参考线的ST边界
void Obstacle::SetReferenceLineStBoundary(const STBoundary& boundary) {reference_line_st_boundary_ = boundary;
}//设定道路参考线ST边界类型
void Obstacle::SetReferenceLineStBoundaryType(const STBoundary::BoundaryType type) {reference_line_st_boundary_.SetBoundaryType(type);
}//擦除类里储存的道路参考线ST边界
void Obstacle::EraseReferenceLineStBoundary() {reference_line_st_boundary_ = STBoundary();
}//是否为有效的障碍物，主要是看感知障碍物的长宽是否为nan(not a number)或者过于小了，是的话就说明是无效的障碍物
bool Obstacle::IsValidObstacle(const perception::PerceptionObstacle& perception_obstacle) {const double object_width = perception_obstacle.width();const double object_length = perception_obstacle.length();const double kMinObjectDimension = 1.0e-6;return !std::isnan(object_width) && !std::isnan(object_length) &&object_width > kMinObjectDimension &&object_length > kMinObjectDimension;
}//检查车道被该障碍物阻塞？输入参数是道路参考线类对象
void Obstacle::CheckLaneBlocking(const ReferenceLine& reference_line) {//如果障碍物不是静止，就没阻塞，并返回if (!IsStatic()) {is_lane_blocking_ = false;return;}//如果执行到这里说明障碍物已经是是静态障碍物了DCHECK(sl_boundary_.has_start_s());DCHECK(sl_boundary_.has_end_s());DCHECK(sl_boundary_.has_start_l());DCHECK(sl_boundary_.has_end_l());//sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标
//如果根据该障碍物的SL边界
//SL边界是相对道路参考线的，若求出的SL边界下限L坐标和上限L坐标异号，说明障碍物SL边界盒占据了道路参考线的两边，则认为是阻塞了if (sl_boundary_.start_l() * sl_boundary_.end_l() < 0.0) {is_lane_blocking_ = true;return;}//如果上面没有返回，说明障碍物是在道路参考线的一侧，且是静态障碍物//驾驶宽度 = 道路参考线计算刨去障碍物边界盒SL边界后左右宽度中更大的那个,也就是障碍物的左边界const double driving_width = reference_line.GetDrivingWidth(sl_boundary_);//获取车辆物理参数auto vehicle_param = common::VehicleConfigHelper::GetConfig().vehicle_param();//根据道路参考线判断sl_boundary_也就是该障碍物的SL边界盒在车道内？且驾驶宽度<小于车辆的宽度+静态横向避让缓冲距离0.3，道路阻塞的标志位is_lane_blocking_为trueif (reference_line.IsOnLane(sl_boundary_) &&driving_width <vehicle_param.width() + FLAGS_static_obstacle_nudge_l_buffer) {is_lane_blocking_ = true;return;}//上面都不满足的话，该障碍物就没有阻塞车道is_lane_blocking_ = false;
}//设置数据成员，该障碍物阻塞换道？用输入参数 距离过近is_distance_clear拷贝给数据成员
//is_lane_change_blocking_
void Obstacle::SetLaneChangeBlocking(const bool is_distance_clear) {is_lane_change_blocking_ = is_distance_clear;
}} // namespace planning
} // namespace apollo附录
最小转弯半径避让停车距离计算函数
将障碍物投影到自车ST图(未完全看懂apollo实现细节)
这里的adc_start_s是指自车边界盒尾部的s坐标?障碍物占据的S上下边界都是相对自车尾部边界的纵向距离？真正的停车距离还要考虑一个自身车长？这块具体怎么在ST图中应用，看完ST图相关代码应该就清楚了。
百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\Obstacle类代码详解相关推荐
- Apollo自动驾驶开发笔记47——apollo编译报错this rule is missing dependency declarations for the following files
Apollo自动驾驶开发笔记47--apollo编译报错this rule is missing dependency declarations for the following files 报错信 ...
- 百度语音+自动驾驶感知+深度学习平台技术解析
HIEV快讯(文/戒僧)本文将解析三部分技术内容,出自百度2023 Create大会-技术开放日: •百度如何用"手机全双工语音交互"改善使用导航应用的体验 •如何用"上 ...
- 百度Apollo自动驾驶感知模块学习笔记、入门
3个跟HM对象跟踪相关的对象类 Object.TrackedObject.ObjectTrack Object类:感知到的车辆信息.包含物体的点云.多边形轮廓(Polygon).类别(vehicle, ...
- 聊聊自动驾驶必须解决哪些感知问题？交通标志识别技术详解
交通标志识别系统是智能交通系统与先进辅助驾驶系统的重要组成部分,由于道路交通较为复杂,提高交通检测与识别算法的准确率和实时性是走向实际应用进程中需要解决的关键问题. 算法的准确率是交通标志识别研究中一 ...
- [自动调参]深度学习模型的超参数自动化调优详解
向AI转型的程序员都关注了这个号
- 百度Apollo自动驾驶学习笔记
Apollo学习笔记 作者:邹镇洪(清华大学车辆学院,个人主页 转到Github项目主页查看持续更新 转到Github项目主页查看持续更新 转到Github项目主页查看持续更新 本文是对百度Apoll ...
- apollo自动驾驶进阶学习之：canbus模块代码分析
文章目录 封面 代码架构 内容结构 封面 apollo自动驾驶:canbus模块代码讲解及测试(1)引言 apollo自动驾驶:canbus模块代码讲解及测试(2)框架讲解 代码架构 但是apollo ...
- 解析百度Apollo自动驾驶平台
最近对百度的自动驾驶平台Apollo项目做了一些了解.下面将我所了解到的一些信息分享给大家. Apollo项目介绍 阿波罗(Apollo)是百度发布的面向汽车行业及自动驾驶领域的合作伙伴提供的软件平台 ...
- 百度Apollo自动驾驶专题讲座笔记之运动规划模块
在百度技术学院有Apollo的技术专题课程,对各个模块都有一个入门级的课程,对于了解各个模块间的相互作用关系有很大的作用,很适合对自动驾驶领域感兴趣的人的入门课程.感谢百度Apollo开放了这么好的课 ...
最新文章
- ArcGIS实验教程——实验二十五：大型商场选址经典案例
- mysql数据库的三级模式_2016年计算机三级MySQL数据库试题
- byte 类型比较_基本类型和包装类对象使用 == 和 equals进行比较的结果？
- Linux高性能server规划——处理池和线程池
- Spring Security OAuth2.0_实现分布式认证授权_扩展用户信息_Spring Security OAuth2.0认证授权---springcloud工作笔记156
- STM32工作笔记0009---认识FSMC和TTL电路
- Windows下打印服务器的管理（二）
- 2021-08-26 转载 Scala快速入门系列博客文章
- Codeforces.612E.Square Root of Permutation(构造)
- Java将对象保存到文件中/从文件中读取对象
- 山西2019数据结构专升本_喜讯！临汾这个学院专升本通过率创新高
- SLAM学习笔记-------------（八）视觉里程计2
- 七个国外免费杀毒软件
- matlab画图的参数,matlab画图参数
- java怎么缓存数据_java中的缓存技术该如何实现
- 计算机英语 1000字论文范文,英语论文格式写作 1000字论文格式-免费论文范文
- GRUB4DOS资源索引帖，欢迎跟帖补充
- 思科网络安全 第四章测验答案
- 如何制作打首板的量化策略
- 凡亿教育发布电子工程师利器:IC封装网 V.1.0