文章目录

一. dp_poly_path_optimizer

看一下 dp path 部分的实现

一. dp_poly_path_optimizer

主要在 dp_poly_path_optimizer 这个文件中，在这个路径下

apollo/modules/planning/tasks/optimizers/dp_poly_path/dp_poly_path_optimizer.h

dp_poly_path_optimizer.h

class DpPolyPathOptimizer : public PathOptimizer {
    
    
 public:
  explicit DpPolyPathOptimizer(const TaskConfig &config);

 private:
  apollo::common::Status Process(const SpeedData &speed_data,
                                 const ReferenceLine &reference_line,
                                 const common::TrajectoryPoint &init_point,
                                 PathData *const path_data) override;
};

分析：

首先 DpPolyPathOptimizer 这个类，继承自 PathOptimizer
构造函数传入了一个 TaskConfig 类型的变量，初始化
主要的一个函数 Process()

下面依次来看一下

这里有两个关键字 explicit 和 override，顺便看一下用法：

explicit

构造的时候必须显示构造，举个例子

DpPolyPathOptimizer dp_path_opt(config);   //编译通过
DpPolyPathOptimizer dp_path_opt = config;  //编译不通过，不允许隐式的转换

override

如果派生类在虚函数声明时使用了override描述符，那么该函数必须重载其基类中的同名函数，否则代码将无法通过编译

1. PathOptimizer 类

class PathOptimizer : public Task {
    
    
 public:
  explicit PathOptimizer(const TaskConfig &config);
  virtual ~PathOptimizer() = default;
  apollo::common::Status Execute(
      Frame *frame, ReferenceLineInfo *reference_line_info) override;

 protected:
  virtual apollo::common::Status Process(
      const SpeedData &speed_data, const ReferenceLine &reference_line,
      const common::TrajectoryPoint &init_point, PathData *const path_data) = 0;

  void RecordDebugInfo(const PathData &path_data);
};

比较重要的：

继承自 Task 这个类
构造函数也是传入 TaskConfig 类型的变量，初始化
Execute 函数，传入 Frame*， ReferenceLineInfo* 在这里重写
Process() 纯虚函数，在子类中重写

先放结论：

Task 类提供两个虚函数 Init、Execute，其中Init 函数用于任务对象的初始化，Task 类目前未做任何实际工作
Execute用于执行实际的优化，Task类也未实现任何功能
PathOptimizer 类重载了 Task 类的虚函数Execute，同时提供一个保护的纯虚函数 Process（由派生类具体实现），在 Execute 内部调用 Process函数完成实际的优化
注意：PathOptimizer是虚基类，不可直接使用它创建类对象，必须从该类派生出具体类后方可实例化

那还是，一个一个分析吧

(1). Task 类

class Task {
    
    
 public:
  explicit Task(const TaskConfig& config);

  virtual ~Task() = default;

  void SetName(const std::string& name) {
    
     name_ = name; }

  const std::string& Name() const;

  const TaskConfig& Config() const {
    
     return config_; }

  virtual apollo::common::Status Execute(
      Frame* frame, ReferenceLineInfo* reference_line_info);

 protected:
  Frame* frame_ = nullptr;
  ReferenceLineInfo* reference_line_info_ = nullptr;

  TaskConfig config_;
  std::string name_;
};

Task::Task(const TaskConfig& config) : config_(config) {
    
    
  name_ = TaskConfig::TaskType_Name(config_.task_type());
}

const std::string& Task::Name() const {
    
     return name_; }

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

分析：

这个就是最上层的基类了

构造函数传入 config，设置这个 config 的 name_
Execute() 函数，输入 Frame 类型和 ReferenceLineInfo 类型的指针初始化

到这里这个 Task 类，基本整明白了，还有几个小问题：

TaskConfig 类型是什么
Frame 类型是什么
ReferenceLineInfo 类型是什么

那就接着往下看

a. TaskConfig

首先要知道在哪里定义的

看 task.h 文件，应该是在

#include "modules/planning/proto/planning_config.pb.h"

这里定义的，但是仔细找是没有这个文件的

因为这个 planning_config.pb.h 文件是通过

apollo/modules/planning/proto/planning_config.proto

这个 proto 文件生成的，具体怎么生成的，可以看这里： apollo .proto 文件使用

ok，那我们看一下 planning_config.proto，里面关于 TaskConfig 的定义

message TaskConfig {
    
    
  enum TaskType {
    
    
    DP_POLY_PATH_OPTIMIZER = 0;
    DP_ST_SPEED_OPTIMIZER = 1;
    QP_SPLINE_PATH_OPTIMIZER = 2;
	......
  };
  optional TaskType task_type = 1;
  oneof task_config {
    
    
    DpPolyPathConfig dp_poly_path_config = 2;
    DpStSpeedConfig dp_st_speed_config = 3;
	......
  }
}

可以看到实际上

TaskConfig 是由 TaskType + task_config 组成的，

即具体 task 的名称 + 该 task 的相关配置

举个栗子：

TaskConfig mission;
mission->set_TaskType(DP_POLY_PATH_OPTIMIZER);
mission->set_dp_poly_path_config(...)

b. Frame

TODO

先马着吧，我觉得我能看一天，有点多…

Frame类

包含了一次Planning计算循环中的所有信息。

Apollo Planning Frame

c. ReferenceLineInfo

TODO

也先马着吧 …

解析百度Apollo之决策规划模块

在Planning模块的每个计算循环中，会先生成ReferencePath，然后在这个基础上进行后面的处理。

例如：把障碍物投影到参考线上。

在ReferenceLine之外，在common目录下还有一个结构：ReferenceLineInfo，这个结构才是各个模块实际用到数据结构，它其中包含了ReferenceLine，但还有其他更详细的数据。

ReferenceLineInfo不仅仅包含了参考线信息，还包含了车辆状态，路径信息，速度信息，决策信息以及轨迹信息等。Planning模块的算法很多都是基于ReferenceLineInfo结构完成的

Smoother

为了保证车辆轨迹的平顺，参考线必须是经过平滑的，目前Apollo中包含了这么几个Smoother用来做参考线的平滑：

在这里插入图片描述

(2). PathOptimizer::Execute(…)

apollo::common::Status PathOptimizer::Execute(
    Frame* frame, ReferenceLineInfo* const reference_line_info) {
    
    
  Task::Execute(frame, reference_line_info);
  auto ret = Process(
      reference_line_info->speed_data(), reference_line_info->reference_line(),
      frame->PlanningStartPoint(), reference_line_info->mutable_path_data());
  RecordDebugInfo(reference_line_info->path_data());
  if (ret != Status::OK()) {
    
    
    reference_line_info->SetDrivable(false);
    AERROR << "Reference Line " << reference_line_info->Lanes().Id()
           << " is not drivable after " << Name();
  }
  return ret;
}

可以看到，实际上还是调用 Process 函数完成实际的优化

2. DpPolyPathOptimizer::Process(…)

Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
                                    const ReferenceLine &,
                                    const common::TrajectoryPoint &init_point,
                                    PathData *const path_data) {
    
    
  CHECK_NOTNULL(path_data);
  const auto &dp_poly_path_config = config_.dp_poly_path_config();
  DpRoadGraph dp_road_graph(dp_poly_path_config, *reference_line_info_,
                            speed_data);
  dp_road_graph.SetDebugLogger(reference_line_info_->mutable_debug());
  dp_road_graph.SetWaypointSampler(
      new WaypointSampler(dp_poly_path_config.waypoint_sampler_config()));

  if (!dp_road_graph.FindPathTunnel(
          init_point,
          reference_line_info_->path_decision()->obstacles().Items(),
          path_data)) {
    
    
    AERROR << "Failed to find tunnel in road graph";
    return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation");
  }

  return Status::OK();
}

写的很清楚了，输入：

SpeedData
ReferenceLine
TrajectoryPoint
PathData

读取 config 的配置，设置 dp path 的参数，然后具体的实现交给 DpRoadGraph 这个类

需要值得注意的点：

输入中的 SpeedData ，ReferenceLine， ReferenceLine，PathData 具体类型
DpRoadGraph 这个类

还是仔细分析一下：

(1). 输入数据类型

a. SpeedData

看一下定义

class SpeedData : public std::vector<common::SpeedPoint> {
    
    
 public:
  SpeedData() = default;

  explicit SpeedData(std::vector<common::SpeedPoint> speed_points);

  void AppendSpeedPoint(const double s, const double time, const double v,
                        const double a, const double da);

  bool EvaluateByTime(const double time,
                      common::SpeedPoint* const speed_point) const;

  double TotalTime() const;

  virtual std::string DebugString() const;
};

继承自 std::vector<common::SpeedPoint>

看一下 SpeedPoint

是包括在：

#include "modules/common/proto/pnc_point.pb.h"

这个头文件中的，定义在：

apollo/modules/common/proto/pnc_point.proto

message SpeedPoint {
    
    
  optional double s = 1;
  optional double t = 2;
  // speed (m/s)
  optional double v = 3;
  // acceleration (m/s^2)
  optional double a = 4;
  // jerk (m/s^3)
  optional double da = 5;
}

构造：传入 vector speed_points 初始化（会按照时间排序）

SpeedData::SpeedData(std::vector<SpeedPoint> speed_points)
    : std::vector<SpeedPoint>(std::move(speed_points)) {
    
    
  std::sort(begin(), end(), [](const SpeedPoint& p1, const SpeedPoint& p2) {
    
    
    return p1.t() < p2.t();
  });
}

EvaluateByTime(), 通过对应的时间 t，查找速度信息（线性插值）

b. ReferenceLine

见上

c. TrajectoryPoint

也是定义在

apollo/modules/common/proto/pnc_point.proto

message TrajectoryPoint {
    
    
  // path point
  optional PathPoint path_point = 1;
  // linear velocity
  optional double v = 2;  // in [m/s]
  // linear acceleration
  optional double a = 3;
  // relative time from beginning of the trajectory
  optional double relative_time = 4;
  // longitudinal jerk
  optional double da = 5;
  // The angle between vehicle front wheel and vehicle longitudinal axis
  optional double steer = 6;
}

message PathPoint {
    
    
  // coordinates
  optional double x = 1;
  optional double y = 2;
  optional double z = 3;

  // direction on the x-y plane
  optional double theta = 4;
  // curvature on the x-y planning
  optional double kappa = 5;
  // accumulated distance from beginning of the path
  optional double s = 6;

  // derivative of kappa w.r.t s.
  optional double dkappa = 7;
  // derivative of derivative of kappa w.r.t s.
  optional double ddkappa = 8;
  // The lane ID where the path point is on
  optional string lane_id = 9;

  // derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
  optional double x_derivative = 10;
  optional double y_derivative = 11;
}

d. PathData

在这里：

apollo/modules/planning/common/path/path_data.h

TODO

把这部分抽出来，详细说

(2). DpRoadGraph

在这里：

apollo/modules/planning/tasks/optimizers/road_graph/dp_road_graph.h

看一下：

class DpRoadGraph {
    
    
 public:
  explicit DpRoadGraph(const DpPolyPathConfig &config,
                       const ReferenceLineInfo &reference_line_info,
                       const SpeedData &speed_data);

  ~DpRoadGraph() = default;

  bool FindPathTunnel(const common::TrajectoryPoint &init_point,
                      const std::vector<const Obstacle *> &obstacles,
                      PathData *const path_data);
  ......
  
  void SetWaypointSampler(WaypointSampler *waypoint_sampler) {
    
    
    waypoint_sampler_.reset(waypoint_sampler);
  }

 private:
	......
 private:
  DpPolyPathConfig config_;
  common::TrajectoryPoint init_point_;
  const ReferenceLineInfo &reference_line_info_;
  const ReferenceLine &reference_line_;
  SpeedData speed_data_;
  common::SLPoint init_sl_point_;
  common::FrenetFramePoint init_frenet_frame_point_;
  apollo::planning_internal::Debug *planning_debug_ = nullptr;

  ObjectSidePass sidepass_;

  std::unique_ptr<WaypointSampler> waypoint_sampler_;
};

构造函数很简单，两个主要的函数 SetWaypointSampler 和 FindPathTunnel，分别实现采样和dp

采样主要由 WaypointSampler 这个类实现

a. WaypointSampler::SamplePathWaypoints(…)

class WaypointSampler {
    
    
 public:
  explicit WaypointSampler(const WaypointSamplerConfig &config)
      : config_(config) {
    
    }
  ~WaypointSampler() = default;

  virtual void Init(const ReferenceLineInfo *reference_line_info,
                    const common::SLPoint &init_sl_point_,
                    const common::FrenetFramePoint &init_frenet_frame_point);

  ......
  virtual bool SamplePathWaypoints(
      const common::TrajectoryPoint &init_point,
      std::vector<std::vector<common::SLPoint>> *const points);

 protected:
  const WaypointSamplerConfig &config_;
  const ReferenceLineInfo *reference_line_info_ = nullptr;
  common::SLPoint init_sl_point_;
  common::FrenetFramePoint init_frenet_frame_point_;
  apollo::planning_internal::Debug *planning_debug_ = nullptr;

  ObjectSidePass sidepass_;
};

仔细看一下 SamplePathWaypoints(…) 这个函数

函数的输入：

bool WaypointSampler::SamplePathWaypoints(
    const common::TrajectoryPoint &init_point,
    std::vector<std::vector<common::SLPoint>> *const points) {
    
    
    }

TrajectoryPoint 前面已经说过了
SLPoint

①. SLPoint

定义的路径：

apollo/modules/common/proto/pnc_point.proto

message SLPoint {
    
    
  optional double s = 1;
  optional double l = 2;
}

然后仔细看一下采样的函数

bool WaypointSampler::SamplePathWaypoints(
    const common::TrajectoryPoint &init_point,
    std::vector<std::vector<common::SLPoint>> *const points) {
    
    
  CHECK_NOTNULL(points);
  points->clear();
  points->insert(points->begin(), std::vector<common::SLPoint>{
    
    init_sl_point_});

  const double kMinSampleDistance = 40.0;
  const double total_length = std::fmin(
      init_sl_point_.s() + std::fmax(init_point.v() * 8.0, kMinSampleDistance),
      reference_line_info_->reference_line().Length());
  const auto &vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  const double half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
  // 每步采样点数
  const double num_sample_per_level =
      FLAGS_use_navigation_mode ? config_.navigator_sample_num_each_level()
                                : config_.sample_points_num_each_level();

  constexpr double kSamplePointLookForwardTime = 4.0;
  const double level_distance =
      common::math::Clamp(init_point.v() * kSamplePointLookForwardTime,
                          config_.step_length_min(), config_.step_length_max());

  double accumulated_s = init_sl_point_.s();
  double prev_s = accumulated_s;

  auto *status = PlanningContext::MutablePlanningStatus();
  if (!status->has_pull_over() && status->pull_over().in_pull_over()) {
    
    
    status->mutable_pull_over()->set_status(PullOverStatus::IN_OPERATION);
    const auto &start_point = status->pull_over().start_point();
    SLPoint start_point_sl;
    if (!reference_line_info_->reference_line().XYToSL(start_point,
                                                       &start_point_sl)) {
    
    
      AERROR << "Fail to change xy to sl.";
      return false;
    }

    if (init_sl_point_.s() > start_point_sl.s()) {
    
    
      const auto &stop_point = status->pull_over().stop_point();
      SLPoint stop_point_sl;
      if (!reference_line_info_->reference_line().XYToSL(stop_point,
                                                         &stop_point_sl)) {
    
    
        AERROR << "Fail to change xy to sl.";
        return false;
      }
      std::vector<common::SLPoint> level_points(1, stop_point_sl);
      points->emplace_back(level_points);
      return true;
    }
  }

  constexpr size_t kNumLevel = 3;
  for (size_t i = 0; i < kNumLevel && accumulated_s < total_length; ++i) {
    
    
    accumulated_s += level_distance;
    if (accumulated_s + level_distance / 2.0 > total_length) {
    
    
      accumulated_s = total_length;
    }
    const double s = std::fmin(accumulated_s, total_length);
    // 最小允许采样步长
    constexpr double kMinAllowedSampleStep = 1.0;
    // 若本次轨迹弧长与上次轨迹弧长间的差值小于最小允许采样步长，跳过本次采样
    if (std::fabs(s - prev_s) < kMinAllowedSampleStep) {
    
    
      continue;
    }
    prev_s = s;

    double left_width = 0.0;
    double right_width = 0.0;
    reference_line_info_->reference_line().GetLaneWidth(s, &left_width,
                                                        &right_width);

    // 边界缓冲
    constexpr double kBoundaryBuff = 0.20;
    // 右侧允许宽度 = 右车道宽 - 半车宽 - 边界缓冲
    const double eff_right_width = right_width - half_adc_width - kBoundaryBuff; 
    // 左侧允许宽度 = 左车道宽 - 半车宽 - 边界缓冲
    const double eff_left_width = left_width - half_adc_width - kBoundaryBuff;

    // the heuristic shift of L for lane change scenarios
    const double delta_dl = 1.2 / 20.0;
    const double kChangeLaneDeltaL = common::math::Clamp(
        level_distance * (std::fabs(init_frenet_frame_point_.dl()) + delta_dl),
        1.2, 3.5);

    // 每个位置上横向采样时，采样点之间的距离，差不多0.2m //
    double kDefaultUnitL = kChangeLaneDeltaL / (num_sample_per_level - 1);
    if (reference_line_info_->IsChangeLanePath() &&
        !reference_line_info_->IsSafeToChangeLane()) {
    
    
      kDefaultUnitL = 1.0;
    }

    // 横向采样的宽度
    const double sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
    // 右采样边界（车辆右侧为负值）
    double sample_right_boundary = -eff_right_width;
    // 左采样边界（车辆左侧为正值）
    double sample_left_boundary = eff_left_width;

    constexpr double kLargeDeviationL = 1.75;
    constexpr double kTwentyMilesPerHour = 8.94;

    // 参考线正在改变车道时
    if (reference_line_info_->IsChangeLanePath() ||
        std::fabs(init_sl_point_.l()) > kLargeDeviationL) {
    
    
      if (EgoInfo::Instance()->start_point().v() > kTwentyMilesPerHour) {
    
    
        sample_right_boundary = std::fmin(-eff_right_width, init_sl_point_.l());
        sample_left_boundary = std::fmax(eff_left_width, init_sl_point_.l());

        if (init_sl_point_.l() > eff_left_width) {
    
    
          sample_right_boundary = std::fmax(
              sample_right_boundary, init_sl_point_.l() - sample_l_range);
        }
        if (init_sl_point_.l() < eff_right_width) {
    
    
          sample_left_boundary = std::fmin(sample_left_boundary,
                                           init_sl_point_.l() + sample_l_range);
        }
      }
    }

    // 横向采样距离数组
    std::vector<double> sample_l;
    // 参考线正在改变车道且改变车道不安全时，将当前参考线到其他参考线的偏移值存储到横向采样距离数组
    if (reference_line_info_->IsChangeLanePath() &&
        !reference_line_info_->IsSafeToChangeLane()) {
    
    
      sample_l.push_back(reference_line_info_->OffsetToOtherReferenceLine());
    } else {
    
    
      // 其他情形，从右采样边界到左采样边界，按照每步采样点数进行均匀采样，并将结果存储到横向采样距离数组
      common::util::uniform_slice(
          sample_right_boundary, sample_left_boundary,
          static_cast<uint32_t>(num_sample_per_level - 1), &sample_l);
    }

    // 本次采样点数组
    std::vector<common::SLPoint> level_points;
    planning_internal::SampleLayerDebug sample_layer_debug;
    for (size_t j = 0; j < sample_l.size(); ++j) {
    
    
      common::SLPoint sl = common::util::MakeSLPoint(s, sample_l[j]);
      sample_layer_debug.add_sl_point()->CopyFrom(sl);
      level_points.push_back(std::move(sl));
    }
    if (!level_points.empty()) {
    
    
      planning_debug_->mutable_planning_data()
          ->mutable_dp_poly_graph()
          ->add_sample_layer()
          ->CopyFrom(sample_layer_debug);
      points->emplace_back(level_points);
    }
  }
  return true;
}

不得不说很冗长…

TODO 仔细分析

b. DpRoadGraph::FindPathTunnel(…) [detail]

①. TODO 仔细分析

FindPathTunnel() 主要分为3部分：

先设置相关前提条件，

然后查找代价最小路径，

最后对每段代价最小路径采样以构造FrenetFramePath类的实例，并存入path_data中。

//FindPathTunnel()的结果是依据若干level之间分段5次多项式的采样点，
//保存在path_data.frenet_path_（SL系）和path_data.discretized_path_（XY系）中
bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
                                 const std::vector<const Obstacle *> &obstacles,
                                 PathData *const path_data) {
    
    
  CHECK_NOTNULL(path_data);

  init_point_ = init_point;
  if (!reference_line_.XYToSL(
          {
    
    init_point_.path_point().x(), init_point_.path_point().y()},
          &init_sl_point_)) {
    
    
    AERROR << "Fail to create init_sl_point from : "
           << init_point.DebugString();
    return false;
  }
  init_frenet_frame_point_ =
      reference_line_.GetFrenetPoint(init_point_.path_point());

  waypoint_sampler_->Init(&reference_line_info_, init_sl_point_,
                          init_frenet_frame_point_);
  waypoint_sampler_->SetDebugLogger(planning_debug_);

  std::vector<DpRoadGraphNode> min_cost_path;
  if (!GenerateMinCostPath(obstacles, &min_cost_path)) {
    
    
    AERROR << "Fail to generate graph!";
    return false;
  }
  std::vector<common::FrenetFramePoint> frenet_path;
  double accumulated_s = min_cost_path.front().sl_point.s();
  const double path_resolution = config_.path_resolution();

  for (size_t i = 1; i < min_cost_path.size(); ++i) {
    
    
    const auto &prev_node = min_cost_path[i - 1];
    const auto &cur_node = min_cost_path[i];

    const double path_length = cur_node.sl_point.s() - prev_node.sl_point.s();
    double current_s = 0.0;
    const auto &curve = cur_node.min_cost_curve;
    while (current_s + path_resolution / 2.0 < path_length) {
    
    
      const double l = curve.Evaluate(0, current_s);
      const double dl = curve.Evaluate(1, current_s);
      const double ddl = curve.Evaluate(2, current_s);
      common::FrenetFramePoint frenet_frame_point;
      frenet_frame_point.set_s(accumulated_s + current_s);
      frenet_frame_point.set_l(l);
      frenet_frame_point.set_dl(dl);
      frenet_frame_point.set_ddl(ddl);
      frenet_path.push_back(std::move(frenet_frame_point));
      current_s += path_resolution;
    }
    if (i == min_cost_path.size() - 1) {
    
    
      accumulated_s += current_s;
    } else {
    
    
      accumulated_s += path_length;
    }
  }
  FrenetFramePath tunnel(frenet_path);
  path_data->SetReferenceLine(&reference_line_);
  path_data->SetFrenetPath(tunnel);
  return true;
}

查找代价最小路径的核心在于 GenerateMinCostPath()，分为3部分：

先采样，

然后构造graph，

最后查找从起点（自车当前位置）到终点（尽可能远的某个采样点）的代价最小路径。

②. DpRoadGraph::GenerateMinCostPath(…)

TODO 仔细分析

bool DpRoadGraph::GenerateMinCostPath(
    const std::vector<const Obstacle *> &obstacles,
    std::vector<DpRoadGraphNode> *min_cost_path) {
    
    
  CHECK(min_cost_path != nullptr);

  std::vector<std::vector<common::SLPoint>> path_waypoints;
  if (!waypoint_sampler_->SamplePathWaypoints(init_point_, &path_waypoints) ||
      path_waypoints.size() < 1) {
    
    
    AERROR << "Fail to sample path waypoints! reference_line_length = "
           << reference_line_.Length();
    return false;
  }
  const auto &vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();

  TrajectoryCost trajectory_cost(
      config_, reference_line_, reference_line_info_.IsChangeLanePath(),
      obstacles, vehicle_config.vehicle_param(), speed_data_, init_sl_point_,
      reference_line_info_.AdcSlBoundary());

  std::list<std::list<DpRoadGraphNode>> graph_nodes;

  // find one point from first row
  const auto &first_row = path_waypoints.front();
  size_t nearest_i = 0;
  for (size_t i = 1; i < first_row.size(); ++i) {
    
    
    if (std::fabs(first_row[i].l() - init_sl_point_.l()) <
        std::fabs(first_row[nearest_i].l() - init_sl_point_.l())) {
    
    
      nearest_i = i;
    }
  }
  graph_nodes.emplace_back();
  graph_nodes.back().emplace_back(first_row[nearest_i], nullptr,
                                  ComparableCost());
  auto &front = graph_nodes.front().front();
  size_t total_level = path_waypoints.size();

  for (size_t level = 1; level < path_waypoints.size(); ++level) {
    
    
    const auto &prev_dp_nodes = graph_nodes.back();
    const auto &level_points = path_waypoints[level];

    graph_nodes.emplace_back();
    std::vector<std::future<void>> results;

    for (size_t i = 0; i < level_points.size(); ++i) {
    
    
      const auto &cur_point = level_points[i];

      graph_nodes.back().emplace_back(cur_point, nullptr);

      auto msg = std::make_shared<RoadGraphMessage>(
          prev_dp_nodes, level, total_level, &trajectory_cost, &front,
          &(graph_nodes.back().back()));

      if (FLAGS_enable_multi_thread_in_dp_poly_path) {
    
    
        results.emplace_back(cyber::Async(&DpRoadGraph::UpdateNode, this, msg));
      } else {
    
    
        UpdateNode(msg);
      }
    }
    if (FLAGS_enable_multi_thread_in_dp_poly_path) {
    
    
      for (auto &result : results) {
    
    
        result.get();
      }
    }
  }

  // find best path
  DpRoadGraphNode fake_head;
  for (const auto &cur_dp_node : graph_nodes.back()) {
    
    
    fake_head.UpdateCost(&cur_dp_node, cur_dp_node.min_cost_curve,
                         cur_dp_node.min_cost);
  }

  const auto *min_cost_node = &fake_head;
  while (min_cost_node->min_cost_prev_node) {
    
    
    min_cost_node = min_cost_node->min_cost_prev_node;
    min_cost_path->push_back(*min_cost_node);
  }
  if (min_cost_node != &graph_nodes.front().front()) {
    
    
    return false;
  }

  std::reverse(min_cost_path->begin(), min_cost_path->end());

  for (const auto &node : *min_cost_path) {
    
    
    ADEBUG << "min_cost_path: " << node.sl_point.ShortDebugString();
    planning_debug_->mutable_planning_data()
        ->mutable_dp_poly_graph()
        ->add_min_cost_point()
        ->CopyFrom(node.sl_point);
  }
  return true;
}

③. trajectory_cost(…)

TODO仔细分析

TrajectoryCost trajectory_cost(
     config_, reference_line_, reference_line_info_.IsChangeLanePath(),
     obstacles, vehicle_config.vehicle_param(), speed_data_, init_sl_point_,
     reference_line_info_.AdcSlBoundary());

计算 node 间的 cost 主要在这里

class TrajectoryCost {
    
    
 public:
  TrajectoryCost() = default;
  explicit TrajectoryCost(const DpPolyPathConfig &config,
                          const ReferenceLine &reference_line,
                          const bool is_change_lane_path,
                          const std::vector<const Obstacle *> &obstacles,
                          const common::VehicleParam &vehicle_param,
                          const SpeedData &heuristic_speed_data,
                          const common::SLPoint &init_sl_point,
                          const SLBoundary &adc_sl_boundary);
  ComparableCost Calculate(const QuinticPolynomialCurve1d &curve,
                           const double start_s, const double end_s,
                           const uint32_t curr_level,
                           const uint32_t total_level);

 private:
	......
};

Calculate 中

ComparableCost TrajectoryCost::Calculate(const QuinticPolynomialCurve1d &curve,
                                         const double start_s,
                                         const double end_s,
                                         const uint32_t curr_level,
                                         const uint32_t total_level) {
    
    
  ComparableCost total_cost;
  // path cost
  total_cost +=
      CalculatePathCost(curve, start_s, end_s, curr_level, total_level);

  // static obstacle cost
  total_cost += CalculateStaticObstacleCost(curve, start_s, end_s);

  // dynamic obstacle cost
  total_cost += CalculateDynamicObstacleCost(curve, start_s, end_s);
  return total_cost;
}

apollo planning dp path 源码阅读

文章目录

一. dp_poly_path_optimizer

1. PathOptimizer 类

(1). Task 类

a. TaskConfig

b. Frame

TODO

c. ReferenceLineInfo

TODO

(2). PathOptimizer::Execute(…)

2. DpPolyPathOptimizer::Process(…)

(1). 输入数据类型

a. SpeedData

b. ReferenceLine

c. TrajectoryPoint

d. PathData

TODO

(2). DpRoadGraph

a. WaypointSampler::SamplePathWaypoints(…)

①. SLPoint

TODO 仔细分析

b. DpRoadGraph::FindPathTunnel(…) [detail]

①. TODO 仔细分析

②. DpRoadGraph::GenerateMinCostPath(…)

TODO 仔细分析

③. trajectory_cost(…)

TODO仔细分析

猜你喜欢