百度apollo自动驾驶planning代码学习-Apollo planning/common/path/PathData类代码详解

 1.类总览

PathData这个类主要是将路径path及其相关的一些操作处理抽象成一个类，其包含path的XY坐标形式，SL坐标形式，这些pathpoint路径点形式间的相互转化，路径path的裁剪，以及根据指定s去路径path上插值出曲率曲率变化率航向角等信息，以及设定阻塞该路径path的静态障碍物id等，这个类主要是完成这些功能。

注:

path仅指位置，仅包含x,y,s,L,kappa,dkappa,heading信息

trajectory = path + speed（path加上速度规划）

 2.path_data.h代码详解

//PathData类的声明头文件path_data.h
#pragma once

#include <list>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

//需要用到离散路径类，Frenet路径类以及参考线类数据结构。
#include "modules/planning/common/path/discretized_path.h"
#include "modules/planning/common/path/frenet_frame_path.h"
#include "modules/planning/reference_line/reference_line.h"

namespace apollo {
namespace planning {

class PathData {
 public:
  //枚举类型路径点类型PathPointType
  //路径点类型包括：在车道/前进离开本车道？/倒车离开本车道？/脱离道路/未知类型
  enum class PathPointType {
    IN_LANE,
    OUT_ON_FORWARD_LANE,
    OUT_ON_REVERSE_LANE,
    OFF_ROAD,
    UNKNOWN,
  };

  //PathData类的默认构造函数
  PathData() = default;

  //成员函数设置离散路径，参数就是离散路径类对象
  bool SetDiscretizedPath(DiscretizedPath path);

  //成员函数设置Frenet系路径，参数就是Frenet系路径类对象
  bool SetFrenetPath(FrenetFramePath frenet_path);

  //成员函数设置参考线，参数就是参考线类对象
  void SetReferenceLine(const ReferenceLine *reference_line);

  //设置路径点决策引导，参数就为路径点决策引导类vector容器对象
  //显然path_point_decision_guide就是路径点搭配上路径点类型构成
  bool SetPathPointDecisionGuide(
      std::vector<std::tuple<double, PathPointType, double>>
          path_point_decision_guide);

  //数据成员离散路径discretized_path
  const DiscretizedPath &discretized_path() const;

  //数据成员Frenet系路径 frenet_frame_path
  const FrenetFramePath &frenet_frame_path() const;

  //数据成员路径点类型类vector，路径点决策引导path_point_decision_guide
  const std::vector<std::tuple<double, PathPointType, double>>
      &path_point_decision_guide() const;

  //用纵向位置s去获取路径点？
  common::PathPoint GetPathPointWithPathS(const double s) const;

  /*
   * 这个函数会在数据成员离散路径discretized_path上寻找这样一个路径点
   * 这个路径点在参考线上的投影接近于ref_s，参考纵向位置？
   */
  bool GetPathPointWithRefS(const double ref_s,
                            common::PathPoint *const path_point) const;

  //通过阅读该函数发现该函数的作用就是根据给定的Frenet系的点去裁减
  //数据成员Frenet路径frenet_path_,将给定的Frenet系的点frenet_point左边的轨迹
  //也就是frenet路径上所有小于frenet_point对应的s的点全部裁减掉(trim裁减)作为
  //新的frenet_path_ 作为新的Frenet系路径
  bool LeftTrimWithRefS(const common::FrenetFramePoint &frenet_point);

  //用给定的参考线去赋值给PathData类的数据成员reference_line_，同时还设置离散路径点？
  bool UpdateFrenetFramePath(const ReferenceLine *reference_line);

  //PathData的成员函数Clear用于清空PathData里的几个vector数据成员和指针数据成员
  //vector的清空直接.clear()，指针的清空 = nullptr
  void Clear();

  //判断PathData类数据成员里离散路径和Frenet系路径vector是否同时为空？
  bool Empty() const;

  //将离散路径点的起点开始不超过10个点的信息转成字符串返回用于debug?
  std::string DebugString() const;

  //用给定标签去初始化数据成员path_label_路径标签
  void set_path_label(const std::string &label);

  //返回数据成员路径标签path_label_的函数
  const std::string &path_label() const;

  //用给定障碍物id去初始化数据成员blocking_obstacle_id_阻塞障碍物id
  void set_blocking_obstacle_id(const std::string &obs_id) {
    blocking_obstacle_id_ = obs_id;
  }
  //返回数据成员阻塞障碍物id blocking_obstacle_id_
  const std::string &blocking_obstacle_id() const {
    return blocking_obstacle_id_;
  }

  //返回数据成员 是否为有效的路径参考标志位is_valid_path_reference_？
  const bool is_valid_path_reference() const {
    return is_valid_path_reference_;
  }
  //设置数据成员——是否为有效的路径参考标志位is_valid_path_reference_的值
  void set_is_valid_path_reference(bool is_valid_path_reference) {
    is_valid_path_reference_ = is_valid_path_reference;
  }

  //返回数据成员——是否朝着参考轨迹优化标志位is_optimized_towards_trajectory_reference_
  const bool is_optimized_towards_trajectory_reference() const {
    return is_optimized_towards_trajectory_reference_;
  }
  //设置数据成员 是否朝着参考轨迹优化标志位is_optimized_towards_trajectory_reference_的值
  void set_is_optimized_towards_trajectory_reference(
      bool is_optimized_towards_trajectory_reference) {
    is_optimized_towards_trajectory_reference_ =
        is_optimized_towards_trajectory_reference;
  }

  //path_reference()返回数据成员path_reference_参考路径
  //参考路径path_reference_是存放PathPoint路径点序列的vector容器
  const std::vector<common::PathPoint> &path_reference() const;
  //设置数据成员参考路径path_reference_
  void set_path_reference(const std::vector<common::PathPoint> &path_reference);

 private:
  /*
   * 转化frenet路径到笛卡尔坐标系路径，通过道路参考线(SL坐标转化为xy坐标)
   */
  //转换结果存放到参数discretized_path，调用时通过引用变量将函数结果传出
  bool SLToXY(const FrenetFramePath &frenet_path,
              DiscretizedPath *const discretized_path);

  //将笛卡尔坐标系路径转化到frenet路径存放到frenet_path，调用时通过引用变量将函数结果传出
  bool XYToSL(const DiscretizedPath &discretized_path,
              FrenetFramePath *const frenet_path);
  //声明并定义数据成员参考线类型指针对象reference_line_初始化为空指针
  const ReferenceLine *reference_line_ = nullptr;
  //声明PathData类数据成员——离散路径类对象discretized_path_
  DiscretizedPath discretized_path_;
  //声明PathData类数据成员——Frenet系路径类对象frenet_path_
  FrenetFramePath frenet_path_;
  /**
   * 速度决策被路径分析器产生，因为参考速度限制在速度边界决策中产生
   * 模板类型tuple由参考线上S轴位置，路径点类型，到最近的障碍物距离构成
   */
  //首先看std::tuple<double, PathPointType, double>定义了一个数据类型
  //这个数据类型里每个数据由double,PathPointType,double 3个变量构成对应上述的3个量
  //然后又创建了一个vector，是由一系列这个tuple构成
  //这个vector容器就是path_point_decision_guide_路径点决策参考，也是PathData类数据成员
  std::vector<std::tuple<double, PathPointType, double>>
      path_point_decision_guide_;

  //声明并初始化PathData类的数据成员路径标签path_label_为空字符串
  std::string path_label_ = "";
  //声明PathData类的数据成员阻塞障碍物id blocking_obstacle_id_
  std::string blocking_obstacle_id_;

  /**
   * 使用学习模型(机器学习)输出参考路径的参数
   *
   */
  // 是否PathData是一条参考路径作为后续模块的优化目标
  // 声明类数据成员是否是一条有效的参考路径is_valid_path_reference_为false
  bool is_valid_path_reference_ = false; 

  /**
   * 给定轨迹参考，该PathData是否已根据trajectory的“path”部分优化，
   * 使trajectory的“speed”部分可以在以后的模块中相应地使用
   * 补充说明:trajectory包含path和speed
   * path指单纯的路径，trajectory是带有速度等信息的path
   */
  //声明类数据成员 是否朝着参考轨迹优化？
  bool is_optimized_towards_trajectory_reference_ = false;

  //声明类数据成员参考路径path_reference_
  //参考路径是由一系列路径点PathPoint构成的vector容器
  std::vector<common::PathPoint> path_reference_;
};

}  // namespace planning
}  // namespace apollo

3.path_data.cc 代码详解

//主要是PathData类的实现
/**
 * @file path_data.cc
 **/

#include "modules/planning/common/path/path_data.h"

#include <algorithm>

#include "absl/strings/str_cat.h"
#include "absl/strings/str_join.h"
#include "cyber/common/log.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/util/point_factory.h"
#include "modules/common/util/string_util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::PathPoint;
using apollo::common::PointENU;
using apollo::common::SLPoint;
using apollo::common::math::CartesianFrenetConverter;
using apollo::common::util::PointFactory;

//设置离散路径，将参数赋给数据成员discretized_path_，同时调用XYToSL
//将XYToSL函数的结果放到PathData类数据成员frenet_path_里
bool PathData::SetDiscretizedPath(DiscretizedPath path) {
  if (reference_line_ == nullptr) {
    AERROR << "Should NOT set discretized path when reference line is nullptr. "
              "Please set reference line first.";
    return false;
  }
  discretized_path_ = std::move(path);
  if (!XYToSL(discretized_path_, &frenet_path_)) {
    AERROR << "Fail to transfer discretized path to frenet path.";
    return false;
  }
  DCHECK_EQ(discretized_path_.size(), frenet_path_.size());
  return true;
}

//设置Frenet系路径 将参数frenet_path赋值给数据成员frenet_path_
//同时调用SLToXY，并将转化的XY坐标系路径放入类数据成员discretized_path_
bool PathData::SetFrenetPath(FrenetFramePath frenet_path) {
  if (reference_line_ == nullptr) {
    AERROR << "Should NOT set frenet path when reference line is nullptr. "
              "Please set reference line first.";
    return false;
  }
  frenet_path_ = std::move(frenet_path);
  if (!SLToXY(frenet_path_, &discretized_path_)) {
    AERROR << "Fail to transfer frenet path to discretized path.";
    return false;
  }
  DCHECK_EQ(discretized_path_.size(), frenet_path_.size());
  return true;
}

//设置路径点决策参考，将输入参数path_point_decision_guide传给
//数据成员path_point_decision_guide_
bool PathData::SetPathPointDecisionGuide(
    std::vector<std::tuple<double, PathPointType, double>>
        path_point_decision_guide) {
  //若发现数据成员道路参考线为空，报错
  if (reference_line_ == nullptr) {
    AERROR << "Should NOT set path_point_decision_guide when reference line is "
              "nullptr. ";
    return false;
  }
  //发现数据成员frenet_path_和discretized_path_只要有一个为空就报错
  //检查frenet路径或xy路径是否为空
  if (frenet_path_.empty() || discretized_path_.empty()) {
    AERROR << "Should NOT set path_point_decision_guide when frenet_path or "
              "world frame trajectory is empty. ";
    return false;
  }
  path_point_decision_guide_ = std::move(path_point_decision_guide);
  return true;
}

//返回PathData类的数据成员——离散路径discretized_path_
const DiscretizedPath &PathData::discretized_path() const {
  return discretized_path_;
}

//返回PathData类的数据成员——frenet系路径frenet_path_
const FrenetFramePath &PathData::frenet_frame_path() const {
  return frenet_path_;
}

//返回PathData类的数据成员——路径点决策参考path_point_decision_guide_
const std::vector<std::tuple<double, PathData::PathPointType, double>>
    &PathData::path_point_decision_guide() const {
  return path_point_decision_guide_;
}

//检查PathData路径类数据是否为空
//就是检查xy路径对象vector discretized_path_和SL路径对象frenet_path_ vector
//是否都不为空
bool PathData::Empty() const {
  return discretized_path_.empty() && frenet_path_.empty();
}

//设置参考线，先清空之前的PathData类对象的数据，然后将输入参数reference_line赋值给
//数据成员reference_line_道路参考线
void PathData::SetReferenceLine(const ReferenceLine *reference_line) {
  Clear();
  reference_line_ = reference_line;
}

//去类数据成员discretized_path_离散路径上寻找给定纵向位置s对应的插值点
common::PathPoint PathData::GetPathPointWithPathS(const double s) const {
  return discretized_path_.Evaluate(s);
}

//获取路径点用参考纵向位置ref_s? 函数结果放入指针path_point中
//其实这个函数的作用就是给定纵向位置ref_s去frenet系上找到最近点，然后根据最近点索引
//去离散路径上discretized_path_求出对应的离散路径点，貌似功能有点重复？
bool PathData::GetPathPointWithRefS(const double ref_s,
                                    common::PathPoint *const path_point) const {
  ACHECK(reference_line_);
  //判断是否相等，离散路径discretized_path_点的个数和frenet系路径点的个数是否相等
  DCHECK_EQ(discretized_path_.size(), frenet_path_.size());
  //如果给定的纵向位置参数ref_s是小于0的则报错
  if (ref_s < 0) {
    AERROR << "ref_s[" << ref_s << "] should be > 0";
    return false;
  }
  //如果给定纵向位置ref_s是大于frenet系路径最后一个点的纵向位置s的，也报错
  if (ref_s > frenet_path_.back().s()) {
    AERROR << "ref_s is larger than the length of frenet_path_ length ["
           << frenet_path_.back().s() << "].";
    return false;
  }
  //索引什么索引？
  uint32_t index = 0;
  //定义一个小正数，是精度误差容许项？
  const double kDistanceEpsilon = 1e-3;
  //遍历frenet系路径点
  for (uint32_t i = 0; i + 1 < frenet_path_.size(); ++i) {
    //实际上frenet_path_和discretized_path_都是继承路径点或frenet系路径点vector类数据结构
    //对于vector序列容器，.at(0)表示获取vector的第一个元素，相比[]会自动检查索引是否越界
    //如果参考点s距离frenet系路径的第i个点的s间的距离小于小正数kDistanceEpsilon
    //那么路径点就从离散路径的第i个进行拷贝并直接返回
    if (fabs(ref_s - frenet_path_.at(i).s()) < kDistanceEpsilon) {
      path_point->CopyFrom(discretized_path_.at(i));
      return true;
    }
    //如果frenet系路径的第i个点的纵向位置s < 给定参数参考纵向位置ref_s
    //就是ref_s大于第i个frenet点s但是又小于第i+1个frenet点的s，介于第i个和第i+1个点之间
    if (frenet_path_.at(i).s() < ref_s && ref_s <= frenet_path_.at(i + 1).s()) {
      //当ref_s介于第i个点和第i+1个点之间，那么取index为第i个点
      index = i;
      //跳出循环
      break;
    }
  }
  //由上面可知index就是当ref_s位于frenet系路径第i,i+1个点的路径之间，index取i
  //即index为ref_s在frenet系路径中的纵向最近点索引
  //定义r为给定(纵向位置ref_s减去frenet系上最近点s)/(第i+1,i个点之间的纵向距离)
  //其实r就是ref_s在i,i+1之间的距离比例，如r=0.5，则ref_s对应第i,i+1点的中点
  double r = (ref_s - frenet_path_.at(index).s()) /
             (frenet_path_.at(index + 1).s() - frenet_path_.at(index).s());

  //离散路径的纵向位置discretized_path_s 等于离散路径discretized_path_上
  //最近点s + r * 第i,i+1点纵向距离
  const double discretized_path_s = discretized_path_.at(index).s() +
                                    r * (discretized_path_.at(index + 1).s() -
                                         discretized_path_.at(index).s());
  //用离散路径点s去插值出离散路径点，再拷贝给指针path_point，传出
  path_point->CopyFrom(discretized_path_.Evaluate(discretized_path_s));

  return true;
}

//路径点的清空函数
//清空类数据成员vector discretized_path_,frenet_path_,path_point_decision_guide_
//清空类数据成员path_reference_,reference_line_ 
void PathData::Clear() {
  discretized_path_.clear();
  frenet_path_.clear();
  path_point_decision_guide_.clear();
  path_reference_.clear();
  reference_line_ = nullptr;
}

//路径点类的数据成员DebugString
std::string PathData::DebugString() const {
  const auto limit =
      std::min(discretized_path_.size(), //离散路径点个数
                //FLAGS_trajectory_point_num_for_debug gflags用法
                //去除gflag文件中trajectory_point_num_for_debug的值，为10
               static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));

  return absl::StrCat(
      "[\n",
      //记录第一个点和至少第10个点之后的点信息转换为字符串输出用于debug
      absl::StrJoin(discretized_path_.begin(),
                    discretized_path_.begin() + limit, ",\n",
                    apollo::common::util::DebugStringFormatter()),
      "]\n");
}

///*
   * 转化frenet路径到笛卡尔坐标系路径，通过道路参考线(SL坐标转化为xy坐标)
   */
  //转换结果存放到参数discretized_path，调用时通过引用变量将函数结果传出
bool PathData::SLToXY(const FrenetFramePath &frenet_path,
                      DiscretizedPath *const discretized_path) {
  std::vector<common::PathPoint> path_points;
  for (const common::FrenetFramePoint &frenet_point : frenet_path) {
    const common::SLPoint sl_point =
        PointFactory::ToSLPoint(frenet_point.s(), frenet_point.l());
    common::math::Vec2d cartesian_point;
    if (!reference_line_->SLToXY(sl_point, &cartesian_point)) {
      AERROR << "Fail to convert sl point to xy point";
      return false;
    }
    const ReferencePoint ref_point =
        reference_line_->GetReferencePoint(frenet_point.s());
    const double theta = CartesianFrenetConverter::CalculateTheta(
        ref_point.heading(), ref_point.kappa(), frenet_point.l(),
        frenet_point.dl());
    ADEBUG << "frenet_point: " << frenet_point.ShortDebugString();
    const double kappa = CartesianFrenetConverter::CalculateKappa(
        ref_point.kappa(), ref_point.dkappa(), frenet_point.l(),
        frenet_point.dl(), frenet_point.ddl());

    double s = 0.0;
    double dkappa = 0.0;
    if (!path_points.empty()) {
      common::math::Vec2d last = PointFactory::ToVec2d(path_points.back());
      const double distance = (last - cartesian_point).Length();
      s = path_points.back().s() + distance;
      dkappa = (kappa - path_points.back().kappa()) / distance;
    }
    path_points.push_back(PointFactory::ToPathPoint(cartesian_point.x(),
                                                    cartesian_point.y(), 0.0, s,
                                                    theta, kappa, dkappa));
  }
  *discretized_path = DiscretizedPath(std::move(path_points));

  return true;
}

//将笛卡尔坐标系路径转化到frenet路径存放到frenet_path，调用时通过引用变量将函数结果传出
bool PathData::XYToSL(const DiscretizedPath &discretized_path,
                      FrenetFramePath *const frenet_path) {
  ACHECK(reference_line_);
  std::vector<common::FrenetFramePoint> frenet_frame_points;
  const double max_len = reference_line_->Length();
  for (const auto &path_point : discretized_path) {
    common::FrenetFramePoint frenet_point =
        reference_line_->GetFrenetPoint(path_point);
    if (!frenet_point.has_s()) {
      SLPoint sl_point;
      if (!reference_line_->XYToSL(path_point, &sl_point)) {
        AERROR << "Fail to transfer cartesian point to frenet point.";
        return false;
      }
      common::FrenetFramePoint frenet_point;
      // NOTICE: does not set dl and ddl here. Add if needed.
      frenet_point.set_s(std::max(0.0, std::min(sl_point.s(), max_len)));
      frenet_point.set_l(sl_point.l());
      frenet_frame_points.push_back(std::move(frenet_point));
      continue;
    }
    frenet_point.set_s(std::max(0.0, std::min(frenet_point.s(), max_len)));
    frenet_frame_points.push_back(std::move(frenet_point));
  }
  *frenet_path = FrenetFramePath(std::move(frenet_frame_points));
  return true;
}

//裁剪掉frenet路径上frenet_point点左边的路径,trim 修剪得意思
bool PathData::LeftTrimWithRefS(const common::FrenetFramePoint &frenet_point) {
  ACHECK(reference_line_);
  //frenet系坐标点FrenetFramePoint类的vector对象frenet_frame_points
  std::vector<common::FrenetFramePoint> frenet_frame_points;
  //frenet_frame_points首先把给定的frenet系路径点frenet_point塞到末尾
  frenet_frame_points.emplace_back(frenet_point);

  //遍历frenet_path_离散路径上的每一个frenet点,如果遍历点的s在给定点附近就跳过
  //感觉这个if有点多余
  for (const common::FrenetFramePoint fp : frenet_path_) {
    if (std::fabs(fp.s() - frenet_point.s()) < 1e-6) {
      continue;
    }
    //如果遍历点的s大于给定点的s的点则塞到frenet_frame_points里
    //这样遍历完后就在只剩下比给定点s大的点
    if (fp.s() > frenet_point.s()) {
      frenet_frame_points.push_back(fp);
    }
  }
  //最后遍历完后，将frenet_frame_points直接move到FrenetFramePath对象里，然后
  //将这个被move的FrenetFramePath对象作为参数调用SetFrenetPath，将裁剪后的frenet轨迹
  //放入PathData类数据成员对象frenet_path_ frenet系路径里
  SetFrenetPath(FrenetFramePath(std::move(frenet_frame_points)));
  return true;
}

//更新frenet系路径，用给定的参考线reference_line赋值给PathData类成员参考线reference_line_ 
//然后再用调用SetDiscretizedPath，里面会调用XYToSL()函数，从而更新frenet路径frenet_path_ 
bool PathData::UpdateFrenetFramePath(const ReferenceLine *reference_line) {
  reference_line_ = reference_line;
  return SetDiscretizedPath(discretized_path_);
}

//用给定的字符传设定PathData类数据成员———路径标签path_label_ 
void PathData::set_path_label(const std::string &label) { path_label_ = label; }

//获取Pathdata 路径数据类对象的路径标签对象path_label_
const std::string &PathData::path_label() const { return path_label_; }

//获取参考路径：PathPoint类数组vector对象path_reference_
const std::vector<PathPoint> &PathData::path_reference() const {
  return path_reference_;
}

//用给定的参考路径去赋值PathData类成员里的参考路径，直接move，而非拷贝
void PathData::set_path_reference(
    const std::vector<PathPoint> &path_reference) {
  path_reference_ = std::move(path_reference);
}

}  // namespace planning
}  // namespace apollo