planning模块(9)-路径规划(scenario/stage/task)

前面已经介绍过了参考线的平滑算法,接下来继续介绍路径规划算法.
1.

bool PlanningComponent::Proc(
    const std::shared_ptr<prediction::PredictionObstacles>&
        prediction_obstacles,
    const std::shared_ptr<canbus::Chassis>& chassis,
    const std::shared_ptr<localization::LocalizationEstimate>&
        localization_estimate)

prediction_obstacles:感知结果
chassis:底盘数据
localization_estimate:定位数据
2.

planning_base_->RunOnce(local_view_, &adc_trajectory_pb);

local_view_:用来集中保存一次规划周期中Planning所“看到”的所有输入数据
adc_trajectory_pb:planning最终输出的轨迹
3.

status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);

start_timestamp:时钟同步后,系统启动以来流逝了多少秒
stitching_trajectory:拼接轨迹
ptr_trajectory_pb:planning最终输出的轨迹

轨迹拼接会单独介绍,这里先介绍第一次规划轨迹的流程,轨迹拼接是在有上一帧轨迹的情况下,才会判断是否进行拼接.所以这里跳过并不影响理清路径规划的流程.但还是要看一下ComputeStitchingTrajectory函数.

std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const canbus::Chassis& vehicle_chassis, const VehicleState& vehicle_state,
    const double current_timestamp, const double planning_cycle_time,
    const size_t preserved_points_num, const bool replan_by_offset,
    const PublishableTrajectory* prev_trajectory, std::string* replan_reason,
    const control::ControlInteractiveMsg& control_interactive_msg)

vehicle_chassis:底盘数据
vehicle_state:当前自车数据
current_timestamp:时钟同步后,系统启动以来流逝了多少秒
planning_cycle_time:表示planning每隔多久执行一次,默认是0.01s
preserved_points_num:表示在轨迹拼接时，从上一帧已发布轨迹中“保留多少个轨迹点”继续使用,默认是20个轨迹点
replan_by_offset:默认为true
prev_trajectory:上一帧生成的轨迹
replan_reason:重新规划的原因
control_interactive_msg:

std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const canbus::Chassis& vehicle_chassis, const VehicleState& vehicle_state,
    const double current_timestamp, const double planning_cycle_time,
    const size_t preserved_points_num, const bool replan_by_offset,
    const PublishableTrajectory* prev_trajectory, std::string* replan_reason,
    const control::ControlInteractiveMsg& control_interactive_msg) {
  // 1.check replan not by offset
  size_t time_matched_index = 0;
  if (need_replan_by_necessary_check(vehicle_state, current_timestamp,
                                     prev_trajectory, replan_reason,
                                     &time_matched_index)) {
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

bool TrajectoryStitcher::need_replan_by_necessary_check(
    const common::VehicleState& vehicle_state, const double current_timestamp,
    const PublishableTrajectory* prev_trajectory, std::string* replan_reason,
    size_t* time_matched_index) {
  if (!FLAGS_enable_trajectory_stitcher) {
    *replan_reason = "stitch is disabled by gflag.";
    return true;
  }
  if (!prev_trajectory) {
    *replan_reason = "replan for no previous trajectory.";
    return true;
  }

如果上一帧轨迹为空,则直接返回true

ComputeReinitStitchingTrajectory函数

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const double planning_cycle_time, const VehicleState& vehicle_state) {
  TrajectoryPoint reinit_point;
  static constexpr double kEpsilon_v = 0.1;
  static constexpr double kEpsilon_a = 0.4;
  // TODO(Jinyun/Yu): adjust kEpsilon if corrected IMU acceleration provided
  if (std::abs(vehicle_state.linear_velocity()) < kEpsilon_v &&
      std::abs(vehicle_state.linear_acceleration()) < kEpsilon_a) {
    reinit_point = ComputeTrajectoryPointFromVehicleState(planning_cycle_time,
                                                          vehicle_state);
  } else {
    VehicleState predicted_vehicle_state;
    predicted_vehicle_state =
        VehicleModel::Predict(planning_cycle_time, vehicle_state);
    reinit_point = ComputeTrajectoryPointFromVehicleState(
        planning_cycle_time, predicted_vehicle_state);
  }

  return std::vector<TrajectoryPoint>(1, reinit_point);
}

因为最开始的时候,车一定是停着的,所以车的速度和加速度为0,那么就会直接调用
ComputeTrajectoryPointFromVehicleState函数

TrajectoryPoint TrajectoryStitcher::ComputeTrajectoryPointFromVehicleState(
    const double planning_cycle_time, const VehicleState& vehicle_state) {
  TrajectoryPoint point;
  point.mutable_path_point()->set_s(0.0);
  point.mutable_path_point()->set_x(vehicle_state.x());
  point.mutable_path_point()->set_y(vehicle_state.y());
  point.mutable_path_point()->set_z(vehicle_state.z());
  point.mutable_path_point()->set_theta(vehicle_state.heading());
  point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  point.set_v(vehicle_state.linear_velocity());
  point.set_a(vehicle_state.linear_acceleration());
  point.set_relative_time(planning_cycle_time);
  return point;
}

此时返回的拼接轨迹stitching_trajectory,只有一个轨迹点在当前自车位置
4.

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

stitching_trajectory.back():就是上面使用自车位置构建的轨迹点,这也是规划的起点
frame_:Frame类包含了planning一次执行中所用到的所有信息,它也包括后续规划器的所有的输入信息,输出信息

这个专栏只介绍PublicRoadPlanner规划器

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                               Frame* frame,
                               ADCTrajectory* ptr_computed_trajectory) {
  scenario_manager_.Update(planning_start_point, frame);

scenario_manager_:是场景管理器,public road planner包含的所有场景在配置文件/home/zsy/apollo/modules/planning/planners/public_road/conf/planner_config.pb.txt中

void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                             Frame* frame) {
  CHECK_NOTNULL(frame);
  for (auto scenario : scenario_list_) {
    if (current_scenario_.get() == scenario.get() &&
        current_scenario_->GetStatus() ==
            ScenarioStatusType::STATUS_PROCESSING) {
      // The previous scenario has higher priority
      return;
    }
    if (scenario->IsTransferable(current_scenario_.get(), *frame)) {
      current_scenario_->Exit(frame);
      AINFO << "switch scenario from" << current_scenario_->Name() << " to "
            << scenario->Name();
      current_scenario_ = scenario;
      current_scenario_->Reset();
      current_scenario_->Enter(frame);
      return;
    }
  }
}

当一个规划器调用Update时,它会遍历planner_config.pb.txt配置中的所有场景,然后通过调用每个场景自己的IsTransferable来判断,当前自车所在位置符合进入哪个场景.

auto result = scenario_->Process(planning_start_point, frame);

ScenarioResult Scenario::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  if (current_stage_ == nullptr) {
    current_stage_ = CreateStage(
        *stage_pipeline_map_[scenario_pipeline_config_.stage(0).name()]);

根据modules/planning/scenarios/lane_follow/conf/pipeline.pb.txt配置文件创建stage

auto ret = current_stage_->Process(planning_init_point, frame);

这里主要介绍LANE_FOLLOW_STAGE

    result =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);

在LaneFollowStage::Process中,会遍历平滑后的参考线,然后分别执行上面逻辑

task->Execute(frame, reference_line_info)

在PlanOnReferenceLine函数中会遍历当前stage,modules/planning/scenarios/lane_follow/conf/pipeline.pb.txt中所有task任务.
每个task类的父类PathGeneration中有一个

apollo::common::Status PathGeneration::Execute(
    Frame* frame, ReferenceLineInfo* reference_line_info) {
  Task::Execute(frame, reference_line_info);
  return Process(frame, reference_line_info);
}

这样的话,配置文件中的每一个task自己的Process函数都会被执行
在介绍lane_follow中的task前,介绍一下初始化流程

bool PlanningComponent::Init() {
injector_ = std::make_shared<DependencyInjector>();
...

planning_base_->Init(config_);

planner_->Init(injector_, FLAGS_planner_config_path);

scenario_manager_.Init(injector, config_);

scenario->Init(injector_, planner_config.scenario(i).name())

stage_ptr->Init(stage_pipeline, injector_, config_dir_, GetContext())

task_ptr->Init(task_config_dir, task.name(), injector)

injector_是DependencyInjector类型.
DependencyInjector:是一个数据缓存中心,管理planning过程中的实时数据和几乎所有的历史数据,用于规划任务的前后帧之间的承接,以及异常处理的回溯

PlanningContext planning_context：负责planning上下文的缓存，比如变道、借道、巡航等各种场景下的规划信息。
FrameHistory frame_history：是一个可索引队列，负责planning的输入、输出主要信息的缓存，以Frame类进行组织，内部包含LocalView结构体（负责输入数据的融合管理）。与上述的History不同的是，该数据自模块启动后就开始缓存所有的Frame对象，不受routing变动的影响。
History history：负责障碍物状态的缓存，包括运动状态、决策结果。该数据与routing结果绑定，routing变更后会清理掉历史数据。
EgoInfo ego_info：提供车辆动、静信息，即车辆运动状态参数（轨迹、速度、加速度等）和车辆结构参数（长宽高等）。
apollo::common::VehicleStateProvider vehicle_state：车辆状态提供器，用于获取车辆实时信息。
LearningBasedData learning_based_data：基于学习的数据，用于学习建模等。

LANE_FOLLOW_STAGE中,如果是换道的情况,下面task会被执行

I1215 22:07:27.587636 365977 lane_follow_stage.cc:169] Planning Perf: task name [LANE_CHANGE_PATH], 0.139952 ms.
I1215 22:07:27.590972 365977 lane_follow_stage.cc:169] Planning Perf: task name [LANE_FOLLOW_PATH], 3.33333 ms.
I1215 22:07:27.590978 365977 lane_follow_stage.cc:169] Planning Perf: task name [LANE_BORROW_PATH], 0.00405312 ms.
I1215 22:07:27.590981 365977 lane_follow_stage.cc:169] Planning Perf: task name [FALLBACK_PATH], 0.00119209 ms.
I1215 22:07:27.591069 365977 lane_follow_stage.cc:169] Planning Perf: task name [PATH_DECIDER], 0.0867844 ms.
I1215 22:07:27.591073 365977 lane_follow_stage.cc:169] Planning Perf: task name [RULE_BASED_STOP_DECIDER], 0.00119209 ms.
I1215 22:07:27.591271 365977 lane_follow_stage.cc:169] Planning Perf: task name [SPEED_BOUNDS_PRIORI_DECIDER], 0.198126 ms.
I1215 22:07:27.591984 365977 lane_follow_stage.cc:169] Planning Perf: task name [SPEED_HEURISTIC_OPTIMIZER], 0.710964 ms.
I1215 22:07:27.591989 365977 lane_follow_stage.cc:169] Planning Perf: task name [SPEED_DECIDER], 0.0026226 ms.
I1215 22:07:27.592176 365977 lane_follow_stage.cc:169] Planning Perf: task name [SPEED_BOUNDS_FINAL_DECIDER], 0.186205 ms.
I1215 22:07:27.594007 365977 lane_follow_stage.cc:169] Planning Perf: task name [PIECEWISE_JERK_SPEED], 1.82629 ms.