Obstacle類是apollo planning模塊下modules\planning\common\obstacle.cc/.h實現

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

成員變量

成員類型變量名說明

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邊界初始化標志位
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_;	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;	車道阻斷標志位
bool	is_lane_change_blocking_ = false;	交通阻斷標志位
bool	is_caution_level_obstacle_ = false;	注意級標志位
double	min_radius_stop_distance_ = -1.0;	最小半徑停止距離

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

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

成員函數

//返回障礙物是否有軌跡？bool HasTrajectory() const {return !(trajectory_.trajectory_point().empty());}//返回感知障礙物對象，屬于Obstacle類成員const perception::PerceptionObstacle& Perception() const {return perception_obstacle_;}//判斷是否為有效的感知障礙物，輸入參數是感知障礙物類對象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_;}/*** 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 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); 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}};

構造函數

/** 參數:* id：障礙物id* perception_obstacle:　預測障礙物* obstacle_priority:　障礙物優先級* is_static: 是不是靜態障礙物*/ 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 || // 是否是導航模式(默認為false)perception_obstacle.polygon_point_size() <= 2) {//或者預測障礙物多邊形點的數量小于等于2// 獲取感知障礙物邊界盒的所有頂點，頂點就是根據感知的長寬，幾何中心，heading角計算而來perception_bounding_box_.GetAllCorners(&polygon_points);} else {ACHECK(perception_obstacle.polygon_point_size() > 2)<< "object " << id << "has less than 3 polygon points";// 把感知障礙物的多邊形點放入vector polygon_pointsfor (const auto& point : perception_obstacle.polygon_point()) {polygon_points.emplace_back(point.x(), point.y());}}// 計算感知障礙物多邊形點集的凸包，得到的多邊形結果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);// 其實就是計算下感知障礙物速度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;// 取出預測軌跡的軌跡點trajectory_pointsauto& trajectory_points = *trajectory_.mutable_trajectory_point();double cumulative_s = 0.0; // 初始定義了一個累計縱向距離s cumulative_sif (trajectory_points.size() >0) { // 如果軌跡點的數目大于0 軌跡點的第一個點的s設置為0，其實就// 相對第一個軌跡點的相對縱坐標strajectory_points[0].mutable_path_point()->set_s(0.0);}// 從預測軌跡點的第二個點開始遍歷設置相對縱坐標s，第一個點上面已經設置了for (int i = 1; i < trajectory_points.size(); ++i) {const auto& prev = trajectory_points[i - 1];const 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);} }

根據時間查詢預測軌跡點：GetPointAtTime

// 根據時間查詢預測軌跡點 // 輸入參數: 相對時間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 {// std::lower_bound的用法就是遍歷預測軌跡點，找到第一個小于給定時間的軌跡點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是預測軌跡里的第一個點/最后一個點就直接返回之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維邊界盒:Obstacle::GetBoundingBox

//其實就是軌跡點作為幾何中心點其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()); }

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

//判斷是否為有效的感知障礙物，輸入參數是感知障礙物類對象 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; }

根據預測輸出構造：Obstacle::CreateObstacles

/*** @brief 根據預測數據構建障礙物* @param predictions 預測結果* @return obstacles 障礙物對象列表*/ 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()) { // 遍歷預測軌跡點if (!IsValidTrajectoryPoint(point)) {AERROR << "obj:" << perception_id<< " TrajectoryPoint: " << trajectory.ShortDebugString()<< " is NOT valid.";is_valid_trajectory = false;break;}}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; }

創建虛擬障礙物：Obstacle::CreateStaticVirtualObstacles

判斷是否為有效的軌跡點：Obstacle::IsValidTrajectoryPoint

設置障礙物的SL邊界:Obstacle::SetPerceptionSlBoundary

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

計算停車距離: Obstacle::MinRadiusStopDistance

//使用自車的最小轉彎半徑計算停車距離。輸入參數：車輛參數類對象 double Obstacle::MinRadiusStopDistance(const common::VehicleParam& vehicle_param) const {//如果針對這個障礙物的最小轉彎半徑的停車距離>0則直接返回？是說明已經設置了嗎？if (min_radius_stop_distance_ > 0) {return min_radius_stop_distance_;}static constexpr double stop_distance_buffer = 0.5; //定義一個停止距離的緩沖區0.5mconst 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

總結

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

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