Apollo学习——planning模块（4）之traffic_decider交通规则决策器

.小墨迹

已于 2025-05-16 14:49:19 修改

阅读量814

点赞数 33

分类专栏： Apollo 文章标签：学习 ubuntu linux c++ 开发语言自动驾驶

于 2025-05-16 14:48:51 首次发布

本文链接：https://blog.csdn.net/qq1240268067/article/details/148003994

版权

Apollo 专栏收录该内容

28 篇文章

订阅专栏

traffic_decider

功能：计算停车点
Init方法
- 代码解释
Execute方法
- 代码解释：
BuildPlanningTarget生成最终车体行为
- 代码解释：

代码在modules/planning/planning_interface_base/traffic_rules_base/traffic_decider.cc

功能：计算停车点

1、初始化交通规则
2、遍历规则rule_list_，输入参数；
3、遍历障碍物，确认停止类型，计算停车的s值
4、将停车的s值放入reference_line_info

Init方法

主要作用，加载插件并且初始化。
1. 初始化检查与配置加载

bool TrafficDecider::Init(const std::shared_ptr<DependencyInjector> &injector) {
  if (init_) return true; // 防止重复初始化
  AINFO << "Load config path:" << FLAGS_traffic_rule_config_filename;
  
  if (!apollo::cyber::common::LoadConfig(FLAGS_traffic_rule_config_filename, &rule_pipeline_)) {
    AERROR << "Load pipeline of Traffic decider failed!";
    return false;
  }

功能解析：
- init_ 标志位：确保 TrafficDecider 仅初始化一次（单例模式常见设计）。
- 配置文件加载：
  - FLAGS_traffic_rule_config_filename 是交通规则配置文件路径（如 modules/planning/conf/traffic_rule_config.pb.txt）。
  - rule_pipeline_ 存储配置中定义的规则序列（如 CROSSWALK、SIGNAL_LIGHT、KEEP_CLEAR）。
- 错误处理：若加载失败，直接终止初始化流程（避免后续逻辑因缺少配置出现未定义行为）。

2. 交通规则插件动态加载

for (int i = 0; i < rule_pipeline_.rule_size(); i++) {
  auto rule = apollo::cyber::plugin_manager::PluginManager::Instance()
              ->CreateInstance<TrafficRule>(
                ConfigUtil::GetFullPlanningClassName(rule_pipeline_.rule(i).type()));
  if (!rule) {
    AERROR << "Init of Traffic rule" << rule_pipeline_.rule(i).name() << " failed!";
    return false;
  }

关键操作：
- 插件管理器调用：通过 PluginManager 动态创建 TrafficRule 实例（如 RegionSpeedLimit 区域限速插件）。
- 类名解析：ConfigUtil::GetFullPlanningClassName 将配置中的简称（如 region_speed_limit）转换为完整类名（如 apollo::planning::RegionSpeedLimit）。
设计思想：
- 插件化架构：允许灵活扩展交通规则，新增规则只需添加插件并更新配置文件，无需修改核心代码。
- 松耦合设计：每个 TrafficRule 插件独立处理特定交通场景（如红绿灯、行人横道），通过统一接口 ApplyRule 实现多态。

3. 规则初始化与注册

  rule->Init(rule_pipeline_.rule(i).name(), injector);
  rule_list_.push_back(rule);
}

细节解析：
- rule->Init：
  - 传递规则名称（如 region_speed_limit）和 DependencyInjector（全局依赖容器）。
  - DependencyInjector 提供车辆状态、规划上下文等共享数据（如 PlanningContext、VehicleStateProvider）。
- 规则注册：将初始化后的规则存入 rule_list_ 列表，供后续规划流程调用（顺序由配置文件定义）。

4. 状态标记与返回

init_ = true;
return true;

作用：标记 TrafficDecider 初始化完成，后续可通过 rule_list_ 调用各交通规则插件。

代码解释

bool TrafficDecider::Init(const std::shared_ptr<DependencyInjector> &injector) {
  //只初始化一次
  if (init_) return true;
  // Load the pipeline config. 交通规则配置文件路径
  AINFO << "Load config path:" << FLAGS_traffic_rule_config_filename;
  // Load the pipeline of scenario. modules/planning/planning_component/conf/traffic_rule_config.pb.txt
  if (!apollo::cyber::common::LoadConfig(FLAGS_traffic_rule_config_filename, &rule_pipeline_)) {
    AERROR << "Load pipeline of Traffic decider" << " failed!";
    return false;
  }
  //-----------------------------------------------
  // 交通规则插件动态加载 将交通规则动态加载到rule_list_中 插件在traffic_rules中
  for (int i = 0; i < rule_pipeline_.rule_size(); i++) {
    auto rule =
        apollo::cyber::plugin_manager::PluginManager::Instance()
            ->CreateInstance<TrafficRule>(ConfigUtil::GetFullPlanningClassName(rule_pipeline_.rule(i).type()));
    if (!rule) {
      AERROR << "Init of Traffic rule" << rule_pipeline_.rule(i).name()
             << " failed!";
      return false;
    }
    // 规则初始化与注册
    rule->Init(rule_pipeline_.rule(i).name(), injector);
    rule_list_.push_back(rule);
  }

  init_ = true;
  return true;
}

Execute方法

1、遍历规则rule_list_
2、重置规则的状态，确保每次决策都是最新的环境rule->Reset();
3、申请规则，将数据传入插件。
4、根据规则处理后的信息，生成最终的车辆行为。BuildPlanningTarget(reference_line_info);

代码解释：

Status TrafficDecider::Execute(Frame *frame, ReferenceLineInfo *reference_line_info) {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
// 遍历规则列表 规则列表（rule_list_） 预定义的交通规则集合（如CrosswalkRule、SignalLightRule等） 
  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();
  }
  // 根据所有规则处理后的参考线信息，生成最终的车辆行为目标
  // 输出内容：
  // 目标速度：结合前方障碍物与交通信号动态调整
  // 路径偏好：优先选择无冲突或低风险的车道
  // 停止点：在需要停车的位置（如红灯前）设置精确停止坐标
  // 数据流向：生成的规划目标将被传递至路径优化器（如EM Planner）进行轨迹生成
  BuildPlanningTarget(reference_line_info);
  return Status::OK();
}

BuildPlanningTarget生成最终车体行为

一、功能概述
该函数用于 确定规划终点（停车点），基于交通规则生成的虚拟障碍物停车决策（如红绿灯、人行横道、目的地等），计算车辆应在参考线上停止的位置，并将其写入 ReferenceLineInfo 供后续路径规划使用。核心步骤如下：

二、代码流程

初始化变量
遍历路径决策中的障碍物
判断停车类型
计算停车位置

1. 初始化变量

double min_s = std::numeric_limits<double>::infinity();
StopPoint stop_point;

min_s：记录距离车辆最近的虚拟障碍物的纵向位置（s 坐标），初始化为极大值。
stop_point：存储最终的停车点信息（类型与位置）。

2. 遍历路径决策中的障碍物

for (const auto *obstacle : reference_line_info->path_decision()->obstacles().Items()) {
  if (obstacle->IsVirtual() && obstacle->HasLongitudinalDecision() &&
      obstacle->LongitudinalDecision().has_stop() &&
      obstacle->PerceptionSLBoundary().start_s() < min_s) {
    // 处理符合条件的虚拟障碍物
  }
}

筛选条件：
- 虚拟障碍：IsVirtual() 过滤由交通规则生成的虚拟障碍物（如红绿灯停止线）。
- 停车决策：LongitudinalDecision().has_stop() 确保障碍物触发了停车行为。
- 最近距离：start_s() < min_s 选择距离车辆最近的停车点。

3. 判断停车类型

const auto &stop_code = obstacle->LongitudinalDecision().stop().reason_code();
if (stop_code == StopReasonCode::STOP_REASON_DESTINATION || ...) {
  stop_point.set_type(StopPoint::HARD);
  ADEBUG << "Hard stop at: " << min_s << "REASON: " << StopReasonCode_Name(stop_code);
} else if (stop_code == StopReasonCode::STOP_REASON_YELLOW_SIGNAL) {
  stop_point.set_type(StopPoint::SOFT);
  ADEBUG << "Soft stop at: " << min_s << "  STOP_REASON_YELLOW_SIGNAL";
}

硬停车（HARD）：针对必须完全停止的场景（如红灯、停车标志、目的地）。
软停车（SOFT）：针对可协商停车的场景（如黄灯需谨慎减速）。
原因编码：STOP_REASON_CROSSWALK（人行横道）、STOP_REASON_REFERENCE_END（参考线结束）等对应交通规则配置文件中的九类交规。

4. 计算停车位置

if (min_s != std::numeric_limits<double>::infinity()) {
  const auto &vehicle_config = common::VehicleConfigHelper::Instance()->GetConfig();
  double front_edge_to_center = vehicle_config.vehicle_param().front_edge_to_center();
  stop_point.set_s(min_s - front_edge_to_center + FLAGS_virtual_stop_wall_length / 2.0);
  reference_line_info->SetLatticeStopPoint(stop_point);
}

车辆参数补偿：
- front_edge_to_center：车辆前保险杠到质心的距离，确保停车时车头不越过停止线。
- FLAGS_virtual_stop_wall_length：虚拟停止墙长度补偿（默认 0.1 米），防止数值误差。
写入参考线：SetLatticeStopPoint 将停车点信息存入 ReferenceLineInfo，供路径优化器（如 LatticePlanner）使用。

三、总结
1. 与交通规则协作

虚拟障碍物由 TrafficDecider 根据交规生成（如人行横道停止线、交叉口停车墙）。
StopReasonCode 映射到配置文件中的九类交规（如 CROSSWALK、TRAFFIC_LIGHT），通过 traffic_rule_config.pb.txt 动态加载。

2. 规划与控制协同

硬停车：控制模块需完全停车（速度降为 0），适用于红绿灯、停车标志等场景。
软停车：控制模块可缓速接近（如黄灯滑行），留出决策响应时间。

3. 车辆动力学适配

通过 vehicle_param() 动态适配不同车型参数（如卡车、轿车的前悬长度差异），提升泛用性。

代码解释：

void TrafficDecider::BuildPlanningTarget(ReferenceLineInfo *reference_line_info) {
  double min_s = std::numeric_limits<double>::infinity();
  StopPoint stop_point;
  //遍历障碍物
  for (const auto *obstacle : reference_line_info->path_decision()->obstacles().Items()) {
    // `IsVirtual()` 过滤由交通规则生成的虚拟障碍物（如红绿灯停止线）。
    // `LongitudinalDecision().has_stop()` 确保障碍物触发了停车行为。
    // `start_s() < min_s` 选择距离车辆最近的停车点。
    if (obstacle->IsVirtual() && obstacle->HasLongitudinalDecision() &&
        obstacle->LongitudinalDecision().has_stop() &&
        obstacle->PerceptionSLBoundary().start_s() < min_s) {
      // 以下是处理符合条件的虚拟障碍物
      min_s = obstacle->PerceptionSLBoundary().start_s();//计算车辆最近的虚拟障碍物的纵向位置（s 坐标）
      //判断停车类型  硬停车（HARD） 软停车（SOFT）
      const auto &stop_code = obstacle->LongitudinalDecision().stop().reason_code();
      //硬停车
      if (stop_code == StopReasonCode::STOP_REASON_DESTINATION ||
          stop_code == StopReasonCode::STOP_REASON_CROSSWALK ||
          stop_code == StopReasonCode::STOP_REASON_STOP_SIGN ||
          stop_code == StopReasonCode::STOP_REASON_YIELD_SIGN ||
          stop_code == StopReasonCode::STOP_REASON_CREEPER ||
          stop_code == StopReasonCode::STOP_REASON_REFERENCE_END ||
          stop_code == StopReasonCode::STOP_REASON_SIGNAL) {

        stop_point.set_type(StopPoint::HARD);
        ADEBUG << "Hard stop at: " << min_s << "REASON: " << StopReasonCode_Name(stop_code);
        // 软停车（SOFT）黄灯的情况
      } else if (stop_code == StopReasonCode::STOP_REASON_YELLOW_SIGNAL) {
        stop_point.set_type(StopPoint::SOFT);
        ADEBUG << "Soft stop at: " << min_s << "  STOP_REASON_YELLOW_SIGNAL";
      } else {
        ADEBUG << "No planning target found at reference line.";
      }
    }
  }
  // 计算停车位置
  if (min_s != std::numeric_limits<double>::infinity()) {
    //获取车辆尺寸信息
    const auto &vehicle_config = common::VehicleConfigHelper::Instance()->GetConfig();
    // 车辆前保险杠到质心的纵向距离，用于补偿停车位置，确保车头不越过停止线。
    double front_edge_to_center = vehicle_config.vehicle_param().front_edge_to_center();
    // s = 停车点纵向位置 - 车辆前保险杠到质心的纵向距离 + 虚拟停车墙长度（默认 0.1 米）/2
    stop_point.set_s(min_s - front_edge_to_center + FLAGS_virtual_stop_wall_length / 2.0);
    //  设置停车点信息
    reference_line_info->SetLatticeStopPoint(stop_point);
  }
}