百度apollo自动驾驶modules\planning\tasks\optimizers\path_time_heuristic\PathTimeHeuristicOptimizer类代码详解

概述

PathTimeHeuristicOptimizer类是apollo planning模块下modules\planning\tasks\optimizers\path_time_heuristic\path_time_heuristic_optimizer.cc/.h实现

从类名来看，PathTimeHeuristicOptimizer应该是路径时间启发式优化器，根据参考的链接可知该类是apollo纵向速度规划DP实现的主入口。

从代码来看PathTimeHeuristicOptimizer类主要是实现：
1.在构造该类对象时加载DP速度规划算法的参数配置；
2.调用相关类执行速度规划的DP算法，并将DP算法的搜索结果传出。

*DP 动态规划（Dynamic programming）是一种在数学、计算机科学和经济学中使用的，通过把原问题分解为相对简单的子问题的方式求解复杂问题的方法。
apollo中纵向速度规划的初始粗糙解就是由DP算法求出，再通过QP(二次规划)，NLP(非线性约束优化)对粗糙解进行优化。

参考链接：
Apollo 6.0的EM Planner (3)：速度规划的动态规划DP过程

path_time_heuristic_optimizer.h

#pragma once

#include <string>

#include "modules/planning/proto/planning_internal.pb.h"
#include "modules/planning/proto/task_config.pb.h"
#include "modules/planning/tasks/optimizers/speed_optimizer.h"

namespace apollo {
namespace planning {

/**
 * @class PathTimeHeuristicOptimizer
 * @brief PathTimeHeuristicOptimizer does ST graph speed planning with dynamic
 * programming algorithm.
 */
class PathTimeHeuristicOptimizer : public SpeedOptimizer {
 public:
 
 //PathTimeHeuristicOptimizer类构造函数，入参是一个TaskConfig的类对象大概是什么任务参数配置之类的
 //PathTimeHeuristicOptimizer类继承SpeedOptimizer类
 //其实当调用该带参构造函数时，将入参初始化一个基类对象作为PathTimeHeuristicOptimizer类对象初始化
 //将入参配置对象里的速度启发式优化器的相关配置参数赋值给类成员
  explicit PathTimeHeuristicOptimizer(const TaskConfig& config);

 private:
 //类成员函数 Process处理函数
//输入参数1：path_data路径数据(纯路径无速度规划)
//输入参数2：规划起始点init_point
//输入参数3：speed_data一系列的速度点代表速度规划，用来存放DP速度规划的搜索结果，这里没有理解，为甚么const的指针形参可以被赋值用来存放函数的运行结果？
  common::Status Process(const PathData& path_data,
                         const common::TrajectoryPoint& init_point,
                         SpeedData* const speed_data) override;

//成员函数，寻找路径时间图？返回值是一个布尔类型，入参是SpeedData类对象指针
//入参speed_data简单来说就是一个ST点vector数组，形式类似于{(s0,t0,v0,a0,da0),(s1,t1,v1,a1,da1),(s2,t2,v2,a2,da2),...}
//但其实这里的入参speed_data应该是个空指针用来存放DP的搜索结果
//这个函数的返回值代表DP求解成功与否
  bool SearchPathTimeGraph(SpeedData* speed_data) const;

 private:
  common::TrajectoryPoint init_point_;
  SLBoundary adc_sl_boundary_;
  SpeedHeuristicOptimizerConfig speed_heuristic_optimizer_config_;
};

}  // namespace planning
}  // namespace apollo

path_time_heuristic_optimizer.cc

#include "modules/planning/tasks/optimizers/path_time_heuristic/path_time_heuristic_optimizer.h"

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/st_graph_data.h"
#include "modules/planning/tasks/optimizers/path_time_heuristic/gridded_path_time_graph.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;

//PathTimeHeuristicOptimizer类构造函数，入参是一个TaskConfig的类对象大概是什么任务参数配置之类的
PathTimeHeuristicOptimizer::PathTimeHeuristicOptimizer(const TaskConfig& config)
    //PathTimeHeuristicOptimizer类继承SpeedOptimizer类
    //其实当调用该带参构造函数时，将入参初始化一个基类对象作为PathTimeHeuristicOptimizer类对象初始化
    : SpeedOptimizer(config) { 
  //检查配置对象里有无速度启发式优化器的相关配置参数？
  ACHECK(config.has_speed_heuristic_optimizer_config());
  //将入参配置对象里的速度启发式优化器的相关配置参数赋值给类成员speed_heuristic_optimizer_config_
  speed_heuristic_optimizer_config_ = config.speed_heuristic_optimizer_config();
}

//成员函数，寻找路径时间图？返回值是一个布尔类型，入参是SpeedData类对象指针
//入参speed_data简单来说就是一个ST点vector数组，形式类似于{(s0,t0,v0,a0,da0),(s1,t1,v1,a1,da1),(s2,t2,v2,a2,da2),...}
//但其实这里的入参speed_data应该是个空指针用来存放DP的搜索结果
//这个函数的返回值代表DP求解成功与否
bool PathTimeHeuristicOptimizer::SearchPathTimeGraph(
    SpeedData* speed_data) const {
    //这里从类成员参考线信息类对象reference_line_info_上获取DP速度规划算法的参数配置，变道和非变道时配置不同，这里有个问题是这个类成员reference_line_info_在哪里被传进来的？
  const auto& dp_st_speed_optimizer_config =
      reference_line_info_->IsChangeLanePath()
          ? speed_heuristic_optimizer_config_.lane_change_speed_config()
          : speed_heuristic_optimizer_config_.default_speed_config();

//定义了个GriddedPathTimeGraph网格化ST图类对象st_graph
//初始化这个类对象的参数：
//参数1：reference_line_info_里的st图数据
//参数2：DP速度规划算法的参数配置
//参数3：reference_line_info_里的路径决策里的障碍物列表
//参数4：init_point_规划起始点
  GriddedPathTimeGraph st_graph(
      reference_line_info_->st_graph_data(), dp_st_speed_optimizer_config,
      reference_line_info_->path_decision()->obstacles().Items(), init_point_);
//调用GriddedPathTimeGraph类的Search函数将DP的搜索结果存放于speed_data里
//若搜索失败则报错，返回false
  if (!st_graph.Search(speed_data).ok()) {
    AERROR << "failed to search graph with dynamic programming.";
    return false;
  }
  //若DP搜索成功则返回true
  return true;
}

//类成员函数 Process处理函数
//输入参数1：path_data路径数据(纯路径无速度规划)
//输入参数2：规划起始点init_point
//输入参数3：speed_data一系列的速度点代表速度规划，用来存放DP速度规划的搜索结果，这里没有理解，为甚么const的指针形参可以被赋值用来存放函数的运行结果？
Status PathTimeHeuristicOptimizer::Process(
    const PathData& path_data, const common::TrajectoryPoint& init_point,
    SpeedData* const speed_data) {
  //将入参的规划起始点赋值给字节的类成员init_point_
  init_point_ = init_point;

  //如果路径数据里离散路径点是空的则报错，报故障码，空的路径
  if (path_data.discretized_path().empty()) {
    const std::string msg = "Empty path data";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
//调用SearchPathTimeGraph函数执行速度规划DP搜索，以speed_data作为输入参数用来存放DP的搜索结果，如果搜索失败就报错
//不管搜索失败与否都将速度规划speed_data塞到reference_line_info_里的ST图debug对象里
  if (!SearchPathTimeGraph(speed_data)) {
    const std::string msg = absl::StrCat(
        Name(), ": Failed to search graph with dynamic programming.");
    AERROR << msg;
    RecordDebugInfo(*speed_data, reference_line_info_->mutable_st_graph_data()
                                     ->mutable_st_graph_debug());
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  RecordDebugInfo(
      *speed_data,
      reference_line_info_->mutable_st_graph_data()->mutable_st_graph_debug());
  return Status::OK();
}

}  // namespace planning
}  // namespace apollo