apollo之规划

原创已于 2025-11-20 09:04:59 修改 · 762 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#apollo

于 2025-11-18 22:37:11 首次发布

apollo 专栏收录该内容

1 篇文章

订阅专栏

规划组件

组件名为PlanningComponent，其依赖PlanningBase

PlanningBase

其依赖规划器Planner和交通决策器TrafficDecider

处理时序为
在这里插入图片描述

Planner

规划器

场景Scenario

场景的pipeline配置是在conf/pipeline.pb.txt中

message StagePipeline {
    // The alias name of the stage.
    required string name = 1;
    // The class type of the stage.
    required string type = 2;
    // If the stage is disabled, it will be skipped.
    optional bool enabled = 3;
    // The contained task list of the stage pipeline.
    repeated PluginDeclareInfo task = 4;
    // Task executed when path or speed planning failed.
    optional PluginDeclareInfo fallback_task = 5;
}

message ScenarioPipeline {
    // The contained stage list of the scenarion pipeline.
    repeated StagePipeline stage = 1;
}

在Scenario的process中来控制Stage的转移，isTransferable看场景是否需要切换
场景有一系列的阶段构成，在当前阶段完成后，没有下一阶段，则表示场景完成，需要调用Update来更新场景

场景管理器ScenarioManager

Update：控制场景的变换

阶段Stage

阶段中包含context(为所在场景的上下文)

Init：初始化Task

bool Stage::Init(const StagePipeline& config,
                 const std::shared_ptr<DependencyInjector>& injector,
                 const std::string& config_dir, void* context) {
  pipeline_config_ = config;
  next_stage_ = config.name();
  injector_ = injector;
  name_ = config.name();
  context_ = context;
  injector_->planning_context()
      ->mutable_planning_status()
      ->mutable_scenario()
      ->set_stage_type(name_);
  std::string path_name = ConfigUtil::TransformToPathName(name_);
  std::string task_config_dir = config_dir + "/" + path_name;
  // Load task plugin.
  for (int i = 0; i < pipeline_config_.task_size(); ++i) {
    auto task = pipeline_config_.task(i);
    auto task_type = task.type();
    auto task_ptr = apollo::cyber::plugin_manager::PluginManager::Instance()
                        ->CreateInstance<Task>(
                            ConfigUtil::GetFullPlanningClassName(task_type));
    if (nullptr == task_ptr) {
      AERROR << "Create task " << task.name() << " of " << name_ << " failed!";
      return false;
    }
    if (task_ptr->Init(task_config_dir, task.name(), injector)) {
      task_list_.push_back(task_ptr);
    } else {
      AERROR << task.name() << " init failed!";
      return false;
    }
  }
  // Load trajectory fallback task.
  // If fallback task is not set, use "FastStopTrajectoryFallback" as default.
  std::string fallback_task_type = "FastStopTrajectoryFallback";
  std::string fallback_task_name = "FAST_STOP_TRAJECTORY_FALLBACK";
  if (pipeline_config_.has_fallback_task()) {
    fallback_task_type = pipeline_config_.fallback_task().type();
    fallback_task_name = pipeline_config_.fallback_task().name();
  }
  fallback_task_ =
      apollo::cyber::plugin_manager::PluginManager::Instance()
          ->CreateInstance<Task>(
              ConfigUtil::GetFullPlanningClassName(fallback_task_type));
  if (nullptr == fallback_task_) {
    AERROR << "Create fallback task " << fallback_task_name << " of " << name_
           << " failed!";
    return false;
  }
  if (!fallback_task_->Init(task_config_dir, fallback_task_name, injector)) {
    AERROR << fallback_task_name << " init failed!";
    return false;
  }
  return true;
}

ExecuteTaskOnReferenceLine，ExecuteTaskOnReferenceLineForOnlineLeanring和ExecuteTaskOnOpenSpace：会调用Task的Execute方法

StageResult Stage::ExecuteTaskOnReferenceLine(
    const common::TrajectoryPoint& planning_start_point, Frame* frame) {
  StageResult stage_result;
  if (frame->reference_line_info().empty()) {
    AERROR << "referenceline is empty in stage" << name_;
    return stage_result.SetStageStatus(StageStatusType::ERROR);
  }
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    if (!reference_line_info.IsDrivable()) {
      AERROR << "The generated path is not drivable skip";
      reference_line_info.SetDrivable(false);
      continue;
    }

    if (reference_line_info.IsChangeLanePath()) {
      AERROR << "The generated refline is change lane path, skip";
      reference_line_info.SetDrivable(false);
      continue;
    }
    common::Status ret = common::Status::OK();
    for (auto task : task_list_) {
      const double start_timestamp = Clock::NowInSeconds();

      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.";
      AINFO << "Planning Perf: task name [" << task->Name() << "], "
            << time_diff_ms << " ms.";
      RecordDebugInfo(&reference_line_info, task->Name(), time_diff_ms);

      if (!ret.ok()) {
        stage_result.SetTaskStatus(ret);
        AERROR << "Failed to run tasks[" << task->Name()
               << "], Error message: " << ret.error_message();
        break;
      }
    }
    // Generate fallback trajectory in case of task error.
    if (!ret.ok()) {
      fallback_task_->Execute(frame, &reference_line_info);
    }
    DiscretizedTrajectory trajectory;
    if (!reference_line_info.CombinePathAndSpeedProfile(
            planning_start_point.relative_time(),
            planning_start_point.path_point().s(), &trajectory)) {
      AERROR << "Fail to aggregate planning trajectory."
             << reference_line_info.IsChangeLanePath();
      reference_line_info.SetDrivable(false);
      continue;
    }
    reference_line_info.SetTrajectory(trajectory);
    reference_line_info.SetDrivable(true);
    return stage_result;
  }
  return stage_result;
}

StageResult Stage::ExecuteTaskOnReferenceLineForOnlineLearning(
    const common::TrajectoryPoint& planning_start_point, Frame* frame) {
  // online learning mode
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    reference_line_info.SetDrivable(false);
  }

  StageResult stage_result;
  // FIXME(all): current only pick up the first reference line to use
  // learning model trajectory
  auto& picked_reference_line_info =
      frame->mutable_reference_line_info()->front();
  for (auto task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();

    const auto ret = task->Execute(frame, &picked_reference_line_info);

    const double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
    ADEBUG << "task[" << task->Name() << "] time spent: " << time_diff_ms
           << " ms.";
    RecordDebugInfo(&picked_reference_line_info, task->Name(), time_diff_ms);

    if (!ret.ok()) {
      stage_result.SetTaskStatus(ret);
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      break;
    }
  }

  const std::vector<common::TrajectoryPoint>& adc_future_trajectory_points =
      picked_reference_line_info.trajectory();
  DiscretizedTrajectory trajectory;
  if (picked_reference_line_info.AdjustTrajectoryWhichStartsFromCurrentPos(
          planning_start_point, adc_future_trajectory_points, &trajectory)) {
    picked_reference_line_info.SetTrajectory(trajectory);
    picked_reference_line_info.SetDrivable(true);
    picked_reference_line_info.SetCost(0);
  }

  return stage_result;
}

StageResult Stage::ExecuteTaskOnOpenSpace(Frame* frame) {
  auto ret = common::Status::OK();
  StageResult stage_result;
  for (auto task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();

    ret = task->Execute(frame);

    if (!ret.ok()) {
      stage_result.SetTaskStatus(ret);
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      const double end_timestamp = Clock::NowInSeconds();
      const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
      AINFO << "Planning Perf: task name [" << task->Name() << "], "
            << time_diff_ms << " ms.";
      return stage_result;
    }

    const double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
    AINFO << "Planning Perf: task name [" << task->Name() << "], "
          << time_diff_ms << " ms.";
  }

  if (frame->open_space_info().fallback_flag() ||
      frame->open_space_info().stop_flag()) {
    auto& trajectory = frame->open_space_info().fallback_trajectory().first;
    auto& gear = frame->open_space_info().fallback_trajectory().second;
    PublishableTrajectory publishable_trajectory(Clock::NowInSeconds(),
                                                 trajectory);
    auto publishable_traj_and_gear =
        std::make_pair(std::move(publishable_trajectory), gear);

    *(frame->mutable_open_space_info()->mutable_publishable_trajectory_data()) =
        std::move(publishable_traj_and_gear);
  } else {
    auto& trajectory =
        frame->open_space_info().chosen_partitioned_trajectory().first;
    auto& gear =
        frame->open_space_info().chosen_partitioned_trajectory().second;
    PublishableTrajectory publishable_trajectory(Clock::NowInSeconds(),
                                                 trajectory);
    auto publishable_traj_and_gear =
        std::make_pair(std::move(publishable_trajectory), gear);

    *(frame->mutable_open_space_info()->mutable_publishable_trajectory_data()) =
        std::move(publishable_traj_and_gear);
  }
  return stage_result;
}

具体的Stage实现类在modules/planning/scenarios下

任务Task

//SpeedOptimizer的处理抽象方法
virtual common::Status Process(const PathData& path_data,
                                 const common::TrajectoryPoint& init_point,
                                 SpeedData* const speed_data) = 0;
//TrajectoryOptimizer的处理抽象方法
virtual apollo::common::Status Process() = 0;

具体的Task实现类在modules/planning/tasks下
Init：任务初始化

bool Task::Init(const std::string& config_dir, const std::string& name,
                const std::shared_ptr<DependencyInjector>& injector) {
  injector_ = injector;
  name_ = name;
  config_path_ =
      config_dir + "/" + ConfigUtil::TransformToPathName(name) + ".pb.txt";

  // Get the name of this class.
  int status;
  std::string class_name =
      abi::__cxa_demangle(typeid(*this).name(), 0, 0, &status);
  // Generate the default task config path from PluginManager.
  default_config_path_ =
      apollo::cyber::plugin_manager::PluginManager::Instance()
          ->GetPluginConfPath<Task>(class_name, "conf/default_conf.pb.txt");
  return true;
}

Execute：任务执行，将参数赋值给成员frame_,reference_line_info_，其它交给具体任务的处理虚函数执行

Status Task::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) {
  frame_ = frame;
  reference_line_info_ = reference_line_info;
  return Status::OK();
}

Status Task::Execute(Frame* frame) {
  frame_ = frame;
  return Status::OK();
}