Apollo9.0 Lattice Planner算法源码学习_apollo lattice planner

一、概述

主文件：lattice_planner.cc，相关路径如下：

modules\planning\planners\lattice\lattice_planner.cc
modules\planning\planners\lattice\lattice_planner.h

 主程序函数：

Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point,
                            Frame* frame,
                            ADCTrajectory* ptr_computed_trajectory) {}

二、主程序学习(详细注释) 

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
/**
 * @file
 **/
 
#include "modules/planning/planners/lattice/lattice_planner.h"
 
#include <limits>
#include <memory>
#include <utility>
#include <vector>
 
#include "cyber/common/log.h"
#include "cyber/common/macros.h"
#include "cyber/time/clock.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/math/path_matcher.h"
#include "modules/planning/planners/lattice/behavior/collision_checker.h"
#include "modules/planning/planners/lattice/behavior/path_time_graph.h"
#include "modules/planning/planners/lattice/behavior/prediction_querier.h"
#include "modules/planning/planners/lattice/trajectory_generation/backup_trajectory_generator.h"
#include "modules/planning/planners/lattice/trajectory_generation/lattice_trajectory1d.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory_combiner.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory_evaluator.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/planning_base/math/constraint_checker/constraint_checker.h"
 
namespace apollo {
namespace planning {
 
using apollo::common::ErrorCode;
using apollo::common::PathPoint;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::CartesianFrenetConverter;
using apollo::common::math::PathMatcher;
using apollo::cyber::Clock;
 
namespace {
 
//---------该函数将输入的参考线进行离散化---------//
//---------输入：为ReferencePoint类型的vector----//
//---------输出：为PathPoint类型的vector---------//
std::vector<PathPoint> ToDiscretizedReferenceLine(
    const std::vector<ReferencePoint>& ref_points) {
  double s = 0.0;
  std::vector<PathPoint> path_points;
  for (const auto& ref_point : ref_points) {
    PathPoint path_point;    //-----Pathpoint相当于定义的一种结构体,包含了很多路径点信息
    path_point.set_x(ref_point.x());
    path_point.set_y(ref_point.y());
    path_point.set_theta(ref_point.heading());
    path_point.set_kappa(ref_point.kappa());
    path_point.set_dkappa(ref_point.dkappa());
 
    if (!path_points.empty()) {
      double dx = path_point.x() - path_points.back().x();
      double dy = path_point.y() - path_points.back().y();
      s += std::sqrt(dx * dx + dy * dy);    //-----s的计算方式：以直代曲！
    }
    path_point.set_s(s);
    path_points.push_back(std::move(path_point));
  }
  return path_points;
}
 
//---------该函数将计算规划起点的Frenet坐标（状态）---------//
//---------其中调用了Cartesian 转 Frenet坐标的函数---------//
void ComputeInitFrenetState(const PathPoint& matched_point,
                            const TrajectoryPoint& cartesian_state,
                            std::array<double, 3>* ptr_s,
                            std::array<double, 3>* ptr_d) {
  CartesianFrenetConverter::cartesian_to_frenet(
      matched_point.s(), matched_point.x(), matched_point.y(),
      matched_point.theta(), matched_point.kappa(), matched_point.dkappa(),
      cartesian_state.path_point().x(), cartesian_state.path_point().y(),
      cartesian_state.v(), cartesian_state.a(),
      cartesian_state.path_point().theta(),
      cartesian_state.path_point().kappa(), ptr_s, ptr_d);
}
 
}  // namespace
 
/*-----------------------------------------------
以下的planner函数就是规划主函数
输入为：planning_start_point 规划起点；frame 一次规划所需要的所有环境信息
ptr_computed_trajectory 待规划的轨迹？？
-----------------------------------------------*/
Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point,
                            Frame* frame,
                            ADCTrajectory* ptr_computed_trajectory) {
  size_t success_line_count = 0;
  size_t index = 0;
/*-----------------------------------------------
由于一个规划帧开始之前有定位和导航routing模块都会得到若干
条参考线，因此规划开始之前要根据给定的cost计算每条参考
线的cost，然后选择其中cost最低的一条进行离散化。SetPriorityCost
-----------------------------------------------*/
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    if (index != 0) {
      reference_line_info.SetPriorityCost(
          FLAGS_cost_non_priority_reference_line);
    } else {
      reference_line_info.SetPriorityCost(0.0);
    }
    /*-----------------------------------------------
    PlanOnReferenceLine()该函数为Lattice算法中最重要的函数，
    即：在选定cost最优的参考线之后，在该参考线上进行规划！
    -----------------------------------------------*/
    auto status =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
 
    if (status != Status::OK()) {
      if (reference_line_info.IsChangeLanePath()) {
        AERROR << "Planner failed to change lane to "
               << reference_line_info.Lanes().Id();
      } else {
        AERROR << "Planner failed to " << reference_line_info.Lanes().Id();
      }
    } else {
      success_line_count += 1;
    }
    ++index;
  }
 
  if (success_line_count > 0) {
    return Status::OK();
  }
  return Status(ErrorCode::PLANNING_ERROR,
                "Failed to plan on any reference line.");
}
 
/*-----------------------------------------------
对每一条参考线都会执行以下规划（？）
以下为PlanOnReferenceLine()的具体实现，分为7个步骤：
1、离散化参考线上的点，并计算s的值（目的：以直代曲，
为了在进行坐标转化以及计算障碍物距离自车的s坐标的时
候可以使用，如制作index2s表格，即根据参考线上点的索
引号映射到s值的表）
2、在参考线上计算“规划起点”的匹配点
3、根据匹配点，计算Frenet坐标系的S-L值
4、计算障碍物的S-T图（斜率表示速度）
5、生成纵横向采样路径
6、计算cost值，进行碰撞检测（依据S-T图）和动力学约束检测
7、优选出cost值最小的trajectory
-----------------------------------------------*/
Status LatticePlanner::PlanOnReferenceLine(
    const TrajectoryPoint& planning_init_point, Frame* frame,
    ReferenceLineInfo* reference_line_info) {
  static size_t num_planning_cycles = 0;
  static size_t num_planning_succeeded_cycles = 0;
 
  double start_time = Clock::NowInSeconds();
  double current_time = start_time;
 
  ADEBUG << "Number of planning cycles: " << num_planning_cycles << " "
         << num_planning_succeeded_cycles;
  ++num_planning_cycles;
 
  reference_line_info->set_is_on_reference_line();
 
  // 1. obtain a reference line and transform it to the PathPoint format.
  // 以下为参考线离散化的过程：
  auto ptr_reference_line =
      std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(
          reference_line_info->reference_line().reference_points()));
 
  // 2. compute the matched point of the init planning point on the reference
  // line.
  // 以下将计算规划起点的匹配点
  // 该函数的实现是在 path_matcher.cc 文件里面
  PathPoint matched_point = PathMatcher::MatchToPath(
      *ptr_reference_line, planning_init_point.path_point().x(),
      planning_init_point.path_point().y());
 
  // 3. according to the matched point, compute the init state in Frenet frame.
  std::array<double, 3> init_s;
  s