【Apollo学习笔记】—— Planning模块

已于 2023-08-22 16:13:51 修改
阅读量1.7k 收藏 23
点赞数 9
分类专栏： Apollo 文章标签： apollo planning 算法 决策规划 自动驾驶
于 2023-08-17 23:22:17 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/sinat_52032317/article/details/132342847
版权

在这里插入图片描述

前言

本文记录学习planning模块时的一些笔记，总体流程参照https://zhuanlan.zhihu.com/p/61982682中的流程图，如上图所示。

planning_component

modules/planning/planning_component.cc
在这里插入图片描述

PlanningComponent::Init部分首先完成规划模式的选择：

  if (FLAGS_use_navigation_mode) {
    planning_base_ = std::make_unique<NaviPlanning>(injector_);
  } else {
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);
  }

再完成相应的消息订阅

routing_reader_ = node_->CreateReader<RoutingResponse>(
traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
pad_msg_reader_ = node_->CreateReader<PadMessage>(
story_telling_reader_ = node_->CreateReader<Stories>(
relative_map_reader_ = node_->CreateReader<MapMsg>(

最后消息发布

  planning_writer_ = node_->CreateWriter<ADCTrajectory>(
  rerouting_writer_ = node_->CreateWriter<RoutingRequest>(
  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(

PlanningComponent::Proc的主要是检查数据，并且执行注册好的Planning，生成路线并且发布。

bool PlanningComponent::Proc(...) {
  // 1. 检查是否需要重新规划线路。
  CheckRerouting();
  // 2. 数据放入local_view_中，并且检查输入数据。
  ...
  // 3. 执行注册好的Planning，生成线路。
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  // 4. 发布消息
  planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
  // 5. 最后记录
  history->Add(adc_trajectory_pb);
}

RunOnce在NaviPlanning和OnLanePlanning都有，本文以常用的OnLanePlanning为例。

PS1

  • prediction_obstacles 从channel中获得的指针，类型为 PredictionObstacles，代码文件在 modules/prediction/proto/prediction_obstacle.proto 中，该类型的消息来自预测模块，包含了以下信息：
    • Header 头结构：包含了这条消息发布的时刻（以秒为单位），目前位置的模块名，该消息的序列号（每个模块各自维护的）等，几乎每个消息都包含有这个头结构，下面就不提及了。
    • PredictionObstacle 预测模块中的若干个障碍物行为：有感知模块中的障碍物（PerceptionObstacle），它包括障碍物的 id，它的三维坐标、速度加速度、边界大小（长高宽）、特殊的形状、类型（行人、非机动车、机动车）、运动轨迹等；还有时间戳，记录 GPS 给出的时刻；还有预测的时间长度；多种可能的运动轨迹；障碍物的运动趋势、优先级等。
    • 开始的时间戳：记录预测开始的时刻
    • 结束时间戳：预测结束的时刻
    • 自动驾驶车辆的运动趋势，停止、正常行驶还是正在变道等
    • Scenery 现在的场景
  • chassis 从channel获得的指针，类型为 Chassis，这条消息直接来自总线 CanBus，代码文件在 modules/canbus/proto/chassis.proto 中，该类包含了很多信息，主要是汽车底盘所给出的机械状态信息，比如：
    • 驾驶模式，有手动驾驶、自动驾驶、仅控制方向、仅控制速度以及紧急模式
    • 档位情况、引擎转速、车辆速度、里程表、燃油情况、电池电量等
    • 刹车、油门踏板的力度，方向盘的旋转角度，车轮转速
    • 转向灯、雾灯、大灯的工作情况
  • localization_estimate 从channel中获得的指针，类型为 LocalizationEstimate，代码文件在 modules/localization/proto/localization.proto 中，这条消息来自定位模块，该类主要包含了：
    • Pose 位置姿势，包括车头朝向，在地图上的线速度、线加速度和相对位置，角速度、仰角度等
    • 测量上述姿势的时刻
    • 车辆已经经过的轨迹点列
    • MSF 定位状态与质量

PS2

std::mutex是C++11标准中提供的一种互斥锁机制，用于保护共享资源的访问。当多个线程需要同时访问共享资源时，为了避免出现数据竞争等问题，需要使用互斥锁进行同步控制。

std::mutex是一种基本的互斥锁类型，可以通过lock()函数锁定互斥锁，通过unlock()函数释放互斥锁。当一个线程持有互斥锁时，其他线程想要获取该互斥锁将会被阻塞，直到持有该互斥锁的线程将锁释放。

例如，当多个线程需要同时访问同一个共享变量时，可以在访问之前使用std::mutex进行锁定，保证每个线程都可以安全地访问该变量。例如：

#include <mutex>
#include <thread>
#include <iostream>

int shared_data = 0;
std::mutex mutex;

void work(int id) {
    for (int i = 0; i < 10000; ++i) {
        mutex.lock();
        shared_data++;
        mutex.unlock();
    }
    std::cout << "Thread " << id << " finished." << std::endl;
}

int main() {
    std::thread t1(work, 1);
    std::thread t2(work, 2);
    t1.join();
    t2.join();
    std::cout << "Shared data: " << shared_data << std::endl;
    return 0;
}

在上述例子中，两个线程都会对shared_data变量进行10000次的自增操作，为了避免出现数据竞争，我们在对shared_data进行访问之前都使用了std::mutex进行了锁定，保证了线程的安全性。

on_lane_planning

modules/planning/on_lane_planning.cc
在这里插入图片描述

OnLanePlanning::Init主要实现分配具体的Planner，启动参考线提供器(reference_line_provider_)。启动参考线提供器，会另启动一个线程，执行一个定时任务，每隔50ms提供一次参考线。注意：分配Planner的部分在OnLanePlanning实例化时就已经完成。

分配Planner的实现部分

modules/planning/planner/on_lane_planner_dispatcher.cc
std::unique_ptr<Planner> OnLanePlannerDispatcher::DispatchPlanner(
    const PlanningConfig& planning_config,
    const std::shared_ptr<DependencyInjector>& injector) {
  return planner_factory_.CreateObject(
      planning_config.standard_planning_config().planner_type(0), injector);
}

可以在modules/planning/conf/planning_config.pb.txt配置文件中看到配置Planner的类型：

standard_planning_config {
  planner_type: PUBLIC_ROAD
  planner_public_road_config {
  }
}

OnLanePlanning 的主逻辑在OnLanePlanning::RunOnce中，内容比较多，有些还没仔细看，大致完成了一下的内容：

void OnLanePlanning::RunOnce(const LocalView& local_view,
                             ADCTrajectory* const ptr_trajectory_pb) {
    //  更新汽车状态和参考线的状态，如果状态无效，直接返回
    //  ...  
    // 是否为出现导航路线变换，如果是 初始化 planner
    //  加上预估的规划触发的周期 得到 stitchingTrajectory
  // planning is triggered by prediction data, but we can still use an estimated
  // cycle time for stitching
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
    //  判断是否符合交通规则
    //  开始正在的规划 planner 开始规划
  status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
    //  记录规划所花费的时间
    //  ...
}

status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);中有如下代码，此部分的Plan是指分配的planner的Plan()函数

  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                               ptr_trajectory_pb);

PS1

make_unique是一个C++11中的函数模板，用于创建并返回一个独占指针（unique_ptr）。它的作用是将一个裸指针封装成一个unique_ptr对象，并确保该对象是唯一持有它所指向的对象。这可以有效避免内存泄漏和多个指针指向同一个对象的问题。

public_road_planner

modules/planning/planner/public_road/public_road_planner.cc
接着来看public_road_planner的实现。

在这里插入图片描述

PublicRoadPlanner::Init实例化一个全局的scenario_manager_对象来进行场景管理。

Status PublicRoadPlanner::Init(const PlanningConfig& config) {
  config_ = config;
  scenario_manager_.Init(config);
  return Status::OK();
}

PublicRoadPlanner::Plan主要完成以下工作

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                               Frame* frame,
                               ADCTrajectory* ptr_computed_trajectory) {
  // 更新场景，决策当前应该执行什么场景
  scenario_manager_.Update(planning_start_point, *frame);
  // 获取当前场景
  scenario_ = scenario_manager_.mutable_scenario();
  // 调用Scenario的Process函数，对具体的场景进行处理
  auto result = scenario_->Process(planning_start_point, frame);

  if (FLAGS_enable_record_debug) {
    auto scenario_debug = ptr_computed_trajectory->mutable_debug()
                              ->mutable_planning_data()
                              ->mutable_scenario();
    scenario_debug->set_scenario_type(scenario_->scenario_type());
    scenario_debug->set_stage_type(scenario_->GetStage());
    scenario_debug->set_msg(scenario_->GetMsg());
  }
  // 当前场景完成
  if (result == scenario::Scenario::STATUS_DONE) {
    // only updates scenario manager when previous scenario's status is
    // STATUS_DONE
    scenario_manager_.Update(planning_start_point, *frame);
  } else if (result == scenario::Scenario::STATUS_UNKNOWN) {
    // 当前场景失败
    return Status(common::PLANNING_ERROR, "scenario returned unknown");
  }
  return Status::OK();
}

Scenario

Introduction

在这里插入图片描述

Apollo规划模块的框架以场景调度为基本决策框架，场景调度本质上是一个双层状态机，顶层是基于场景的管理器，底层是一系列小模块的任务组合，由此来构建出Apollo planning的整体框架。

在这里插入图片描述在执行的每一个场景中，又分为一个或多个阶段，称为stage；而每一个stage又包含一个或多个任务，称为task。执行一个场景，最终就是顺序执行不同阶段的不同任务。由此可见，Scenario在Apollo规划决策中占据着重要的地位。

Scenario 类型

在apollo中，Scenario被定义了十余种类型，在官方文档Planing 2.0综述中有对各个场景的直观描述。例如下图是LANE_FOLLOW场景的示意图。

在这里插入图片描述

在apollo8.0中，可以从modules/planning/proto/planning_config.proto文件中看到关于Scenario的相关类型定义:

message ScenarioConfig {

  message StageConfig {
    // 阶段配置
    // 阶段类型
    optional StageType stage_type = 1;
    // 是否启用该阶段配置，默认值为true
    optional bool enabled = 2 [default = true];
    // 运行时使用的任务列表，按照顺序排列
    // 决定了这些任务的运行时顺序
    // 任务类型
    repeated TaskConfig.TaskType task_type = 3;
    // 未排序的任务配置列表
    // 任务配置
    repeated TaskConfig task_config = 4;
  }

  // 场景类型
  optional ScenarioType scenario_type = 1;

  // 场景配置，使用oneof是为了确保只有一个字段被设置
  oneof scenario_config {
    // 车道跟随场景配置
    ScenarioLaneFollowConfig lane_follow_config = 2;
    // 无保护交叉口场景配置
    ScenarioBareIntersectionUnprotectedConfig bare_intersection_unprotected_config = 3;
    // 紧急靠边停车场景配置
    ScenarioEmergencyPullOverConfig emergency_pull_over_config = 4;
    // 紧急停车场景配置
    ScenarioEmergencyStopConfig emergency_stop_config = 5;
    // 学习模型样本场景配置
    ScenarioLearningModelSampleConfig learning_model_sample_config = 6;
    // 窄街道U型转弯场景配置
    ScenarioNarrowStreetUTurnConfig narrow_street_u_turn_config = 7;
    // 停车即走场景配置
    ScenarioParkAndGoConfig park_and_go_config = 8;
    // 靠边停车场景配置
    ScenarioPullOverConfig pull_over_config = 9;
    // 无保护停车标志左转场景配置
    ScenarioStopSignUnprotectedConfig stop_sign_unprotected_config = 10;
    // 交通信号灯保护左转场景配置
    ScenarioTrafficLightProtectedConfig traffic_light_protected_config = 11;
    // 无保护交通信号灯左转场景配置
    ScenarioTrafficLightUnprotectedLeftTurnConfig traffic_light_unprotected_left_turn_config = 12;
    // 无保护交通信号灯右转场景配置
    ScenarioTrafficLightUnprotectedRightTurnConfig traffic_light_unprotected_right_turn_config = 13;
    // 代客停车场景配置
    ScenarioValetParkingConfig valet_parking_config = 14;
    // 让路标志场景配置
    ScenarioYieldSignConfig yield_sign_config = 15;
    // 死胡同场景配置（掉头）
    ScenarioDeadEndTurnAroundConfig deadend_turnaround_config = 18;
  }
  // 运行时使用的阶段列表，第一个是默认阶段
  // 阶段类型
  repeated StageType stage_type = 16;
  // 运行时使用的阶段配置列表，未排序
  // 阶段配置
  repeated StageConfig stage_config = 17;
}

接着来看看Scenario Manager做了些什么。
modules/planning/scenarios/scenario_manager.cc
在这里插入图片描述

场景注册

// modules/planning/scenarios/scenario_manager.cc
bool ScenarioManager::Init(const PlanningConfig& planning_config) {
  planning_config_.CopyFrom(planning_config);
  // 注册场景
  RegisterScenarios();
  // 默认为LANE_FOLLOW
  default_scenario_type_ = ScenarioType::LANE_FOLLOW;
  current_scenario_ = CreateScenario(default_scenario_type_);
  return true;
}

ScenarioManager负责实际的场景注册，Init函数负责对场景的初始化，首先调用::RegisterScenario这个函数对各个场景配置文件进行读取和注册；在开始创建LANE_FOLLOW作为默认运行场景。

// modules/planning/planner/public_road/public_road_planner.cc
Status PublicRoadPlanner::Init(const PlanningConfig& config) {
  config_ = config;
  scenario_manager_.Init(config);
  return Status::OK();
}

在PublicRoadPlanner初始化时会调用配置文件里的参数来建立这个对象，ScenarioManager会实例化一个全局的scenario_manager_对象来进行场景管理。

场景转换

// 更新场景
void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                             const Frame& frame) {
  ACHECK(!frame.reference_line_info().empty());
  // 获取当前要处理的overlaps
  Observe(frame);
  // 场景分发
  ScenarioDispatch(frame);
}

ScenarioManager除了负责场景的配置与注册外也负责对场景进行转换。场景转换在ScenarioManager::Update中实现，这个函数有两个输入，车辆当前的状态信息ego_point，和planning循环计算所需要的其他相关信息frame。ScenarioManager::Update首先调用Observe函数，用以获取当前要处理的overlaps.

// 获取当前要处理的overlaps
void ScenarioManager::Observe(const Frame& frame) {
  // init first_encountered_overlap_map_
  first_encountered_overlap_map_.clear();
  const auto& reference_line_info = frame.reference_line_info().front();
  const auto& first_encountered_overlaps =
      reference_line_info.FirstEncounteredOverlaps();
  for (const auto& overlap : first_encountered_overlaps) {
    if (overlap.first == ReferenceLineInfo::PNC_JUNCTION ||
        overlap.first == ReferenceLineInfo::SIGNAL ||
        overlap.first == ReferenceLineInfo::STOP_SIGN ||
        overlap.first == ReferenceLineInfo::YIELD_SIGN) {
      first_encountered_overlap_map_[overlap.first] = overlap.second;
    }
  }
}

在apollo里 overlap 是指地图上任意重合的东西，比如PNC_JUNCTION里是道路之间有相互重合，SIGNAL是信号灯与道路有重合，STOP_SIGN是停止标志与道路有重合，YIELD_SIGN是让行标志与道路有重合。

// 通过一个有限状态机，决定当前的场景
// 会根据配置选择基于规则还是基于学习的决策方法。
void ScenarioManager::ScenarioDispatch(const Frame& frame) {
  ACHECK(!frame.reference_line_info().empty());
  ScenarioType scenario_type;

  int history_points_len = 0;
  if (injector_->learning_based_data() &&
      injector_->learning_based_data()->GetLatestLearningDataFrame()) {
    history_points_len = injector_->learning_based_data()
                             ->GetLatestLearningDataFrame()
                             ->adc_trajectory_point_size();
  }
  if ((planning_config_.learning_mode() == PlanningConfig::E2E ||
       planning_config_.learning_mode() == PlanningConfig::E2E_TEST) &&
      history_points_len >= FLAGS_min_past_history_points_len) {
    scenario_type = ScenarioDispatchLearning();
  } else {
    scenario_type = ScenarioDispatchNonLearning(frame);
  }

  ADEBUG << "select scenario: " << ScenarioType_Name(scenario_type);

  // update PlanningContext
  UpdatePlanningContext(frame, scenario_type);

  if (current_scenario_->scenario_type() != scenario_type) {
    current_scenario_ = CreateScenario(scenario_type);
  }
}

当遇到这些overlaps时，在函数ScenarioDispatch中对其进行具体的处理或者场景切换。ScenarioDispatch会根据配置选择基于规则还是基于学习的决策方法。

接着来看ScenarioDispatchNonLearning函数：

ScenarioType ScenarioManager::ScenarioDispatchNonLearning(const Frame& frame) {
  
  // default: LANE_FOLLOW
  ScenarioType scenario_type = default_scenario_type_;

  
  // Pad Msg scenario(驾驶员意图？)
  scenario_type = SelectPadMsgScenario(frame);

  if (scenario_type == default_scenario_type_) {
    // check current_scenario (not switchable)
    switch (current_scenario_->scenario_type()) {
      case ScenarioType::LANE_FOLLOW:
      case ScenarioType::PULL_OVER:
        break;
      case ScenarioType::BARE_INTERSECTION_UNPROTECTED:
      case ScenarioType::EMERGENCY_PULL_OVER:
      case ScenarioType::PARK_AND_GO:
      case ScenarioType::STOP_SIGN_PROTECTED:
      case ScenarioType::STOP_SIGN_UNPROTECTED:
      case ScenarioType::TRAFFIC_LIGHT_PROTECTED:
      case ScenarioType::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN:
      case ScenarioType::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:
      case ScenarioType::VALET_PARKING:
      case ScenarioType::YIELD_SIGN:
        // must continue until finish
        if (current_scenario_->GetStatus() !=
            Scenario::ScenarioStatus::STATUS_DONE) {
          scenario_type = current_scenario_->scenario_type();
        }
        break;
      default:
        break;
    }
  }
  // ParkAndGo / starting scenario
  if (scenario_type == default_scenario_type_) {
    if (FLAGS_enable_scenario_park_and_go) {
      scenario_type = SelectParkAndGoScenario(frame);
    }
  }
  // intersection scenarios
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectInterceptionScenario(frame);
  }
  // pull-over scenario
  if (scenario_type == default_scenario_type_) {
    if (FLAGS_enable_scenario_pull_over) {
      scenario_type = SelectPullOverScenario(frame);
    }
  }
  // VALET_PARKING scenario
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectValetParkingScenario(frame);
  }
  return scenario_type;
}

首先，确定一个默认的场景类型，即LANE_FOLLOW。然后，判断是否有驾驶员意图，如果有，则将场景类型改为相应的意图场景类型。如果没有，则进入下一步逻辑。接着检查当前的场景类型，如果是LANE_FOLLOW或PULL_OVER，直接break；若是其他场景，且其他场景还未执行完毕，则继续执行当前场景。如果当前场景已完成，则进入下一步逻辑。PARK_AND_GO和PULL_OVER若有相关配置，则执行相应的函数。注意：每次切换场景都从默认场景开始，最后回到默认场景。

Scenario::Process

由PublicRoadPlanner::Plan的流程可知，在获取完当前场景之后，会调用Scenario的Process函数，对具体的场景进行处理。接下来便来看看Scenario的Process函数：
modules/planning/scenarios/scenario.cc

Scenario::ScenarioStatus Scenario::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  // stage类型unknow
  if (current_stage_ == nullptr) {
    AWARN << "Current stage is a null pointer.";
    return STATUS_UNKNOWN;
  }
  // stage全部执行完成
  if (current_stage_->stage_type() == StageType::NO_STAGE) {
    scenario_status_ = STATUS_DONE;
    return scenario_status_;
  }
  // 当前处于某一stage，调用这个stage的Process()函数，处理具体规划逻辑
  auto ret = current_stage_->Process(planning_init_point, frame);
  switch (ret) {
    case Stage::ERROR: {
      AERROR << "Stage '" << current_stage_->Name() << "' returns error";
      scenario_status_ = STATUS_UNKNOWN;
      break;
    }
    case Stage::RUNNING: {
      scenario_status_ = STATUS_PROCESSING;
      break;
    }
    // 读取当前stage完成的状态，并对下一个stage进行处理
    case Stage::FINISHED: {
      auto next_stage = current_stage_->NextStage();
      if (next_stage != current_stage_->stage_type()) {
        AINFO << "switch stage from " << current_stage_->Name() << " to "
              << StageType_Name(next_stage);
        if (next_stage == StageType::NO_STAGE) {
          scenario_status_ = STATUS_DONE;
          return scenario_status_;
        }
        if (stage_config_map_.find(next_stage) == stage_config_map_.end()) {
          AERROR << "Failed to find config for stage: " << next_stage;
          scenario_status_ = STATUS_UNKNOWN;
          return scenario_status_;
        }
        current_stage_ = CreateStage(*stage_config_map_[next_stage], injector_);
        if (current_stage_ == nullptr) {
          AWARN << "Current stage is a null pointer.";
          return STATUS_UNKNOWN;
        }
      }
      if (current_stage_ != nullptr &&
          current_stage_->stage_type() != StageType::NO_STAGE) {
        scenario_status_ = STATUS_PROCESSING;
      } else {
        scenario_status_ = STATUS_DONE;
      }
      break;
    }
    default: {
      AWARN << "Unexpected Stage return value: " << ret;
      scenario_status_ = STATUS_UNKNOWN;
    }
  }
  return scenario_status_;
}

Scenario都是顺序执行，只需要判断这一阶段是否结束，然后转到下一个阶段就可以了。CreateStage生成相应的stage，之后再进行stage的process。

Stage

以Lanefollowstage为例进行介绍。
modules/planning/scenarios/lane_follow/lane_follow_stage.cc

Stage::StageStatus LaneFollowStage::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool has_drivable_reference_line = false;

  ADEBUG << "Number of reference lines:\t"
         << frame->mutable_reference_line_info()->size();

  unsigned int count = 0;
  // 遍历所有的参考线，直到找到可用来规划的参考线后退出
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    // TODO(SHU): need refactor
    if (count++ == frame->mutable_reference_line_info()->size()) {
      break;
    }
    ADEBUG << "No: [" << count << "] Reference Line.";
    ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();
    // 找到可用来行驶的参考线，退出循环
    if (has_drivable_reference_line) {
      reference_line_info.SetDrivable(false);
      break;
    }
    // 执行LaneFollow的规划
    auto cur_status =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
    // 判断规划结果是否OK
    if (cur_status.ok()) {
      // 如果发生lanechange，判断reference_line的cost
      if (reference_line_info.IsChangeLanePath()) {
        ADEBUG << "reference line is lane change ref.";
        ADEBUG << "FLAGS_enable_smarter_lane_change: "
               << FLAGS_enable_smarter_lane_change;
        if (reference_line_info.Cost() < kStraightForwardLineCost &&
            (LaneChangeDecider::IsClearToChangeLane(&reference_line_info) ||
             FLAGS_enable_smarter_lane_change)) {
          // If the path and speed optimization succeed on target lane while
          // under smart lane-change or IsClearToChangeLane under older version
          has_drivable_reference_line = true;
          reference_line_info.SetDrivable(true);
          LaneChangeDecider::UpdatePreparationDistance(
              true, frame, &reference_line_info, injector_->planning_context());
          ADEBUG << "\tclear for lane change";
        } else {
          LaneChangeDecider::UpdatePreparationDistance(
              false, frame, &reference_line_info,
              injector_->planning_context());
          reference_line_info.SetDrivable(false);
          ADEBUG << "\tlane change failed";
        }
      } else {
      // 如果没有lanechange，stage执行结果为OK，则has_drivable_reference_line置位true
        ADEBUG << "reference line is NOT lane change ref.";
        has_drivable_reference_line = true;
      }
    } else {
      reference_line_info.SetDrivable(false);
    }
  }

  return has_drivable_reference_line ? StageStatus::RUNNING
                                     : StageStatus::ERROR;
}

Process主要是查找可供规划的参考线，之后在PlanOnReferenceLine函数中进行具体的执行。同时还对变道的状况进行规划，如果规划成功后，还需要判断目标车道的变道cost，如果cost太高，那么就会舍弃掉这条目标车道的reference_line, 此时放弃变道的规划，继续循环使用原车道的reference_line进行规划。

PlanOnReferenceLine函数

Status LaneFollowStage::PlanOnReferenceLine(
    const TrajectoryPoint& planning_start_point, Frame* frame,
    ReferenceLineInfo* reference_line_info) {
  // 判断是否有lanechange意图，如果否增加当前参考线的cost？有点疑问，增加了变道的可能
  if (!reference_line_info->IsChangeLanePath()) {
    reference_line_info->AddCost(kStraightForwardLineCost);
  }
  ADEBUG << "planning start point:" << planning_start_point.DebugString();
  ADEBUG << "Current reference_line_info is IsChangeLanePath: "
         << reference_line_info->IsChangeLanePath();
  // 顺序执行stage中的任务
  auto ret = Status::OK();
  for (auto* task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();
    // 执行每个task的具体逻辑
    ret = task->Execute(frame, reference_line_info);

    const double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
    ADEBUG << "after task[" << task->Name()
           << "]:" << reference_line_info->PathSpeedDebugString();
    ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";
    RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);
    // 如果task执行失败，退出task执行序列，并且记录失败信息
    if (!ret.ok()) {
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      break;
    }

    // TODO(SHU): disable reference line order changes for now
    // updated reference_line_info, because it is changed in
    // lane_change_decider by PrioritizeChangeLane().
    // reference_line_info = &frame->mutable_reference_line_info()->front();
    // ADEBUG << "Current reference_line_info is IsChangeLanePath: "
    //        << reference_line_info->IsChangeLanePath();
  }

  RecordObstacleDebugInfo(reference_line_info);

  // check path and speed results for path or speed fallback
  reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
  // 如果task执行失败，则使用备用的规划轨迹
  if (!ret.ok()) {
    PlanFallbackTrajectory(planning_start_point, frame, reference_line_info);
  }
  // 对规划的轨迹进行合成，如果合成失败，返回失败状态
  DiscretizedTrajectory trajectory;
  if (!reference_line_info->CombinePathAndSpeedProfile(
          planning_start_point.relative_time(),
          planning_start_point.path_point().s(), &trajectory)) {
    const std::string msg = "Fail to aggregate planning trajectory.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // determine if there is a destination on reference line.
  double dest_stop_s = -1.0;
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->LongitudinalDecision().has_stop() &&
        obstacle->LongitudinalDecision().stop().reason_code() ==
            STOP_REASON_DESTINATION) {
      SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                  reference_line_info->reference_line());
      dest_stop_s = dest_sl.s();
    }
  }
  // 增加障碍物的代价
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    if (!obstacle->IsStatic()) {
      continue;
    }
    if (obstacle->LongitudinalDecision().has_stop()) {
      bool add_stop_obstacle_cost = false;
      if (dest_stop_s < 0.0) {
        add_stop_obstacle_cost = true;
      } else {
        SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                    reference_line_info->reference_line());
        if (stop_sl.s() < dest_stop_s) {
          add_stop_obstacle_cost = true;
        }
      }
      if (add_stop_obstacle_cost) {
        static constexpr double kReferenceLineStaticObsCost = 1e3;
        reference_line_info->AddCost(kReferenceLineStaticObsCost);
      }
    }
  }

  if (FLAGS_enable_trajectory_check) {
    if (ConstraintChecker::ValidTrajectory(trajectory) !=
        ConstraintChecker::Result::VALID) {
      const std::string msg = "Current planning trajectory is not valid.";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  }

  reference_line_info->SetTrajectory(trajectory);
  reference_line_info->SetDrivable(true);
  return Status::OK();
}

ret = task->Execute(frame, reference_line_info);依次调用task_list中的task。

Task

还是以Lanefollow为例，Task的配置部分在此处。modules/planning/conf/scenario/lane_follow_config.pb.txt

stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

参考以及推荐阅

[1] Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1
[2] Apollo Planning决策规划代码详细解析 (1)：Scenario选择
[3] apollo介绍之planning模块(四)
[4] 解析百度Apollo之决策规划模块
[5] Apollo规划模块（一）：scenario
[6] Apollo星火计划学习笔记——Apollo决策规划技术详解及实现（以交通灯场景检测为例）
[7] Apollo Planning 模块

yuan〇
关注
专栏目录
Apollo9.0 PNC源码学习之Planning模块（三）—— public_road_planner
BigDavid123的博客
06-27 357
本文对public_road_planner做了源码解析
【Apollo 6.0学习笔记】Apollo模块
xiewf的博客
12-11 2489
本文主要简单整理下 Apollo 各模块接口及算法。
Apollo 算法阅读之Public Road轨迹规划算法--整体流程
同学醒醒放学了
05-26 4045
本文主要介绍Apollo内部的public road规划的主要流程，主要是针对lane_follow场景，因为该场景覆盖面更广。以下内容是本人的一些个人阅读代码时的个人理解，欢迎指正。 路径规划 1.在lane change decider中会进行referline_info的数量查看，如果没有开启换道功能，那么始终只有当前一个referline_info，删去换道路径；否则保留多个referline_info. 2.在lane_borrow_decider判断是否当前在借道场景， 如果...
百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\trajectory\PublishableTrajectory类代码解析
weixin_39199083的博客
04-23 615
对Apollo planning模块的PublishableTrajectory类进行解析
Apollo学习笔记
weixin_41245988的博客
04-26 3647
Apollo学习笔记 Apollo课程 智能驾驶入门课程 无人驾驶概览 1、软件层分为三层： 实时操作系统(RTOS)：确保在给定时间内完成特定任务，实时时确保系统稳定性、驾驶安全性的重要要求。通过在Ubuntu Linux操作系统加入Apollo内核，使其成为RTOS 运行时框架：ROS的定制版，改进了共享内存的功能和性能(一次写入多次读取)加快数据传输速度、去中心化和数据兼容性(ROS需要所有节点通信消息格式相同，当一个节点消息升级后，需要修改其他所有节点，比较麻烦，于是改进protobuf通信，在消息
Apollo开放平台9.0让自动驾驶开发者轻松上手
热门推荐
石臻臻的杂货铺(szzdzhp001)
12-19 2万+
在自动驾驶技术飞速发展的今天，成为这个领域的一名开发者是一次挑战、一次冒险，更是一次心灵之旅。作为这个领域的先锋之一，Apollo开放平台9.0于12月19日发布，同时Apollo开放平台9.0为开发者提供系统且全面的入门开发指南，让刚接触这个领域的开发者非常容易上手，下面与大家分享我在平台搭建中的成长与心得体会。
自动驾驶技术入门平台分享：百度Apollo开放平台9.0全方位升级
小淼淼的博客
12-19 8167
主模型更先进、效果更好，准召率大幅超越老模型。增量训练解决场景适应性问题，少量数据促场景效果大幅提升， 开源高质代码调优助模型训练独立完成。全面支持4D毫米波雷达，丰富前沿传感器选择，提升检测效果、加强极端天气场景安全性。调试流程更简洁窗口布局更灵活资源取用更方便降低学习使用成本操作更便捷阅读更顺畅内容更充实。
解析百度Apollo之参考线与轨迹——保罗的酒吧
David's Tweet
12-27 6885
本文紧接着上一篇文章：《解析百度Apollo之决策规划模块》。会详细讲解决策规划模块中是如何生成参考线和行车轨迹供车辆行驶的。参考线和轨迹直接影响了自动驾驶车辆对于方向和速度的控制，因此，说它是自动驾驶系统中最重要的数据都不为过。 前言 本文是百度自动驾驶平台Apollo项目系列文章中的一篇，会讲解Planning模块中的参考线逻辑。 对于刚接触Apollo项目的读者可以阅读我博客中的另外一篇文章 -《解析百度Apollo自动驾驶平台》，那里对Apollo项目做了整体的介绍。 ReferenceL.
Apollo Planning（一）
whuzhang16的博客
01-17 1398
Apollo Planning
Apollo Planning（三）
whuzhang16的博客
01-17 1292
一、Scenario 我们同样先看下"scenarios"的目录结构： . ├── bare_intersection ├── common ├── dead_end ├── emergency ├── lane_follow // 车道线保持 ├── narrow_street_u_turn // 狭窄掉头 ├── learning_model ├── park // 泊车 ├── park_and_go ├── stop_sign // 停止 ├── traffi
Apollo星火计划学习笔记——第一讲 使用Apollo学习自动驾驶
m0_51902001的博客
10-01 2121
学习Apollo"星火计划"系列培训课程，进一步学习自动驾驶理论与实践
Apollo教程笔记1——无人驾驶概览
lalala
10-28 833
文章目录What will you learn?HDMapsLocalizationPerceptionPredictionPlanningControlWhat is Self-Driving?How Self-Driving Cars Work?Apollo Team and ArchitectureReference Vehicle Platform and Hardware Platfor...
apollo学习之---planning理论到实践（1-1）总体流程及架构
qq_41593516的博客
09-24 1860
Lattice Planner从学习到实践
【Apollo 6.0学习笔记】Apollo 6.0软硬件框架
xiewf的博客
12-10 2538
主要介绍 Apollo 6.0软硬件框架。
百度Apollo：自动驾驶的领航者
鸽芷咕的博客
01-04 1171
在自动驾驶系统中，摄像头是一种重要的感知传感器，负责捕捉周围环境的图像信息。Camera感知模块是自动驾驶系统中的核心组成部分之一，它通过处理摄像头采集的图像数据来实现目标检测、车道线识别等功能。
Apollo智能驾驶|百度Apollo开放平台－应用层介绍(2)
最新发布
2301_77162163的博客
01-31 3万+
在RTK定位的基础上，增加激光点云SLAM定位方案，解决了园区场景中的树木、建筑物遮挡导致的定位漂移问题。开发者通过该方案可以解决如下图所示的树木遮挡导致的定位漂移问题，也可以适配短距离建筑物、桥梁遮挡等场景。遮挡环境下SLAM定位在感知方面，我们简化了Lidar与Camera融合的难度，同时使用百万级别的标注数据对模型进行训练，使得感知的准召率大幅提升。
Apollo Routing模块源代码分析
davidhopper的博客
01-27 1万+
严正声明：本文系作者davidhopper原创，未经允许，严禁转载！ Apollo Routing模块位于命名空间：apollo::routing，其作用在简单意义上可以理解为实现无人车软件系统内部的导航功能，即在宏观层面上指导无人车软件系统的规划（Planning）模块按照什么样的道路行驶，以便顺利完成从起点到终点的行驶。值得指出的是，这里的路由寻径虽然在一定程序上类似传统导航，但其在细节上紧
apollo规划:scenario-＞stage-＞task
sinat_39307513的博客
11-22 3744
Apollo中的Planning（规划）模块的场景选择是通过scenario-stage-task并利用状态机行车对车辆状态和规划功能进行维护的。 一.场景scenario scenario代表当前所处的场景，包括正常的路上行驶场景、停车场景、红绿灯路口场景等。 scenario功能模块中的主要功能如下： 通过Scenario_manager作为scenario场景的执行和处理的入口，是由PublicRoadPlanner::Plan(）调用 scenario及其子类，通过定义的场景以及con
Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1
sinat_52032317的博客
12-13 3806
在自动驾驶系统中，与人类驾驶员对比，感知模块相当于人类眼和耳朵;地图搜索路径和车道级别的路径规划，相当于人脑对道路的记忆和搜索；最后决策规划功能，相当于人类对道路环境和客观世界的反应和判断。规划技术就是包括地图搜索路径，以及道路决策和规划这部分重要的功能。Apollo规划模块根据感知预测的结果，当前车辆定位信息，以及Routing的结果，规划出一条安全行驶的轨迹，这个轨迹会发送给Control模块，Control模块通过油门，刹车和方向盘，控制车辆按照规划的轨迹运行。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

yuan〇

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值