apollo自动驾驶进阶学习之：common:path_data.cc

    #pragma once
    
    #include <list>
    #include <string>
    #include <tuple>
    #include <utility>
    #include <vector>
    
    #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:
      //枚举类，用法参考1.1.1
      enum class PathPointType {
    IN_LANE,
    OUT_ON_FORWARD_LANE,
    OUT_ON_REVERSE_LANE,
    OFF_ROAD,
    UNKNOWN,
      };
      //默认构造函数
      PathData() = default;
      //设置discretized_path_
      bool SetDiscretizedPath(DiscretizedPath path);
      //设置frenet_path_
      bool SetFrenetPath(FrenetFramePath frenet_path);
      //设置path_reference_
      void SetReferenceLine(const ReferenceLine *reference_line);
      
      //设置path的前进方式（比如左侧借并行车道，左侧借反向车道，右侧借并行车道等等）
      bool SetPathPointDecisionGuide(
      std::vector<std::tuple<double, PathPointType, double>>
          path_point_decision_guide);
      //获取discretized_path_
      const DiscretizedPath &discretized_path() const;
      //获取frenet_path_
      const FrenetFramePath &frenet_frame_path() const;
      //获取path的前进方式（比如左侧借并行车道，左侧借反向车道，右侧借并行车道等等）
      const std::vector<std::tuple<double, PathPointType, double>>
      &path_point_decision_guide() const;
    
      //差值法求PathPoint
      common::PathPoint GetPathPointWithPathS(const double s) const;
    
      /* * brief: this function will find the path_point in discretized_path whose
       * projection to reference line has s value closest to ref_s.
       */
      bool GetPathPointWithRefS(const double ref_s,
                            common::PathPoint *const path_point) const;
      //裁剪PathData
      bool LeftTrimWithRefS(const common::FrenetFramePoint &frenet_point);
      //更新reference_line_
      bool UpdateFrenetFramePath(const ReferenceLine *reference_line);
    
      void Clear();
    
      bool Empty() const;
      //打印调试信息
      std::string DebugString() const;
      
      //fallback,regular/pullover,regular/lanechange,regular/laneborrow
      void set_path_label(const std::string &label);
      //获取path_label_
      const std::string &path_label() const;
      //设置障碍物信息
      void set_blocking_obstacle_id(const std::string &obs_id) {
    blocking_obstacle_id_ = obs_id;
      }
      //获取障碍物信息
      const std::string &blocking_obstacle_id() const {
    return blocking_obstacle_id_;
      }
    
      const bool is_valid_path_reference() const {
    return is_valid_path_reference_;
      }
      void set_is_valid_path_reference(bool is_valid_path_reference) {
    is_valid_path_reference_ = is_valid_path_reference;
      }
    
      const bool is_optimized_towards_trajectory_reference() const {
    return 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_
      const std::vector<common::PathPoint> &path_reference() const;
      //设置path_reference_
      void set_path_reference(const std::vector<common::PathPoint> &path_reference);
    
     private:
      /* * convert frenet path to cartesian path by reference line
       */
      //坐标系变换
      bool SLToXY(const FrenetFramePath &frenet_path,
              DiscretizedPath *const discretized_path);
      //坐标系变换
      bool XYToSL(const DiscretizedPath &discretized_path,
              FrenetFramePath *const frenet_path);
      const ReferenceLine *reference_line_ = nullptr;
      DiscretizedPath discretized_path_;
      FrenetFramePath frenet_path_;
      /** * @brief speed decision generated by path analyzer for guiding speed limit
       * generation in speed bounds decider
       * @param tuple consists of s axis position on reference line; PathPointType
       * Enum; distance to closest obstacle
       */
      std::vector<std::tuple<double, PathPointType, double>>
      path_point_decision_guide_;
    
      std::string path_label_ = "";
      std::string blocking_obstacle_id_;
    
      /** * @brief parameters for using the learning model output as a path reference
       * */
      // wheter this PathData is a path reference serving as an optimization target
      // for later modules
      bool is_valid_path_reference_ = false;
    
      /** * @brief Given a trajectory reference, whether this PathData is optimized
       * according to the "path" part of the trajectory so that "speed" part of the
       * trajectory could be used in later modules accordingly
       * */
      bool is_optimized_towards_trajectory_reference_ = false;
    
      // path reference
      std::vector<common::PathPoint> path_reference_;
    };
    
    }  // namespace planning
    }  // namespace apollo

    enum class peppers {
    red,
    yellow,
    green
    }；
     
    enum class flowers{
    red,
    while,
    green
    }；
     
     
    peppers p = green; //错误，没使用枚举类的作用域访问，无法直接访问枚举类的成员。
    peppers p2 = peppers::red; //正确，遵循作用域准则，可以访问作用域内的成员。
    flowers f = flowers::red; //正确，遵循作用域准则，可以访问作用域内的成员。

    #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;
    
    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;
    }
    
    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;
    }
    
    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;
      }
      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;
    }
    
    const DiscretizedPath &PathData::discretized_path() const {
      return discretized_path_;
    }
    
    const FrenetFramePath &PathData::frenet_frame_path() const {
      return frenet_path_;
    }
    
    const std::vector<std::tuple<double, PathData::PathPointType, double>>
    &PathData::path_point_decision_guide() const {
      return path_point_decision_guide_;
    }
    //清空成员变量
    bool PathData::Empty() const {
      return discretized_path_.empty() && frenet_path_.empty();
    }
    
    void PathData::SetReferenceLine(const ReferenceLine *reference_line) {
      Clear();
      reference_line_ = reference_line;
    }
    //线性插值的方式寻找s对应的PathPoint
    common::PathPoint PathData::GetPathPointWithPathS(const double s) const {
      return discretized_path_.Evaluate(s);
    }

    bool PathData::GetPathPointWithRefS(const double ref_s,
                                    common::PathPoint *const path_point) const {
      ACHECK(reference_line_);
      DCHECK_EQ(discretized_path_.size(), frenet_path_.size());
      if (ref_s < 0) {
    AERROR << "ref_s[" << ref_s << "] should be > 0";
    return false;
      }
      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;
      for (uint32_t i = 0; i + 1 < frenet_path_.size(); ++i) {
    if (fabs(ref_s - frenet_path_.at(i).s()) < kDistanceEpsilon) {
      path_point->CopyFrom(discretized_path_.at(i));
      return true;
    }
    if (frenet_path_.at(i).s() < ref_s && ref_s <= frenet_path_.at(i + 1).s()) {
      index = i;
      break;
    }
      }
      double r = (ref_s - frenet_path_.at(index).s()) /
             (frenet_path_.at(index + 1).s() - frenet_path_.at(index).s());
      //这段代码本质上就是线性插值，和GetPathPointWithPathS没有什么本质的区别
      const double discretized_path_s = discretized_path_.at(index).s() +
                                    r * (discretized_path_.at(index + 1).s() -
                                         discretized_path_.at(index).s());
      path_point->CopyFrom(discretized_path_.Evaluate(discretized_path_s));
    
      return true;
    }
    
    void PathData::Clear() {
      discretized_path_.clear();
      frenet_path_.clear();
      path_point_decision_guide_.clear();
      path_reference_.clear();
      reference_line_ = nullptr;
    }
    
    std::string PathData::DebugString() const {
      const auto limit =
      std::min(discretized_path_.size(),
               static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));
    
      return absl::StrCat(
      "[\n",
      absl::StrJoin(discretized_path_.begin(),
                    discretized_path_.begin() + limit, ",\n",
                    apollo::common::util::DebugStringFormatter()),
      "]\n");
    }
    //Frenet坐标到笛卡尔坐标系的变换
    //参考文章https://zhuanlan.zhihu.com/p/109193953
    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坐标的变换
    //参考文章https://zhuanlan.zhihu.com/p/109193953
    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;
    }
    
    bool PathData::LeftTrimWithRefS(const common::FrenetFramePoint &frenet_point) {
      ACHECK(reference_line_);
      std::vector<common::FrenetFramePoint> frenet_frame_points;
      frenet_frame_points.emplace_back(frenet_point);
    
      for (const common::FrenetFramePoint fp : frenet_path_) {
    if (std::fabs(fp.s() - frenet_point.s()) < 1e-6) {
      continue;
    }
    if (fp.s() > frenet_point.s()) {
      frenet_frame_points.push_back(fp);
    }
      }
      SetFrenetPath(FrenetFramePath(std::move(frenet_frame_points)));
      return true;
    }
    
    bool PathData::UpdateFrenetFramePath(const ReferenceLine *reference_line) {
      reference_line_ = reference_line;
      return SetDiscretizedPath(discretized_path_);
    }
    
    void PathData::set_path_label(const std::string &label) { path_label_ = label; }
    
    const std::string &PathData::path_label() const { return path_label_; }
    
    const std::vector<PathPoint> &PathData::path_reference() const {
      return path_reference_;
    }
    
    void PathData::set_path_reference(
    const std::vector<PathPoint> &path_reference) {
      path_reference_ = std::move(path_reference);
    }
    
    }  // namespace planning
    }  // namespace apollo

    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;
    }