Obstacle類是apollo planning模塊下modules\planning\common\obstacle.cc/.h實現(xiàn)

從名字來看，是障礙物類，表示將一個障礙物的所有相關(guān)信息封裝成這個類。

成員變量

成員類型變量名說明

std::string	id_;	障礙物 id
int32_t	perception_id_ = 0;	感知障礙物id
bool	is_static_ = false;	障礙物是否為靜止
bool	is_virtual_ = false;	障礙物是否為虛擬的
double	speed_ = 0.0;	障礙物的速度
bool	path_st_boundary_initialized_ = false;	st邊界初始化標(biāo)志位
prediction::Trajectory	trajectory_;	障礙物的預(yù)測軌跡
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_;	決策器標(biāo)簽
SLBoundary	sl_boundary_;	st邊界
STBoundary	reference_line_st_boundary_;	參考線st邊界
STBoundary	path_st_boundary_;	路徑st邊界
ObjectDecisionType	lateral_decision_;	橫向決策
ObjectDecisionType	longitudinal_decision_; 　縱向決策
bool	is_blocking_obstacle_ = false;	是不是阻塞了
bool	is_lane_blocking_ = false;	車道阻斷標(biāo)志位
bool	is_lane_change_blocking_ = false;	交通阻斷標(biāo)志位
bool	is_caution_level_obstacle_ = false;	注意級標(biāo)志位
double	min_radius_stop_distance_ = -1.0;	最小半徑停止距離

障礙物的基本信息包括 id、速度、BoundingBox、預(yù)測軌跡；

除上述信息外，Obstacle 類成員變量還包括決策信息、SL/ST邊界

成員函數(shù)

//返回障礙物是否有軌跡？bool HasTrajectory() const {return !(trajectory_.trajectory_point().empty());}//返回感知障礙物對象，屬于Obstacle類成員const perception::PerceptionObstacle& Perception() const {return perception_obstacle_;}//判斷是否為有效的感知障礙物，輸入?yún)?shù)是感知障礙物類對象static bool IsValidPerceptionObstacle(const perception::PerceptionObstacle& obstacle);//判斷是否為有效的軌跡點(diǎn)，輸入?yún)?shù)是一個軌跡點(diǎn)類型對象 //只要軌跡點(diǎn)沒有路徑點(diǎn) 或 x/y/z/kappa/s/dkappa/ddkappa/v/a/relative_time里只要有 //一個為nan(not a number)就說明這個軌跡點(diǎn)是無效的，否則有效static bool IsValidTrajectoryPoint(const common::TrajectoryPoint& point);//內(nèi)聯(lián)函數(shù)，返回障礙物是否為注意等級的障礙物？inline bool IsCautionLevelObstacle() const {return is_caution_level_obstacle_;}/*** return 融合的橫向決策（成員函數(shù)）* Lateral decision is one of {Nudge, Ignore}**/ //返回Obstacle類數(shù)據(jù)成員，針對這個障礙物的橫向決策const ObjectDecisionType& LateralDecision() const;/*** @brief return 融合的縱向決策（成員函數(shù)）* Longitudinal decision is one of {Stop, Yield, Follow, Overtake, Ignore}**/const ObjectDecisionType& LongitudinalDecision() const;//返回debug字符串std::string DebugString() const;/獲取該感知障礙物的sl邊界盒？是感知障礙物邊界在參考線上的投影？const SLBoundary& PerceptionSLBoundary() const;//獲取數(shù)據(jù)成員 參考線ST邊界const STBoundary& reference_line_st_boundary() const;//獲取數(shù)據(jù)成員 路徑ST邊界，是指該障礙物所占據(jù)的的路徑邊界？const STBoundary& path_st_boundary() const;//返回數(shù)據(jù)成員，決策標(biāo)簽？一個字符串的vector,decider_tags_const std::vector<std::string>& decider_tags() const;//返回目標(biāo)決策類型列表？const std::vector<ObjectDecisionType>& decisions() const;//針對該障礙物增加一個橫向決策，輸入?yún)?shù)決策標(biāo)簽字符串decider_tag //以及目標(biāo)決策類型對象decision //其實就是將輸入的目標(biāo)橫向決策類型對象與之前類里儲存的進(jìn)行融合。 //類的數(shù)據(jù)成員決策列表decisions_和決策標(biāo)簽列表decider_tags_里再增加一個void AddLateralDecision(const std::string& decider_tag,const ObjectDecisionType& decision);//檢查針對該障礙物是否有橫向決策？bool HasLateralDecision() const;//設(shè)定路徑的ST邊界，也就是針對該障礙物的ST邊界？void set_path_st_boundary(const STBoundary& boundary);//返回數(shù)據(jù)成員 路徑ST邊界被初始化？bool is_path_st_boundary_initialized() {return path_st_boundary_initialized_;}//設(shè)置ST邊界類型？void SetStBoundaryType(const STBoundary::BoundaryType type);//擦除類里儲存的路徑ST邊界void EraseStBoundary();//設(shè)定道路參考線的ST邊界void SetReferenceLineStBoundary(const STBoundary& boundary);//設(shè)定道路參考線ST邊界類型void SetReferenceLineStBoundaryType(const STBoundary::BoundaryType type);//擦除類里儲存的道路參考線ST邊界void EraseReferenceLineStBoundary();//檢查針對該障礙物是否有縱向決策？bool HasLongitudinalDecision() const;//針對該障礙物的決策是否不可忽略bool HasNonIgnoreDecision() const;/*** @brief* 用自車的最小轉(zhuǎn)彎半徑去計算到該障礙物的停止距離*/double MinRadiusStopDistance(const common::VehicleParam& vehicle_param) const;/*** @brief* 檢查該障礙物是否可以被安全的忽略* 目標(biāo)可以被忽略僅當(dāng)縱向和橫向決策都是可以忽略的時候*/bool IsIgnore() const;bool IsLongitudinalIgnore() const;bool IsLateralIgnore() const;//針對障礙物對象建立自車參考線的ST邊界，輸入?yún)?shù)是道路參考線類對象，以及adc自車的起始s坐標(biāo)，將障礙物投影到自車的ST圖上void BuildReferenceLineStBoundary(const ReferenceLine& reference_line,const double adc_start_s);//設(shè)置障礙物的SL邊界也就是Frenet系的橫縱坐標(biāo)的邊界類對象sl_boundary //輸入?yún)?shù)就是SLBoundary SL邊界類對象 //就是將參數(shù)拷貝給類數(shù)據(jù)成員sl_boundary_ 其實就是障礙物邊界點(diǎn)相對于道路參考線的SL坐標(biāo)void SetPerceptionSlBoundary(const SLBoundary& sl_boundary);/*** @brief * 是否為縱向決策，有ignore，有stop/yield/follow/overtake都是**/static bool IsLongitudinalDecision(const ObjectDecisionType& decision);//nudge在apollo里代表橫向上輕輕繞一下避開？ //判斷目標(biāo)決策類型對象是橫向決策？如果有ignore或nudge就認(rèn)為是static bool IsLateralDecision(const ObjectDecisionType& decision);//設(shè)置該障礙物為阻塞障礙物？void SetBlockingObstacle(bool blocking) { is_blocking_obstacle_ = blocking; }//返回該障礙物是否為阻塞自車的障礙物？bool IsBlockingObstacle() const { return is_blocking_obstacle_; }/** @brief IsLaneBlocking is only meaningful when IsStatic() == true.* 只有當(dāng)障礙物為靜態(tài)時，判斷車道是否被阻塞*/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邊界 //輸入?yún)?shù) 參考線類對象，自車起始的frenet系縱坐標(biāo)s，第三個參數(shù)用以存放得到的結(jié)果 //這個函數(shù)的實現(xiàn)代碼沒太看明白，大致就是根據(jù)參考線，自車的縱向坐標(biāo)s，用動態(tài)感知障礙物對象及其預(yù)測軌跡去修改之前的自車參考線ST邊界，起始就是用該障礙物信息去修正ST圖中搜索的可行域？將動態(tài)障礙物投影到自車的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); namespace { //又定義了個namespace防止其他程序引用該類時出現(xiàn)重名重復(fù)定義現(xiàn)象。 //定義了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_，存放目標(biāo)場景標(biāo)簽和縱向距離的映射？ //這個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_，存放目標(biāo)場景標(biāo)簽和橫向距離的映射？ //這個map的名字字面意義來看，橫向決策安全分類？ const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,Obstacle::ObjectTagCaseHash>Obstacle::s_lateral_decision_safety_sorter_ = {//橫向決策忽略，Nudge是靠邊避讓？{ObjectDecisionType::kIgnore, 0}, {ObjectDecisionType::kNudge, 100}};

構(gòu)造函數(shù)

/** 參數(shù):* id：障礙物id* perception_obstacle:　預(yù)測障礙物* obstacle_priority:　障礙物優(yōu)先級* is_static: 是不是靜態(tài)障礙物*/ Obstacle::Obstacle(const std::string& id,const PerceptionObstacle& perception_obstacle,const ObstaclePriority::Priority& obstacle_priority,const bool is_static): id_(id),perception_id_(perception_obstacle.id()),perception_obstacle_(perception_obstacle),perception_bounding_box_({perception_obstacle_.position().x(),perception_obstacle_.position().y()},perception_obstacle_.theta(),perception_obstacle_.length(),perception_obstacle_.width()) {is_caution_level_obstacle_ = (obstacle_priority == ObstaclePriority::CAUTION);std::vector<common::math::Vec2d> polygon_points;if (FLAGS_use_navigation_mode || // 是否是導(dǎo)航模式(默認(rèn)為false)perception_obstacle.polygon_point_size() <= 2) {//或者預(yù)測障礙物多邊形點(diǎn)的數(shù)量小于等于2// 獲取感知障礙物邊界盒的所有頂點(diǎn)，頂點(diǎn)就是根據(jù)感知的長寬，幾何中心，heading角計算而來perception_bounding_box_.GetAllCorners(&polygon_points);} else {ACHECK(perception_obstacle.polygon_point_size() > 2)<< "object " << id << "has less than 3 polygon points";// 把感知障礙物的多邊形點(diǎn)放入vector polygon_pointsfor (const auto& point : perception_obstacle.polygon_point()) {polygon_points.emplace_back(point.x(), point.y());}}// 計算感知障礙物多邊形點(diǎn)集的凸包，得到的多邊形結(jié)果ACHECK(common::math::Polygon2d::ComputeConvexHull(polygon_points,&perception_polygon_))<< "object[" << id << "] polygon is not a valid convex hull.\n"<< perception_obstacle.DebugString();// 判斷是否為靜態(tài) = 是否為靜態(tài) 或// 障礙物優(yōu)先級為可忽略，就是障礙物可忽略的話也可直接視為靜態(tài)障礙物is_static_ = (is_static || obstacle_priority == ObstaclePriority::IGNORE);// 是否為虛擬障礙物，若感知障礙物id<0的話則為虛擬障礙物is_virtual_ = (perception_obstacle.id() < 0);// 其實就是計算下感知障礙物速度X方向，Y方向的矢量和speed_ = std::hypot(perception_obstacle.velocity().x(),perception_obstacle.velocity().y()); }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;// 取出預(yù)測軌跡的軌跡點(diǎn)trajectory_pointsauto& trajectory_points = *trajectory_.mutable_trajectory_point();double cumulative_s = 0.0; // 初始定義了一個累計縱向距離s cumulative_sif (trajectory_points.size() >0) { // 如果軌跡點(diǎn)的數(shù)目大于0 軌跡點(diǎn)的第一個點(diǎn)的s設(shè)置為0，其實就// 相對第一個軌跡點(diǎn)的相對縱坐標(biāo)strajectory_points[0].mutable_path_point()->set_s(0.0);}// 從預(yù)測軌跡點(diǎn)的第二個點(diǎn)開始遍歷設(shè)置相對縱坐標(biāo)s，第一個點(diǎn)上面已經(jīng)設(shè)置了for (int i = 1; i < trajectory_points.size(); ++i) {const auto& prev = trajectory_points[i - 1];const auto& cur = trajectory_points[i];// 如果前繼點(diǎn)的相對時間 >= 當(dāng)前點(diǎn)相對時間，報錯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個點(diǎn)到第i個點(diǎn)的距離cumulative_s +=common::util::DistanceXY(prev.path_point(), cur.path_point());// 設(shè)定第i個點(diǎn)的相對起點(diǎn)的縱坐標(biāo)strajectory_points[i].mutable_path_point()->set_s(cumulative_s);} }

根據(jù)時間查詢預(yù)測軌跡點(diǎn)：GetPointAtTime

// 根據(jù)時間查詢預(yù)測軌跡點(diǎn) // 輸入?yún)?shù): 相對時間relative_time common::TrajectoryPoint Obstacle::GetPointAtTime(const double relative_time) const {// 首先去預(yù)測軌跡trajectory_上取出所有的預(yù)測軌跡點(diǎn)const auto& points = trajectory_.trajectory_point();// 如果預(yù)測軌跡點(diǎn)個數(shù)小于2，那么軌跡點(diǎn)point的s,t,v,a直接設(shè)置為0，x,y,z,theta直接設(shè)置為當(dāng)前值返回這個軌跡點(diǎn)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 {// std::lower_bound的用法就是遍歷預(yù)測軌跡點(diǎn)，找到第一個小于給定時間的軌跡點(diǎn)auto comp = [](const common::TrajectoryPoint p, const double time) {return p.relative_time() < time;};auto it_lower =std::lower_bound(points.begin(), points.end(), relative_time, comp);// 如果it_lower是預(yù)測軌跡里的第一個點(diǎn)/最后一個點(diǎn)就直接返回之if (it_lower == points.begin()) {return *points.begin();} else if (it_lower == points.end()) {return *points.rbegin();}// 如果it_lower不是預(yù)測軌跡里的第一個點(diǎn)/最后一個點(diǎn)就用it_lower和其上一個點(diǎn)線性插值得到relative_time對應(yīng)的軌跡點(diǎn)信息并返回return common::math::InterpolateUsingLinearApproximation(*(it_lower - 1), *it_lower, relative_time);} }

獲取障礙物的2維邊界盒:Obstacle::GetBoundingBox

//其實就是軌跡點(diǎn)作為幾何中心點(diǎn)其x,y,theta信息加上數(shù)據(jù)成員感知障礙物的長寬構(gòu)建二維邊界盒 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()); }

是否為有效的感知障礙物：IsValidPerceptionObstacle

//判斷是否為有效的感知障礙物，輸入?yún)?shù)是感知障礙物類對象 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;}}//上面都不滿足沒有返回，那么遍歷感知障礙物的多邊形點(diǎn)，只要有一個點(diǎn)的x或y坐標(biāo)為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; }

根據(jù)預(yù)測輸出構(gòu)造：Obstacle::CreateObstacles

/*** @brief 根據(jù)預(yù)測數(shù)據(jù)構(gòu)建障礙物* @param predictions 預(yù)測結(jié)果* @return obstacles 障礙物對象列表*/ std::list<std::unique_ptr<Obstacle>> Obstacle::CreateObstacles(const prediction::PredictionObstacles& predictions) {std::list<std::unique_ptr<Obstacle>> obstacles;// 遍歷預(yù)測障礙物列表for (const auto& prediction_obstacle : predictions.prediction_obstacle()) {// 判斷預(yù)測障礙物的成員感知障礙物是否是有效的感知障礙物，否則報錯并跳到下一個障礙物if (!IsValidPerceptionObstacle(prediction_obstacle.perception_obstacle())) {AERROR << "Invalid perception obstacle: "<< prediction_obstacle.perception_obstacle().DebugString();continue;}// 將第i個被遍歷的預(yù)測障礙物的id，軌跡，優(yōu)先級，是否為靜態(tài)障礙物作為參數(shù)調(diào)用Obstacle類的構(gòu)造函數(shù)，獲得的對象塞到障礙物類對象指針列表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個預(yù)測障礙物的預(yù)測軌跡,// 判斷是否都是合法的軌跡，如果不是，則直接跳到下一個障礙物int trajectory_index = 0;for (const auto& trajectory : prediction_obstacle.trajectory()) {bool is_valid_trajectory = true;for (const auto& point :trajectory.trajectory_point()) { // 遍歷預(yù)測軌跡點(diǎn)if (!IsValidTrajectoryPoint(point)) {AERROR << "obj:" << perception_id<< " TrajectoryPoint: " << trajectory.ShortDebugString()<< " is NOT valid.";is_valid_trajectory = false;break;}}if (!is_valid_trajectory) {continue;}// 若執(zhí)行到這里說明第i個障礙物有效，且其預(yù)測軌跡是有效的// 那么就將其id,優(yōu)先級，預(yù)測軌跡，是否為靜態(tài)障礙物的信息截出來拿去調(diào)用Obstacle類的構(gòu)造函數(shù)，得到的障礙物對象塞入障礙物對象列表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; }

創(chuàng)建虛擬障礙物：Obstacle::CreateStaticVirtualObstacles

判斷是否為有效的軌跡點(diǎn)：Obstacle::IsValidTrajectoryPoint

設(shè)置障礙物的SL邊界:Obstacle::SetPerceptionSlBoundary

//設(shè)置障礙物的SL邊界也就是Frenet系的橫縱坐標(biāo)的邊界類對象sl_boundary。其實就是障礙物邊界點(diǎn)相對于道路參考線的SL坐標(biāo) void Obstacle::SetPerceptionSlBoundary(const SLBoundary& sl_boundary) {sl_boundary_ = sl_boundary; }

計算停車距離: Obstacle::MinRadiusStopDistance

//使用自車的最小轉(zhuǎn)彎半徑計算停車距離。輸入?yún)?shù)：車輛參數(shù)類對象 double Obstacle::MinRadiusStopDistance(const common::VehicleParam& vehicle_param) const {//如果針對這個障礙物的最小轉(zhuǎn)彎半徑的停車距離>0則直接返回？是說明已經(jīng)設(shè)置了嗎？if (min_radius_stop_distance_ > 0) {return min_radius_stop_distance_;}static constexpr double stop_distance_buffer = 0.5; //定義一個停止距離的緩沖區(qū)0.5mconst double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();//獲取車輛參數(shù)的最小轉(zhuǎn)彎半徑//sl_boundary_其實就是障礙物邊界點(diǎn)相對于道路參考線的SL坐標(biāo)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);//利用勾股定理計算計算最小轉(zhuǎn)彎半徑下的停車距離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();//對這個停車距離再進(jìn)行限幅并返回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邊界，輸入?yún)?shù)是道路參考線類對象，以及adc自車的起始s坐標(biāo)，將障礙物投影到自車的ST圖上 //文字不足以描述，詳細(xì)原理見文末附圖 void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,const double adc_start_s) {//獲取ADC 自動駕駛車輛的物理參數(shù)配置const auto& adc_param =VehicleConfigHelper::Instance()->GetConfig().vehicle_param();//獲取車輛物理參數(shù)中的寬度const double adc_width = adc_param.width();//如果障礙物類成員表明其是靜態(tài)障礙物或障礙物預(yù)測軌跡點(diǎn)為空，其實就是個靜止障礙物？if (is_static_ || trajectory_.trajectory_point().empty()) {//定義一個存放STPoint點(diǎn)對的vetor point_pairsstd::vector<std::pair<STPoint, STPoint>> point_pairs;//sl_boundary_其實就是障礙物邊界點(diǎn)相對于道路參考線的SL坐標(biāo)//獲取障礙物在參考線上對應(yīng)的起始s坐標(biāo)，其實就是障礙物的尾部邊界點(diǎn)？double start_s = sl_boundary_.start_s();//獲取障礙物在參考線上對應(yīng)的終點(diǎn)s坐標(biāo)，其實就是障礙物的頭部邊界點(diǎn)？double end_s = sl_boundary_.end_s();//如果終點(diǎn)s-起點(diǎn)s<閾值0.2m，那么終點(diǎn)s=起點(diǎn)s+0.2mif (end_s - start_s < kStBoundaryDeltaS) {end_s = start_s + kStBoundaryDeltaS;}//參考線道路被阻塞？輸入?yún)?shù)是感知障礙物邊界盒，自車的寬度//檢查是否有邊界盒堵塞了路面。標(biāo)準(zhǔn)是檢查路面上的剩余空間是否大于自車寬度。if (!reference_line.IsBlockRoad(perception_bounding_box_, adc_width)) {return;}//往ST邊界里塞ST點(diǎn)對，起點(diǎn)t 0.0//S分別塞入(障礙物SL邊界起始點(diǎn)s-自車的起始s，0.0) (SL邊界終點(diǎn)s-自車的起始s)//其實就是這個障礙物的S邊界上下限距離自車起始點(diǎn)s的距離//相對時間0對應(yīng)的S邊界就是自車到障礙物頭部和尾部縱向位置的差值？這個邊界其實還是//障礙物的頭部尾部的相對縱向位置s投影到自車參考線上//其實就是把障礙物投影到自車的ST圖上？//相對時間0也就是當(dāng)下，直接把現(xiàn)在障礙物頭部尾部所處位置直接放入ST圖point_pairs.emplace_back(STPoint(start_s - adc_start_s, 0.0),STPoint(end_s - adc_start_s, 0.0)); //因為這是一個靜態(tài)障礙物(在這個大if里說明是靜態(tài)障礙物) ，障礙物會阻塞參考線//s軸上相對自車位置的s坐標(biāo)8s(最大ST規(guī)劃時間？)//認(rèn)為這8s該障礙物頭部尾部會一直占據(jù)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邊界就為這個障礙物相對縱向位置占據(jù)8sreference_line_st_boundary_ = STBoundary(point_pairs);} else { //不是靜態(tài)障礙物的話，調(diào)用建立軌跡ST邊界函數(shù)，輸入?yún)?shù)是道路參考線，自車的起始s，求得的st邊界放入reference_line_st_boundary_，起始就是對于這個障礙物對象將其的影響考慮成對參考線ST邊界的影響上，其實就是將動態(tài)障礙物映射到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邊界 //輸入?yún)?shù) 參考線類對象，自車起始的frenet系縱坐標(biāo)s，第三個參數(shù)用以存放得到的結(jié)果 //這個函數(shù)的實現(xiàn)代碼沒太看明白，大致就是根據(jù)參考線，自車的縱向坐標(biāo)s，用動態(tài)感知障礙物對象及其預(yù)測軌跡去修改之前的自車參考線ST邊界，起始就是用該障礙物信息去修正ST圖中搜索的可行域？將動態(tài)障礙物投影到自車的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;}//獲取這個障礙物的長寬，以及預(yù)測軌跡點(diǎn)const double object_width = perception_obstacle_.width();const double object_length = perception_obstacle_.length();const auto& trajectory_points = trajectory_.trajectory_point();//如果預(yù)測軌跡點(diǎn)為空直接返回falseif (trajectory_points.empty()) {AWARN << "object " << id_ << " has no trajectory points";return false;}//獲取自車的物理參數(shù)，長度，長度一半，寬度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();//這里創(chuàng)建了兩個邊界盒對象最小盒，最大盒？沒看到在哪里用？//Box2d構(gòu)造函數(shù)，center中心點(diǎn)，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點(diǎn)，就是縱向位置s和時間t構(gòu)成的點(diǎn)(s,t)//又創(chuàng)建了一個ST點(diǎn)對的vector polygon_points,多邊形點(diǎn)？std::vector<std::pair<STPoint, STPoint>> polygon_points;//又創(chuàng)建了一個SL邊界類對象，last_sl_boundary上一次的sl邊界？SLBoundary last_sl_boundary;//初始定義了上次的索引為0 last_indexint last_index = 0;//從障礙物預(yù)測軌跡的第二個點(diǎn)開始遍歷for (int i = 1; i < trajectory_points.size(); ++i) {ADEBUG << "last_sl_boundary: " << last_sl_boundary.ShortDebugString();//traj軌跡 = path路徑 + 速度規(guī)劃//障礙物第i-1個預(yù)測軌跡點(diǎn)const auto& first_traj_point = trajectory_points[i - 1];//障礙物第i個預(yù)測軌跡點(diǎn)const auto& second_traj_point = trajectory_points[i];//障礙物第i-1個預(yù)測軌跡點(diǎn)上的路徑點(diǎn)const auto& first_point = first_traj_point.path_point();//障礙物第i個預(yù)測軌跡點(diǎn)上的路徑點(diǎn)const auto& second_point = second_traj_point.path_point();//定義了一個物體移動邊界盒長度 = 物體本身長度 + 兩個軌跡點(diǎn)之間的長度，又準(zhǔn)備搞障礙物第//i個軌跡點(diǎn)的邊界盒了double object_moving_box_length =object_length + common::util::DistanceXY(first_point, second_point);//定義該障礙物(Obstacle類實例)第i個軌跡點(diǎn)的邊界盒的中心，就是第i個，i-1軌跡點(diǎn)的中點(diǎn)common::math::Vec2d center((first_point.x() + second_point.x()) / 2.0,(first_point.y() + second_point.y()) / 2.0);//構(gòu)建一個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 目標(biāo)邊界？SLBoundary object_boundary;//Apollo原注釋//注意：這種方法在參考線不是直的時候有誤差，需要雙回路來cover所有的corner cases。//粗糙的跳過距離上次sl邊界盒過近的點(diǎn)？//這里就是計算障礙物第i預(yù)測軌跡點(diǎn)和第i-1個點(diǎn)之間的距離//上一個點(diǎn)的索引并不是單純的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個點(diǎn)挪動到第i個軌跡點(diǎn)，橫向上變化量假設(shè)最惡劣變化distance_xy兩點(diǎn)間直線距離，都沒有超過上一次的SL邊界，就直接跳到下一個軌跡點(diǎn)？這塊原理不是特別清楚？if (last_sl_boundary.start_l() > distance_xy ||last_sl_boundary.end_l() < -distance_xy) {continue;}//上一次SL邊界的縱向中點(diǎn)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那么就忽略這個軌跡點(diǎn)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那么就忽略這個軌跡點(diǎn)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需要注意這個級別或者障礙物頭部都在自車后方，這個軌跡點(diǎn)也可忽略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; }//獲取數(shù)據(jù)成員 參考線ST邊界 const STBoundary& Obstacle::reference_line_st_boundary() const {return reference_line_st_boundary_; } //獲取數(shù)據(jù)成員 路徑ST邊界 const STBoundary& Obstacle::path_st_boundary() const {return path_st_boundary_; }//返回數(shù)據(jù)成員，決策標(biāo)簽？一個字符串的vector,decider_tags_ const std::vector<std::string>& Obstacle::decider_tags() const {return decider_tags_; }//返回目標(biāo)決策類型列表？ const std::vector<ObjectDecisionType>& Obstacle::decisions() const {return decisions_; }//nudge在apollo里代表橫向上輕輕繞一下避開？ //判斷目標(biāo)決策類型對象是橫向決策？如果有ignore或nudge就認(rèn)為是 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異常或終止進(jìn)程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類數(shù)據(jù)成員，針對這個障礙物的縱向決策 const ObjectDecisionType& Obstacle::LongitudinalDecision() const {return longitudinal_decision_; } //返回Obstacle類數(shù)據(jù)成員，針對這個障礙物的橫向決策 const ObjectDecisionType& Obstacle::LateralDecision() const {return lateral_decision_; }//障礙物可被忽略？調(diào)用函數(shù)判斷橫/縱向均可被忽略？ 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(); }//融合兩個橫向的目標(biāo)決策類型對象，保留避讓nudge距離大的那個橫向目標(biāo)決策類型對象并返回 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()); }//針對該障礙物增加一個縱向決策，輸入?yún)?shù)決策標(biāo)簽字符串decider_tag //以及目標(biāo)決策類型對象decision //其實就是將輸入的目標(biāo)縱向決策類型對象與之前類里儲存的進(jìn)行融合。 //類的數(shù)據(jù)成員決策列表decisions_和決策標(biāo)簽列表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); }//針對該障礙物增加一個橫向決策，輸入?yún)?shù)決策標(biāo)簽字符串decider_tag //以及目標(biāo)決策類型對象decision //其實就是將輸入的目標(biāo)橫向決策類型對象與之前類里儲存的進(jìn)行融合。 //類的數(shù)據(jù)成員決策列表decisions_和決策標(biāo)簽列表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_其實就是障礙物邊界點(diǎn)相對于道路參考線的SL坐標(biāo) const SLBoundary& Obstacle::PerceptionSLBoundary() const {return sl_boundary_; }//設(shè)定路徑的ST邊界，也就是針對該障礙物的ST邊界？ void Obstacle::set_path_st_boundary(const STBoundary& boundary) {path_st_boundary_ = boundary;path_st_boundary_initialized_ = true; }//設(shè)定ST邊界類型，設(shè)置為輸入的類型 void Obstacle::SetStBoundaryType(const STBoundary::BoundaryType type) {path_st_boundary_.SetBoundaryType(type); }//擦除類里儲存的路徑ST邊界 void Obstacle::EraseStBoundary() { path_st_boundary_ = STBoundary(); }//設(shè)定道路參考線的ST邊界 void Obstacle::SetReferenceLineStBoundary(const STBoundary& boundary) {reference_line_st_boundary_ = boundary; }//設(shè)定道路參考線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; }//檢查車道被該障礙物阻塞？輸入?yún)?shù)是道路參考線類對象 void Obstacle::CheckLaneBlocking(const ReferenceLine& reference_line) {//如果障礙物不是靜止，就沒阻塞，并返回if (!IsStatic()) {is_lane_blocking_ = false;return;}//如果執(zhí)行到這里說明障礙物已經(jīng)是是靜態(tài)障礙物了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_其實就是障礙物邊界點(diǎn)相對于道路參考線的SL坐標(biāo) //如果根據(jù)該障礙物的SL邊界 //SL邊界是相對道路參考線的，若求出的SL邊界下限L坐標(biāo)和上限L坐標(biāo)異號，說明障礙物SL邊界盒占據(jù)了道路參考線的兩邊，則認(rèn)為是阻塞了if (sl_boundary_.start_l() * sl_boundary_.end_l() < 0.0) {is_lane_blocking_ = true;return;}//如果上面沒有返回，說明障礙物是在道路參考線的一側(cè)，且是靜態(tài)障礙物//駕駛寬度 = 道路參考線計算刨去障礙物邊界盒SL邊界后左右寬度中更大的那個,也就是障礙物的左邊界const double driving_width = reference_line.GetDrivingWidth(sl_boundary_);//獲取車輛物理參數(shù)auto vehicle_param = common::VehicleConfigHelper::GetConfig().vehicle_param();//根據(jù)道路參考線判斷sl_boundary_也就是該障礙物的SL邊界盒在車道內(nèi)？且駕駛寬度<小于車輛的寬度+靜態(tài)橫向避讓緩沖距離0.3，道路阻塞的標(biāo)志位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; }//設(shè)置數(shù)據(jù)成員，該障礙物阻塞換道？用輸入?yún)?shù) 距離過近is_distance_clear拷貝給數(shù)據(jù)成員 //is_lane_change_blocking_ void Obstacle::SetLaneChangeBlocking(const bool is_distance_clear) {is_lane_change_blocking_ = is_distance_clear; }} // namespace planning } // namespace apollo

總結(jié)

以上是生活随笔為你收集整理的Apollo：modules/planning/common/obstacle.cc分析的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。