百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\util\common.cc & common.h代码解析

概述

common.cc/.h是planning模块下的common/util路径下
根据路径和命名也可以看出
common.cc其实就是实现了一些planning中会用到的一些通用的辅助工具，util这个文件夹在很多工程中都可以看到，都是存放一些辅助功能的实现代码。

从代码来看,common.cc主要是实现：
第一个函数
根据停止线的Frenet系的纵坐标s构建PathDecision路径决策类对象(停车决策)，并设置相关的停车原因，停车点xyz坐标，停车处heading角等信息。

第二个函数(重载第一个函数)
根据障碍物所处的车道id以及在障碍物在车道中的Frenet系的纵坐标s构建PathDecision路径决策类对象(停车决策)，并设置相关的停车原因，停车点xyz坐标，停车处heading角等信息。

主要就是实现针对停止线/车道中的障碍物设定路径决策对象。

common.h

#pragma once

#include <string>
#include <vector>

#include "modules/planning/common/frame.h"
#include "modules/planning/common/reference_line_info.h"

namespace apollo {
namespace planning {
namespace util { //命名空间 apollo::planning::util::
/*
 * @brief:建立虚拟的障碍物或这虚拟墙，并且增加STOP停车决策
 */
 //参数stop_wall_id障碍物ID，是字符串类
 //参数stop_line_s是Frenet坐标系下停止线的纵坐标s
 //参数安全距离stop_distance
 //参数stop_reason_code是StopReasonCode类型变量，停车原因代码
 //参数wait_for_obstacles是一个string类型的vector，猜测其是一系列障碍物ID?
 //参数decision_tag是string类型，猜测就是决策的tag,也就是表明是停车/超车/减速避让等
 //参数Frame类 frame  Frame里面保存了一次规划周期的所有数据，后续的博客将会解析到
 //参数ReferenceLineInfo类 reference_line_info ReferenceLineInfo类提供平滑的参考
                        //线，通常是道路中心线
int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
                      const double stop_distance,
                      const StopReasonCode& stop_reason_code,
                      const std::vector<std::string>& wait_for_obstacles,
                      const std::string& decision_tag, Frame* const frame,
                      ReferenceLineInfo* const reference_line_info);

//函数重载，建立stop停车决策，参数几乎同上，唯一的不同是将参数stop_line_s停止线位置换
//成了lane_id车道Id和对应的车道的障碍物相对与车道的纵向位置lane_s
int BuildStopDecision(const std::string& stop_wall_id,
                      const std::string& lane_id, const double lane_s,
                      const double stop_distance,
                      const StopReasonCode& stop_reason_code,
                      const std::vector<std::string>& wait_for_obstacles,
                      const std::string& decision_tag, Frame* const frame,
                      ReferenceLineInfo* const reference_line_info);
}  // namespace util
}  // namespace planning
}  // namespace apollo

common.cc

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

namespace apollo {
namespace planning {
namespace util {

//用到apollo::common::util::命名空间下的另一个函数WithinBound,判断一个value是否在上下边界之间，返回true or false
using apollo::common::util::WithinBound;

/*
 * @brief:建立虚拟的障碍物或这虚拟墙，并且增加STOP停车决策
 */
 //参数stop_wall_id障碍物ID，是字符串类
 //参数stop_line_s是Frenet坐标系下停止线的纵坐标s
 //参数安全距离stop_distance
 //参数stop_reason_code是StopReasonCode类型变量，停车原因代码
 //参数wait_for_obstacles是一个string类型的vector，猜测其是一系列障碍物ID?
 //参数decision_tag是string类型，猜测就是决策的tag,也就是表明是停车/超车/减速避让等
 //参数Frame类 frame  Frame里面保存了一次规划周期的所有数据，后续的博客将会解析到
 //参数ReferenceLineInfo类 reference_line_info ReferenceLineInfo类提供平滑的参考
                        //线，通常是道路中心线
int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
                      const double stop_distance,
                      const StopReasonCode& stop_reason_code,
                      const std::vector<std::string>& wait_for_obstacles,
                      const std::string& decision_tag, Frame* const frame,
                      ReferenceLineInfo* const reference_line_info) {
  CHECK_NOTNULL(frame);//判断frame不为NULL空，否则报错
  CHECK_NOTNULL(reference_line_info);//判断reference_line_info不为NULL空，否则报错

  // check
  //获取reference_line_info中的参考线数据放入reference_line
  const auto& reference_line = reference_line_info->reference_line();
  //判断停止线的纵向位置s是否处于 区间[0 参考线总长度]，否则报错
  if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
    AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line";
    return 0;
  }

  // create virtual stop wall
  // 创建虚拟的停止墙
  //可以看出CreateStopObstacle是Frame类的成员函数
  //参数reference_line_info参考线信息，stop_wall_id虚拟墙障碍物id
  //stop_line_s是停止线Frenet纵向位置，涉及Frame部分后续会有专门博客讲解
  const auto* obstacle =
      frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s);
  //如果obstacle为空，则报错
  if (!obstacle) {
    AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
    return -1;
  }
  //这里又调用了ReferenceLineInfo类的成员函数AddObstacle()其实就是在参考线上增加
  //了一个障碍物，并返回一个障碍物对象
  const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
  if (!stop_wall) {
    AERROR << "Failed to add obstacle[" << stop_wall_id << "]";
    return -1;
  }

  // build stop decision
  // 建立停车决策
  // 停止位置stop_s = 停止线位置 - 安全距离stop_distance，就是留一个安全距离作为缓冲
  // 不能刚好停在停止线那，而是把期望停止点往回挪一个安全距离
  const double stop_s = stop_line_s - stop_distance;
  //参考线reference_line调用成员函数GetReferencePoint()查询参考线上停止点信息
  const auto& stop_point = reference_line.GetReferencePoint(stop_s);
  //查询停止点在参考线上的heading角
  const double stop_heading =
      reference_line.GetReferencePoint(stop_s).heading();

  //ObjectDecisionType是个找不到的类，是.proto文件里的message生成的类
  //这是google protobuf库的用法，message可以表示一种数据结构
  //message ObjectDecisionType定义位于modules\planning\proto\decision.proto
  //这个proto里储存者多种针对object物体的行为决策类型 
  //例如:ignore/stop/follow/yield/overtake/nudge/side_pass
  //忽略/停止/跟随/减速避让(车，人)/超车/接近(催促？)/绕行？
  ObjectDecisionType stop;  //定义一个物体决策类型对象stop
  auto* stop_decision = stop.mutable_stop(); //将指针stop_decision指向物体决策类型
  											 //对象stop里的stop
  //设置物体决策类型对象stop里的stop决策的停车原因
  stop_decision->set_reason_code(stop_reason_code);
  //设置物体决策类型对象stop里的stop决策的停车安全距离
  stop_decision->set_distance_s(-stop_distance);
  //设置物体决策类型对象stop里的stop决策的停止点的heading
  stop_decision->set_stop_heading(stop_heading);
  //设置物体决策类型对象stop里的stop决策的停止点x坐标
  stop_decision->mutable_stop_point()->set_x(stop_point.x());
  //设置物体决策类型对象stop里的stop决策的停止点y坐标
  stop_decision->mutable_stop_point()->set_y(stop_point.y());
  //设置物体决策类型对象stop里的stop决策的停止点z坐标
  stop_decision->mutable_stop_point()->set_z(0.0);

  //将一些列需要等待的障碍物加到设置物体决策类型对象stop里的stop决策的
  //加到这一项wait_for_obstacle，这是protobuf库的用法
  //add_就可以增加一个repeated元素
  for (size_t i = 0; i < wait_for_obstacles.size(); ++i) {
    stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]);
  }
  
  //路径决策path_decision = 参考线调用成员函数path_decision()返回其数据成员路径决策
  //路径决策其实是隐藏在ReferenceLineInfo类里的吗？
  auto* path_decision = reference_line_info->path_decision();
  //路径决策path_decision，纵向决策增加一个stop
  path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);

  return 0;
}
//函数重载，建立stop停车决策，参数几乎同上，唯一的不同是将参数stop_line_s停止线位置换
//成了lane_id车道Id和对应的车道的障碍物相对与车道的纵向位置lane_s
int BuildStopDecision(const std::string& stop_wall_id,
                      const std::string& lane_id, const double lane_s,
                      const double stop_distance,
                      const StopReasonCode& stop_reason_code,
                      const std::vector<std::string>& wait_for_obstacles,
                      const std::string& decision_tag, Frame* const frame,
                      ReferenceLineInfo* const reference_line_info) {
  //检查frame，reference_line_info是否为空，否则报错
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);

  //获取reference_line_info中的参考线
  const auto& reference_line = reference_line_info->reference_line();

  // create virtual stop wall
  //创建虚拟墙障碍物对象，这个CreateStopObstacle()函数也是一个重载函数
  //其参数也变成了停止墙id stop_wall_id,车道id lane_id,停止点的在车道上对应的Frenet
  //纵向坐标lane_s
  const auto* obstacle =
      frame->CreateStopObstacle(stop_wall_id, lane_id, lane_s);
  if (!obstacle) { //检查障碍物对象是否创建成功
    AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
    return -1;
  }

  //在参考线信息上增加障碍物
  const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
  if (!stop_wall) {
    AERROR << "Failed to create obstacle for: " << stop_wall_id;
    return -1;
  }
  //停止墙盒由障碍物对象调用其成员函数PerceptionBoundingBox()返回其障碍物边界盒
  const auto& stop_wall_box = stop_wall->PerceptionBoundingBox();
  //判断障碍物边界盒的中心在车道上？，不不在的话报debug信息并直接返回
  if (!reference_line.IsOnLane(stop_wall_box.center())) {
    ADEBUG << "stop point is not on lane. SKIP STOP decision";
    return 0;
  }

  // build stop decision
  //建立停止决策
  //停止点就位于参考线上查找停止墙边界盒的起始Frenet纵向坐标start_s - 停车安全距离
  //stop_distance
  auto stop_point = reference_line.GetReferencePoint(
      stop_wall->PerceptionSLBoundary().start_s() - stop_distance);

  //往物体决策类型对象stop里写入相关的信息
  //做出停车决策的原因，安全距离，停止点期望的x,y,z坐标及heading角
  ObjectDecisionType stop;
  auto* stop_decision = stop.mutable_stop();
  stop_decision->set_reason_code(stop_reason_code);
  stop_decision->set_distance_s(-stop_distance);
  stop_decision->set_stop_heading(stop_point.heading());
  stop_decision->mutable_stop_point()->set_x(stop_point.x());
  stop_decision->mutable_stop_point()->set_y(stop_point.y());
  stop_decision->mutable_stop_point()->set_z(0.0);

  //路径决策path_decision = 参考线调用成员函数path_decision()返回其数据成员路径决策
  //路径决策其实是隐藏在ReferenceLineInfo类里的吗？
  auto* path_decision = reference_line_info->path_decision();
  //路径决策path_decision，纵向决策增加一个stop
  path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);

  return 0;
}

}  // namespace util
}  // namespace planning
}  // namespace apollo