apollo DP dp_road_graph.cc代码注释

/******************************************************************************
 * 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 dp_road_graph.cc
 **/

#include "modules/planning/tasks/optimizers/road_graph/dp_road_graph.h"

#include "cyber/common/log.h"
#include "cyber/task/task.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/proto/error_code.pb.h"
#include "modules/common/util/util.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/path/frenet_frame_path.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
#include "modules/planning/proto/planning_internal.pb.h"
#include "modules/planning/proto/planning_status.pb.h"

namespace apollo {
namespace planning {

DpRoadGraph::DpRoadGraph(const DpPolyPathConfig &config,
                         const ReferenceLineInfo &reference_line_info,
                         const SpeedData &speed_data)
    : config_(config),
      reference_line_info_(reference_line_info),
      reference_line_(reference_line_info.reference_line()),
      speed_data_(speed_data) {}

//借助GenerateMinCostPath得到代价最小路径 将每段最小代价curve以path_resolution为间隔离散化，结果存入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(), &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()); //得到参考线上点的s值有计算笛卡尔坐标和ferenet坐标转换时，计算

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

  // Step B. 获取当前参考线下最优的前进路线
  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();

  //如果是4个点 那么 i<4 i只能到3 cur_node[3]
  for (size_t i = 1; i < min_cost_path.size(); ++i) {   //min_cost_path是一个DpRoadGraphNode 结构体
    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;   //s是每一个prev到cur之间的s
    const auto &curve = cur_node.min_cost_curve;  //现在该点下一时刻的最小cost曲线
    //对每一段curve进行采样   current_s 以path_resolution的进行增加
    while (current_s + path_resolution / 2.0 < path_length) {     
      //表示当前s向前移动一个分辨率步长的中点的长度，这个中点位置被用来判断当前位置是否已经接近路径的末端， 如果跳出循环说明距离不够一个分辨率
      const double l = curve.Evaluate(0, current_s);    //计算出轨迹在当前s的l至
      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) {  //之前并没有计算累计距离 ，对于最后一个路径点距离应该是初始的s+现在的s  因为最后一段距离可能不够一定、
      accumulated_s += current_s;       
    } else {
      accumulated_s += path_length; //这个看整个for循环
    }
  }
  path_data->SetReferenceLine(&reference_line_);
  path_data->SetFrenetPath(FrenetFramePath(std::move(frenet_path)));
  return true;
}


bool DpRoadGraph::GenerateMinCostPath(
    const std::vector<const Obstacle *> &obstacles,
    std::vector<DpRoadGraphNode> *min_cost_path) {
  ACHECK(min_cost_path != nullptr);
  
  //基于当前参考线及初始点，生成候选路径采样点数组
  //路径航向（path_waypoints）里面的每个vector存储相同s值 即每个s时的横向采样
  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());
  
  /*--------最小代价值路图节表点链表---------*/
  //储存构建的graph，速度DP过程用的是数据结构 vector<vector<>>,  路径这里是list< list<> >     每个node都保存了从前一列到该节点的最小cost
  std::list<std::list<DpRoadGraphNode>> graph_nodes;      //这是最后的前向遍历图

  // find one point from first row  从第一个s下找一个点
  const auto &first_row = path_waypoints.front();   //first_row = [l1,l2,l3,l4,l5] 第一列的点
  size_t nearest_i = 0;
  //在第一列中寻找距离初始点node最近的l点   
  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();//空的    第0列
  graph_nodes.back().emplace_back(first_row[nearest_i], nullptr,
                                  ComparableCost());    //是在第0列中emplace也就是 加入第0列第0行
  auto &front = graph_nodes.front().front();    //第0列第0行
  size_t total_level = path_waypoints.size();   //总的列
  //采用自下而上的动态规划算法，迭代更新最小代价值
  //graph_nodes 储存的就是各级（level）路径航点（path_waypoints）所包含的最小代价航点
  //graph_nodes.back（）  （即最后一个航点链表）就是我们所需的最小代价航点链表

  //构建  graph_nodes
  //两层循环 ：
  //        外循环  -- 撒点的列数;
  //        内循环  --  列中的每个点
  //说白了 在for循环中可以看到的语句就是 遍历点 以及生成node message 
  for (size_t level = 1; level < path_waypoints.size(); ++level) {      //path_waypoints    vector<vector<>> 遍历路径点的各层
    // graph_nodes  list< list<> >  目前数据内部只含有 第一列距离初始位置最近的点
    const auto &prev_dp_nodes = graph_nodes.back();    //上一层路径的最优路径点的集合         
    const auto &level_points = path_waypoints[level];    //获取当前层的路径航点

    graph_nodes.emplace_back();         //作用是在list中插入空的列
    std::vector<std::future<void>> results;   //用future储存并行执行结果

    //遍历当前层的所有航迹点
    for (size_t i = 0; i < level_points.size(); ++i) {      //每一列点的大小 
      //第level列第i行
      const auto &cur_point = level_points[i];    //因为在这已经不太容易判断类型了 每一列的第一个点开始撒点
       
      //因为graph_nodes是双list嵌套的 就是在列empalce_back 而类型是DpRoadGraphNode 详见其构造函数 可以看到需要传入参数的类型
      
      //在外层空列中 插入当前node
      graph_nodes.back().emplace_back(cur_point, nullptr);

      auto msg = std::make_shared<RoadGraphMessage>(    //作用就是记录下来当前node和前一个node下的cost
          prev_dp_nodes, level, total_level, &trajectory_cost, &front,    //front是上一个点
          &(graph_nodes.back().back()));  //graph_nodes.back

      //采用多线程并行计算最小代价值行点        
      if (FLAGS_enable_multi_thread_in_dp_poly_path) {
        results.emplace_back(cyber::Async(&DpRoadGraph::UpdateNode, this, msg));
      } else {            
      /*-----------------------------------------------------------------------------------*/  
      /*----------计算前一层prev_dp_nodes和当前层的节点cur_node的开销cost，取prev_dp_nodes中与cur_node开销cost最小的节点，设置最优路径----------------------*/
      /*-----------------------------------------------------------------------------------*/
        UpdateNode(msg);    //msg中的东西由上面赋值
      }
    }
    if (FLAGS_enable_multi_thread_in_dp_poly_path) {
      for (auto &result : results) {
        result.get();
      }
    }
  }
  //上面计算出来了所有level中每一个node：  当前node与前一level中的最小cost






  //以上只是把2node间的代价计算出来了，并没有寻找代价最小的路径 
  //选择最后一个level中，具有最小cost的node最为规划终点
  // find best path
  DpRoadGraphNode fake_head;
  //这是有两层遍历
  for (const auto &cur_dp_node : graph_nodes.back()) {    //只遍历最后一列  cur_dp_node 是遍历列中的点
    fake_head.UpdateCost(&cur_dp_node, cur_dp_node.min_cost_curve,
                         cur_dp_node.min_cost);    
     //选择最后一个level中，具有最小cost的node作为规划终点  记录下来最小cost的点                    
  }
    //在最后的level采样点中，寻找代价最小的节点，作为一次规划的终点，即fake_head的prev
    //其实其他level的采样点很可能代价更小，这里为什么从最后的level选呢？
    //因为规划的长度和时间越长越好，规划越长，相当于自车看到的越远
  //根据父指针向前遍历，得到至后向前的一条路径
  const auto *min_cost_node = &fake_head;     //倒数第二列到最后一列的最小cost
  //Todo:min_cost_path没有把fake_head保存进去，fake_head->min_cost_prev_node才是真正的终点
  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;
}




//遍历前一蹭的node 计算出 从初始点到当前node的最小cost
void DpRoadGraph::UpdateNode(const std::shared_ptr<RoadGraphMessage> &msg) {
  CHECK_NOTNULL(msg);
  CHECK_NOTNULL(msg->trajectory_cost);
  CHECK_NOTNULL(msg->front);
  CHECK_NOTNULL(msg->cur_node);
  for (const auto &prev_dp_node : msg->prev_nodes) {        //对上一列的node进行遍历
    const auto &prev_sl_point = prev_dp_node.sl_point;      //上一列遍历点的sl
    const auto &cur_point = msg->cur_node->sl_point;        //本个遍历点的
    double init_dl = 0.0;
    double init_ddl = 0.0;
    if (msg->level == 1) {                        //
      init_dl = init_frenet_frame_point_.dl();    //因为第一列没有dl和ddl
      init_ddl = init_frenet_frame_point_.ddl();
    }
    QuinticPolynomialCurve1d curve(prev_sl_point.l(), init_dl, init_ddl,
                                   cur_point.l(), 0.0, 0.0,
                                   cur_point.s() - prev_sl_point.s());    //由l，s值拟合五次多项式

    if (!IsValidCurve(curve)) {
      continue;
    }
    const auto cost =
        msg->trajectory_cost->Calculate(curve, prev_sl_point.s(), cur_point.s(),    //通过调用函数来计算cost
                                        msg->level, msg->total_level) +             //为什么要传入level和total_level 因为最后一列时要保证尽量的接近参考线
        prev_dp_node.min_cost;
    //cost = 现在两列node之间的cost + 之前    因为每个node中，都保存了，从规划起始点到该节点的最小cost 所以 要加上之前的min_cost   
    //更新前一个点与现在点的最小cost，并记录下来前一个点的位置，和上边拟合的曲线数据
    msg->cur_node->UpdateCost(&prev_dp_node, curve, cost);    //曲线，该曲线的cost
  }

  // try to connect the current point with the first point directly   尝试用5次多项式拟合第一个点与当前node
  if (reference_line_info_.IsChangeLanePath() && msg->level >= 2) {   //如果 大于第二列并且要变道
    const double init_dl = init_frenet_frame_point_.dl();
    const double init_ddl = init_frenet_frame_point_.ddl();
    QuinticPolynomialCurve1d curve(
        init_sl_point_.l(), init_dl, init_ddl, msg->cur_node->sl_point.l(), 0.0,
        0.0, msg->cur_node->sl_point.s() - init_sl_point_.s());
    if (!IsValidCurve(curve)) {
      return;
    }
    const auto cost = msg->trajectory_cost->Calculate(
        curve, init_sl_point_.s(), msg->cur_node->sl_point.s(), msg->level,
        msg->total_level);
    msg->cur_node->UpdateCost(msg->front, curve, cost);
  }




}





bool DpRoadGraph::IsValidCurve(const QuinticPolynomialCurve1d &curve) const {
  static constexpr double kMaxLateralDistance = 20.0;
  for (double s = 0.0; s < curve.ParamLength(); s += 2.0) {
    const double l = curve.Evaluate(0, s);
    if (std::fabs(l) > kMaxLateralDistance) {
      return false;
    }
  }
  return true;
}

void DpRoadGraph::GetCurveCost(TrajectoryCost trajectory_cost,
                               const QuinticPolynomialCurve1d &curve,
                               const double start_s, const double end_s,
                               const uint32_t curr_level,
                               const uint32_t total_level,
                               ComparableCost *cost) {
  *cost =
      trajectory_cost.Calculate(curve, start_s, end_s, curr_level, total_level);
}

}  // namespace planning
}  // namespace apollo