概述

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 apollo

obstacle.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类代码详解的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。