概述

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

从代码来看util.cc主要是实现：
1.判断当前反馈的车辆状态是否有效；
2.判断两次routing(全局路径规划)的结果是否相同；
3.根据还剩下的刹车距离计算自车刹停需要的减速度；
4.检查道路参考线信息上是否有stopsign停车标志；
5.检查道路参考线信息上是否有交通灯；
6.检查自车是否处于路口；
7.读取指定路径下的文件的名称。

util.h

#pragma once

#include <string>
#include <vector>

#include <boost/filesystem.hpp>
#include <boost/range/iterator_range.hpp>

#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/routing/proto/routing.pb.h"

namespace apollo {
    
namespace planning {
    
namespace util {
    

//判断VehicleState车辆状态是否有效，输入参数是一个VehicleState类对象
bool IsVehicleStateValid(const apollo::common::VehicleState& vehicle_state);

//判断是否是不同的routing
//输入参数就是之前的RoutingResponse和之后的RoutingResponse
bool IsDifferentRouting(const apollo::routing::RoutingResponse& first,
                        const apollo::routing::RoutingResponse& second);

//获取ADC(Automative Driving Car)的刹停减速度
//参数VehicleStateProvider类对象vehicle_state
//参数自车前端对应的Frenet纵坐标adc_front_edge_s
//参数停止线对应的Frenet纵坐标stop_line_s
double GetADCStopDeceleration(
    apollo::common::VehicleStateProvider* vehicle_state,
    const double adc_front_edge_s, const double stop_line_s);

/*
 * 检查一个stop_sign_overlap是否在参考线上？就是检查参考线上是否有停车标志
 * Apollo中的overlap表示空间中两个元素的重叠关系
 */
 //参数：reference_line_info 参考线上的信息
 //参数：stop_sign_overlap_id停车标志的id
bool CheckStopSignOnReferenceLine(const ReferenceLineInfo& reference_line_info,
                                  const std::string& stop_sign_overlap_id);

/*
 *  检查一个traffic_light_overlap 是否在参考线上？就是检查参考线上是否有交通灯
 *  Apollo中的overlap表示空间中两个元素的重叠关系
 */
 //参数：reference_line_info 参考线上的信息
 //参数：traffic_light_overlap_id 交通灯overlap的id
bool CheckTrafficLightOnReferenceLine(
    const ReferenceLineInfo& reference_line_info,
    const std::string& traffic_light_overlap_id);

/*
 * 检查ADC(Automative Driving Car)是否在路口pnc-junction
 */
//参数 reference_line_info参考线信息
bool CheckInsidePnCJunction(const ReferenceLineInfo& reference_line_info);

/*
 * 获取一个路径下的文件名
 */
 //参数 路径path
 //参数 文件名，还是个string的vector,存放指定路径下读取到的多个文件名
 //涉及到boost库的用法
 //给定一个路径把指定路径下的文件的名称都读到vector files里
void GetFilesByPath(const boost::filesystem::path& path,
                    std::vector<std::string>* files);

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

util.cc

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

#include <limits>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/map/pnc_map/path.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
    
namespace planning {
    
namespace util {
    
//引用modules\common\vehicle_state\proto\vehicle_state.proto定义的message生成的
//VehicleState类  google protobuf生成.cc和.h文件，将.proto定义的message数据结构生成
//c++类
using apollo::common::VehicleState;
//引用modules\map\pnc_map\path.h里定义的PathOverlap结构体数据类型
using apollo::hdmap::PathOverlap;
//引用modules\routing\proto\routing.proto里定义的message RoutingResponse生成的类
using apollo::routing::RoutingResponse;

//判断VehicleState车辆状态是否有效，输入参数是一个VehicleState类对象
bool IsVehicleStateValid(const VehicleState& vehicle_state) {
    
  //判断vehicle_state的x,y,z,heading,曲率kappa,v,acc是否为nan(not a number)
  //只要有一个量是nan就直接返回false，否则返回true
  if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) ||
      std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) ||
      std::isnan(vehicle_state.kappa()) ||
      std::isnan(vehicle_state.linear_velocity()) ||
      std::isnan(vehicle_state.linear_acceleration())) {
    
    return false;
  }
  return true;
}

//判断是否是不同的routing
//输入参数就是之前的RoutingResponse和之后的RoutingResponse
bool IsDifferentRouting(const RoutingResponse& first,
                        const RoutingResponse& second) {
    
  if (first.has_header() && second.has_header()) {
    
    //若前后RoutingResponse里的header里的sequence_num,推测是车道段数是否不相同
    //是不相同则返回true，否则返回false
    return first.header().sequence_num() != second.header().sequence_num();
  }
  //如果有一个没有header那都说明是不同的routing
  return true;
}

//获取ADC(Automative Driving Car)的刹停减速度
//参数VehicleStateProvider类对象vehicle_state
//参数自车前端对应的Frenet纵坐标adc_front_edge_s
//参数停止线对应的Frenet纵坐标stop_line_s
double GetADCStopDeceleration(
    apollo::common::VehicleStateProvider* vehicle_state,
    const double adc_front_edge_s, const double stop_line_s) {
    
  //先获取车辆的实际车速放入adc_speed 
  double adc_speed = vehicle_state->linear_velocity();
  //从车辆参数配置文件modules\common\data\vehicle_param.pb.txt读取
  //读取判定停车时的最大速度绝对值(默认是0.2m/s)，当车速小于此阈值时，认为车辆已经停住
  //了
  const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()
                                        ->GetConfig()
                                        .vehicle_param()
                                        .max_abs_speed_when_stopped();
  //如果车辆实际速度已经小于阈值max_adc_stop_speed，则认为车辆已经停下
  //则需要刹停的减速度为0.0
  if (adc_speed < max_adc_stop_speed) {
    
    return 0.0;
  }

  //初始化stop_distance，目前还剩下可以用来减速的距离
  double stop_distance = 0;

  //如果停止线对应Frenet的纵坐标stop_line_s 比车辆前端对应的Frenet的纵坐标
  //adc_front_edge_s要大的话
  //剩下的可用来刹停的距离stop_distance  = 停止线s坐标-车前端s坐标
  if (stop_line_s > adc_front_edge_s) {
    
    stop_distance = stop_line_s - adc_front_edge_s;
  }
  //如果剩下可以用来刹停的距离小于1e-5，减速度直接拉满
  //让减速度为double可以表示的最大数值，返回这个最大数值
  if (stop_distance < 1e-5) {
    
    return std::numeric_limits<double>::max();
  }

  //如果剩下刹停的距离并不小，就根据高中物理 a = v^2/(2*x)，那么这里计算到的是
  //减速度的绝对值
  return (adc_speed * adc_speed) / (2 * stop_distance);
}

/*
 * 检查一个stop_sign_overlap是否在参考线上？就是检查参考线上是否有停车标志
 * Apollo中的overlap表示空间中两个元素的重叠关系
 */
 //参数：reference_line_info 参考线上的信息
 //参数：stop_sign_overlap_id停车标志的id
bool CheckStopSignOnReferenceLine(const ReferenceLineInfo& reference_line_info,
                                  const std::string& stop_sign_overlap_id) {
    
  //先从reference_line_info参考线信息上获取所有的stop_sign的overlap
  const std::vector<PathOverlap>& stop_sign_overlaps =
      reference_line_info.reference_line().map_path().stop_sign_overlaps();
  //遍历参考线线信息里的所有stopsign的overlap，看有没有id和给定Id相同的
  //令这个id相同的overlap对象赋给stop_sign_overlap_it，若没找到
  //stop_sign_overlap_it就会等于overlap里的最后一个
  auto stop_sign_overlap_it =
      std::find_if(stop_sign_overlaps.begin(), stop_sign_overlaps.end(),
                   [&stop_sign_overlap_id](const PathOverlap& overlap) {
    
                     return overlap.object_id == stop_sign_overlap_id;
                   });
  //所以只要stop_sign_overlap_it不是参考线上overlap里的最后一个，说明找到了相同id
  //只要下面!=成立，说明参数id可以在参考线信息的overlap里找到，说明这个stopsign的
  //overlap是在参考线信息上的，下面表达式成立就在参考线上，返回true，否则false
  return (stop_sign_overlap_it != stop_sign_overlaps.end());
}

/*
 *  检查一个traffic_light_overlap 是否在参考线上？就是检查参考线上是否有交通灯
 *  Apollo中的overlap表示空间中两个元素的重叠关系
 */
 //参数：reference_line_info 参考线上的信息
 //参数：traffic_light_overlap_id 交通灯overlap的id
bool CheckTrafficLightOnReferenceLine(
    const ReferenceLineInfo& reference_line_info,
    const std::string& traffic_light_overlap_id) {
    
  //先从reference_line_info参考线信息上获取所有的traffic_light的overlap
  const std::vector<PathOverlap>& traffic_light_overlaps =
      reference_line_info.reference_line().map_path().signal_overlaps();
  //遍历参考线线信息里的所有traffic_light的overlap，看有没有id和给定Id相同的
  //令这个id相同的overlap对象赋给traffic_light_overlap_it ，若没找到
  //traffic_light_overlap_it 就会等于交通灯overlap里的最后一个
  auto traffic_light_overlap_it =
      std::find_if(traffic_light_overlaps.begin(), traffic_light_overlaps.end(),
                   [&traffic_light_overlap_id](const PathOverlap& overlap) {
    
                     return overlap.object_id == traffic_light_overlap_id;
                   });
  //所以只要traffic_light_overlap_it不是参考线上overlap里最后一个，说明找到了相同id
  //只要下面!=成立，说明参数id可以在参考线信息的overlap里找到，说明该traffic_light的
  //overlap是在参考线信息上的，下面表达式成立就在参考线上，返回true，否则false
  return (traffic_light_overlap_it != traffic_light_overlaps.end());
}

/*
 * 检查ADC(Automative Driving Car)是否在路口pnc-junction
 */
//参数 reference_line_info参考线信息
bool CheckInsidePnCJunction(const ReferenceLineInfo& reference_line_info) {
    
  //首先从参考线信息上获取自车前端frenet系s坐标，就是自车sl边界的末尾s
  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  //首先从参考线信息上获取自车尾部frenet系s坐标，就是自车sl边界的起始s
  const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();

  //先定义一个PathOverlap类型的pnc_junction_overlap路口overlap对象
  hdmap::PathOverlap pnc_junction_overlap;
  //用自车前端frenet系s坐标去参考先信息上查询是否处于路口内，若是，将所在的路口对象赋给
  //pnc_junction_overlap
  reference_line_info.GetPnCJunction(adc_front_edge_s, &pnc_junction_overlap);
  //若pnc_junction_overlap是空的，说明上一步并不在路口内，找不到，就返回false
  //说明自车不在pnc路口
  if (pnc_junction_overlap.object_id.empty()) {
    
    return false;
  }

  //这里定义了一个常数kIntersectionPassDist 阈值2.0m
  static constexpr double kIntersectionPassDist = 2.0;  // unit: m
  //这里是判断自车在路口里有没有出来，看自车后端对应的frenet系s坐标减去
  //减去pnc路口末尾点在frenet系s坐标，两者之差distance_adc_pass_intersection 
  //若自车后端已经离开路口末尾2.0m以上，则判定自车已经不在路口。否则就在路口。
  const double distance_adc_pass_intersection =
      adc_back_edge_s - pnc_junction_overlap.end_s;
  ADEBUG << "distance_adc_pass_intersection[" << distance_adc_pass_intersection
         << "] pnc_junction_overlap[" << pnc_junction_overlap.object_id
         << "] start_s[" << pnc_junction_overlap.start_s << "]";

  return distance_adc_pass_intersection < kIntersectionPassDist;
}

/*
 * 获取一个路径下的文件名
 */
 //参数 路径path
 //参数 文件名，还是个string的vector,存放指定路径下读取到的多个文件名
 //涉及到boost库的用法
 //给定一个路径把指定路径下的文件的名称都读到vector files里
void GetFilesByPath(const boost::filesystem::path& path,
                    std::vector<std::string>* files) {
    
  ACHECK(files);
  //判断路径是否存在，不存在的话直接返回
  if (!boost::filesystem::exists(path)) {
    
    return;
  }
  //boost::filesystem::is_regular_file 检查文件是否存在
  if (boost::filesystem::is_regular_file(path)) {
    
    AINFO << "Found record file: " << path.c_str();
    //存在的话往files指针里塞path的sting字符串
    files->push_back(path.c_str());
    return;
  }
  //判断path是否为一个文件夹，这是遍历path下的每一个文件/文件夹，
  //都读到vector files里？
  if (boost::filesystem::is_directory(path)) {
    
    for (auto& entry : boost::make_iterator_range(
             boost::filesystem::directory_iterator(path), {
    })) {
    
      GetFilesByPath(entry.path(), files);
    }
  }
}

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