百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\PathDecision类代码详解

概述

*跳过Apollo\modules\planning\common\PathDecision类，其主要针对非结构化道路，如泊车等？开放空间信息类，目前只专注于onlaneplanning

PathDecision类是apollo planning模块下modules\planning\common\path_decision.cc/.h实现

从类名来看，应该是路径决策类。

从代码来看PathDecision类主要是实现：

储存/增加障碍物类对象到障碍物列表；
增加针对某个障碍物横纵向决策；
设置某个障碍物里的ST边界；
将针对目标物体的停车决策与主停车决策融合；(主停车决策可能是针对车道终点，道路终点停止线等，针对目标的停车决策是临时的)

后续结合其他代码一起看，可能会对一些细节的概念理解更加清楚。

path_decision.h

#pragma once

#include <limits>
#include <string>

#include "modules/planning/common/indexed_list.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/proto/decision.pb.h"

namespace apollo {
namespace planning {

/**
 * @class PathDecision 路径决策类
 *
 * @brief 路径决策类在一条路径上表示了所有障碍物
 */
class PathDecision {
 public:
  //默认构造函数
  PathDecision() = default;

  //增加一个障碍物对象，在类数据成员障碍物列表obstacles_里增加一个障碍物对象
  Obstacle *AddObstacle(const Obstacle &obstacle);

  //返回类数据成员，障碍物列表，里面储存着id和障碍物对象的映射关系，按对储存
  const IndexedList<std::string, Obstacle> &obstacles() const;

  //增加横向决策，输入参数是tag字符串，障碍物id，给定目标决策类型
  //ObjectDecisionType类由modules\planning\proto\decision.proto定义生成的message类，包括 ignore/stop/follow/yield/...，对目标物体的决策类型
  //其实就是把输入参数的决策类型，增加到这个id的障碍物对象的数据成员决策列表里
  bool AddLateralDecision(const std::string &tag, const std::string &object_id,
                          const ObjectDecisionType &decision);
  bool AddLongitudinalDecision(const std::string &tag,
                               const std::string &object_id,
                               const ObjectDecisionType &decision);
//根据输入的障碍物id，去类成员障碍物列表中跟据id返回对应的障碍物对象,不可修改？
  const Obstacle *Find(const std::string &object_id) const;

//根据输入的感知障碍物id(感知id难道和障碍物Id不一样？)去类数据成员障碍物列表中根据输入参数的感知id去返回相应的感知障碍物对象
  const perception::PerceptionObstacle *FindPerceptionObstacle(
      const std::string &perception_obstacle_id) const;

//根据输入的障碍物id，去类成员障碍物列表中跟据id返回对应的障碍物对象,可修改？
  Obstacle *Find(const std::string &object_id);

//设置ST边界，根据输入的障碍物Id,用这个障碍物去修改自车的ST边界，修改后的边界存放在boundary
  void SetSTBoundary(const std::string &id, const STBoundary &boundary);
  
  //擦除ST边界
  void EraseStBoundaries();
  
  //返回类数据成员
  //主停止类 是由modules\planning\proto\decision.proto定义的message类生成
  //里面主要包含停止点，停止原因，停止航向角等信息
  MainStop main_stop() const { return main_stop_; }
  //返回类数据成员 停止参考线s
  double stop_reference_line_s() const { return stop_reference_line_s_; }
  //将对于障碍物做出的停车决策与主停止决策混合？
  bool MergeWithMainStop(const ObjectStop &obj_stop, const std::string &obj_id,
                         const ReferenceLine &ref_line,
                         const SLBoundary &adc_sl_boundary);

 private:
  //类成员 障碍物列表
  IndexedList<std::string, Obstacle> obstacles_;
  //主停止类对象
  MainStop main_stop_;
  //停止的参考线的s初始化为一个非常大的数，就是double表示的最大的数
  double stop_reference_line_s_ = std::numeric_limits<double>::max();
};

}  // namespace planning
}  // namespace apollo

path_decision.cc

#include "modules/planning/common/path_decision.h"

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/util.h"
#include "modules/perception/proto/perception_obstacle.pb.h"

namespace apollo {
namespace planning {
//在路径决策类数据成员障碍物列表里增加一个障碍物对象
Obstacle *PathDecision::AddObstacle(const Obstacle &obstacle) {
  return obstacles_.Add(obstacle.Id(), obstacle);
}

//返回PathDecision类的数据成员障碍物列表
const IndexedObstacles &PathDecision::obstacles() const { return obstacles_; }
//去PathDecision类的数据成员障碍物列表里根据给定的id返回相应的障碍物对象,返回的对象可以修改
Obstacle *PathDecision::Find(const std::string &object_id) {
  return obstacles_.Find(object_id);
}

//去PathDecision类的数据成员障碍物列表里根据给定的id返回相应的障碍物对象,返回的对象不可以修改
const Obstacle *PathDecision::Find(const std::string &object_id) const {
  return obstacles_.Find(object_id);
}

//去障碍物列表里寻找和给定的感知障碍物id相同的感知障碍物对象，难道感知障碍物id和障碍物id不同？
const perception::PerceptionObstacle *PathDecision::FindPerceptionObstacle(
    const std::string &perception_obstacle_id) const {
  for (const auto *obstacle : obstacles_.Items()) {
    if (std::to_string(obstacle->Perception().id()) == perception_obstacle_id) {
      return &(obstacle->Perception());
    }
  }

  return nullptr;
}

//根据给定的障碍物id，去障碍物列表里找到障碍物对象，并用这个障碍物对象去访问其自身的障碍物类的成员函数set_path_st_boundary，根据输入参数给定的ST边界去设置障碍物的类成员path_st_boundary_路径st边界
void PathDecision::SetSTBoundary(const std::string &id,
                                 const STBoundary &boundary) {
  auto *obstacle = obstacles_.Find(id);

  if (!obstacle) {
    AERROR << "Failed to find obstacle : " << id;
    return;
  } else {
    obstacle->set_path_st_boundary(boundary);
  }
}

//从类里储存的障碍物列表获取给定id的障碍物对象，然后针对这个障碍物对象增加一个横向的决策，增加的横向决策的tag和决策都是由输入参数传入
//目标决策类型包括：stop/follow/yield/...
bool PathDecision::AddLateralDecision(const std::string &tag,
                                      const std::string &object_id,
                                      const ObjectDecisionType &decision) {
  auto *obstacle = obstacles_.Find(object_id);
  if (!obstacle) {
    AERROR << "failed to find obstacle";
    return false;
  }
  obstacle->AddLateralDecision(tag, decision);
  return true;
}

//擦除ST边界，遍历PathDecision类数据成员障碍物列表，清空每个障碍物对象里储存的ST边界
void PathDecision::EraseStBoundaries() {
  for (const auto *obstacle : obstacles_.Items()) {
    auto *obstacle_ptr = obstacles_.Find(obstacle->Id());
    obstacle_ptr->EraseStBoundary();
  }
}

//针对给定的障碍物Id对应的障碍物对象，障碍物对象增加一个纵向决策，纵向决策的目标决策类型也是由输入参数decision给定
bool PathDecision::AddLongitudinalDecision(const std::string &tag,
                                           const std::string &object_id,
                                           const ObjectDecisionType &decision) {
  auto *obstacle = obstacles_.Find(object_id);
  if (!obstacle) {
    AERROR << "failed to find obstacle";
    return false;
  }
  obstacle->AddLongitudinalDecision(tag, decision);
  return true;
}

//将输入参数的针对目标的停止与主停止决策融合
//输入参数目标停止类对象，待融合的
//输入参数 目标障碍物id
//输入参数 参考线类对象
//输入参数 自车的sl边界
bool PathDecision::MergeWithMainStop(const ObjectStop &obj_stop,
                                     const std::string &obj_id,
                                     const ReferenceLine &reference_line,
                                     const SLBoundary &adc_sl_boundary) {
  //获取目标停止类的停止点
  common::PointENU stop_point = obj_stop.stop_point();
  //定义一个停止线sl坐标
  common::SLPoint stop_line_sl;
  //输入参数的参考线访问其成员函数，将停止点的xy坐标转化为停止点的SL坐标
  //SL坐标都是相对参考线的横纵向，即frenet系
  reference_line.XYToSL(stop_point, &stop_line_sl);

  //获取停止点sl坐标的frenet系纵坐标s
  double stop_line_s = stop_line_sl.s();
  //如果目标停止点的s坐标小于0或目标停止点的s比道路参考线的长度还要长那么就忽略这个物体，直接返回false
  if (stop_line_s < 0.0 || stop_line_s > reference_line.Length()) {
    AERROR << "Ignore object:" << obj_id << " fence route_s[" << stop_line_s
           << "] not in range[0, " << reference_line.Length() << "]";
    return false;
  }

  //检查停止点纵坐标s vs 自车s，如果目标停止点s比主停止更远的话忽略，直接返回
  const auto &vehicle_config = common::VehicleConfigHelper::GetConfig();
  stop_line_s = std::fmax(
      stop_line_s, adc_sl_boundary.end_s() -
                       vehicle_config.vehicle_param().front_edge_to_center());

  if (stop_line_s >= stop_reference_line_s_) {
    ADEBUG << "stop point is farther than current main stop point.";
    return false;
  }

//如果上面都没返回，那么就用针对目标对象的停止点代替主停止点
//重新设置主停止点，heading，停止原因等
  main_stop_.Clear();
  main_stop_.set_reason_code(obj_stop.reason_code());
  main_stop_.set_reason("stop by " + obj_id);
  main_stop_.mutable_stop_point()->set_x(obj_stop.stop_point().x());
  main_stop_.mutable_stop_point()->set_y(obj_stop.stop_point().y());
  main_stop_.set_stop_heading(obj_stop.stop_heading());
  stop_reference_line_s_ = stop_line_s;

  ADEBUG << " main stop obstacle id:" << obj_id
         << " stop_line_s:" << stop_line_s << " stop_point: ("
         << obj_stop.stop_point().x() << obj_stop.stop_point().y()
         << " ) stop_heading: " << obj_stop.stop_heading();
  return true;
}

}  // namespace planning
}  // namespace apollo