Apollo -- Planning (四) 交通决策器 TrafficDecider-CSDN博客

本文链接：https://blog.csdn.net/qhr_1012/article/details/140665514

1、代码入口

traffic rules是planning在运行场景之前，根据不同的交通规则，决策车辆是否需要停车，减速或其他操作。因为它是在运行场景之前进行的，所以对所有的场景都会起作用。

OnLanePlanning中代码在Proc()函数中

  for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {
    auto traffic_status =
        traffic_decider_.Execute(frame_.get(), &ref_line_info);
    if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {
      ref_line_info.SetDrivable(false);
      AWARN << "Reference line " << ref_line_info.Lanes().Id()
            << " traffic decider failed";
    }
  }

目前支持的traffic rules有：

backside_vehicle: 向车辆的处理，决定是否要忽略后向车辆
crosswalk: 行道交通规则，根据人行道附近障碍物情况，决定是否要停车
destination: 近终点的处理，在接近终点时设置停止墙
keepclear: 停区域的处理，在禁停区域车辆不能停车，如果禁停区域内有block障碍物，需要在禁停区域外停车
reference_line_end: 近参考线终点的处理，在参考线终点设置停止墙
rerouting: lanning被阻塞时需要重新路由时的处理，发送rerouting请求
stop_sign: 止标志交通规则的处理，在停止标志前停车
traffic_light: 通信号灯的处理，在交通灯为绿色通行，否则需要停车
yield_sign: 止让行标志的处理，在这个标志附近如果有冲突车辆，自车停止让行

文件配置路径：modules/planning/planning_component/conf/traffic_rule_config.pb.txt

循环遍历traffic rules并将判断的结果挂载到对应的参考线上，用于后面决策使用

  for (const auto &rule : rule_list_) {
    if (!rule) {
      AERROR << "Could not find rule ";
      continue;
    }
    rule->Reset();
    rule->ApplyRule(frame, reference_line_info);
    ADEBUG << "Applied rule " << rule->Getname();
  }

Frenet坐标系

插一个知识点：

坐标转换代码如下：

#include <iostream>
#include <vector>
#include <cmath>

// Define a structure for Cartesian coordinates.
struct CartesianPoint {
    double x, y;
};

// Define a structure for Frenet coordinates.
struct FrenetPoint {
    double s, d; // s is the arc length, d is the lateral distance from the reference line.
};

// Function to calculate the arc length (s) along the path.
double CalculateArcLength(const std::vector<CartesianPoint>& path, int index) {
    double totalLength = 0.0;
    for (int i = 1; i <= index; ++i) {
        totalLength += std::sqrt(std::pow(path[i].x - path[i-1].x, 2) + std::pow(path[i].y - path[i-1].y, 2));
    }
    return totalLength;
}

// Function to calculate the tangent vector at a point.
std::pair<double, double> CalculateTangent(const std::vector<CartesianPoint>& path, int index) {
    double dx = path[index+1].x - path[index].x;
    double dy = path[index+1].y - path[index].y;
    double norm = std::sqrt(dx * dx + dy * dy);
    return {dx / norm, dy / norm};
}

// Function to convert Cartesian to Frenet coordinates.
FrenetPoint ConvertToFrenet(const std::vector<CartesianPoint>& path, const CartesianPoint& point, int index) {
    double s = CalculateArcLength(path, index);
    auto [tangentX, tangentY] = CalculateTangent(path, index);
    double d = (point.x - path[index].x) * tangentY - (point.y - path[index].y) * tangentX;

    FrenetPoint frenetPoint{s, d};
    return frenetPoint;
}

int main() {
    // Example path as a list of points.
    std::vector<CartesianPoint> path = {{0, 0}, {1, 1}, {2, 2}, {3, 3}, {4, 4}};

    // Point to convert.
    CartesianPoint point{2.5, 3.5};

    // Convert the point.
    FrenetPoint frenetPoint = ConvertToFrenet(path, point, 3);

    std::cout << "Frenet coordinates: s=" << frenetPoint.s << ", d=" << frenetPoint.d << std::

2、交通规则逻辑走读

2.1、BacksideVehicle

BacksideVehicle用于产生后方来车是否忽略的决策, 决策结果保存在 reference_line_info 的 path_decision 中

通过遍历所有障碍物, 根据其位置信息, 判断是否产生忽略的决策, 条件包括:

不能忽略自车前方或侧方的障碍物
忽略从自车正后方过来的障碍物

2.2、Crosswalk

Crosswalk用于在人行横道处生成虚拟障碍物, 礼让行人优先通行

遍历参考线上的人行横道信息, 若自车车头已越过某处的人行横道, 则忽略该人行横道
遍历障碍物信息, 满足下列条件, 则需停车让行
- 属于行人和非机动车类型
- 在人行横道的区域范围内
- 障碍物要横穿自车path, 或在自车前方的道路范围内
- 自车停车所需的减速度小于所允许的最大减速度
记录各人行横道所需停车让行的障碍物
在需要停车让行的人行横道处, 生成虚拟障碍物, 用于停车

其他rules用到再记录