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

这一块代码作用没有看的很明白，后续planning模块若有涉及再结合其他代码一起思考。

Smoother类就是当足够接近停止点10m内，规划轨迹就保持上一帧的数据？来达到平滑的效果？

modules\planning\common\smoothers\smoother.h

#pragma once

#include "modules/common/status/status.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/proto/decision.pb.h"

namespace apollo {
namespace planning {

class Smoother { //Smoother类顾名思义平滑器类？
 public:
  //默认构造函数及默认析构函数
  Smoother() = default;  
  virtual ~Smoother() = default;

  //核心函数Smooth，输入参数frame_history，current_frame，current_trajectory_pb
  //current_frame:Frame帧类变量, Frame帧类包含一个规划周期规划所有相关数据
  //frame_history:FrameHistory帧历史类变量，就是Frame类的队列，所有历史周期的规划相关数据
  //Frame类型包含哪些内容？参见modules\planning\common\frame.h
  //Frame类型包含规划sequence_num轨迹段数？local_view规划模块的全部输入，planning_start_point
  //规划起始点，vehicle_state车辆状态，reference_line_provider道路参考线
  //current_trajectory_pb用来存放平滑后的结果
  apollo::common::Status Smooth(const FrameHistory* frame_history,
                                const Frame* current_frame,
                                ADCTrajectory* const current_trajectory_pb);

 private:
  //是否足够接近停止点？
  bool IsCloseStop(const common::VehicleState& vehicle_state,
                   const MainStop& main_stop);
};

}  // namespace planning
}  // namespace apollo

modules\planning\common\smoothers\smoother.cc 

#include "modules/planning/common/smoothers/smoother.h"

#include <string>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/vec2d.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::math::Vec2d; //vec2d就是apollo定义的二维向量数据结构(x,y)

//该函数就是判断车辆当前位置到主停止点位置是否已经小于平滑器所需最小距离
//是否接近停止？输入参数车辆状态，
//MainStop类参见modules\planning\proto\decision.proto
//main_stop就是车辆的主停止点？MainStop类是主停止点的数据结构，包含停止点，停止原因航向等
bool Smoother::IsCloseStop(const common::VehicleState& vehicle_state,
                           const MainStop& main_stop) {
  //判断main_stop是否为空
  if (!main_stop.has_stop_point()) {
    ADEBUG << "not close for main stop:" << main_stop.DebugString();
    return false;
  }
  //车辆当前位置(x,y)去初始化Vec2d类对象current_car_pos
  Vec2d current_car_pos(vehicle_state.x(), vehicle_state.y());
  //车辆main_stop主停止点位置(x,y)去初始化Vec2d类对象stop_pos
  Vec2d stop_pos(main_stop.stop_point().x(), main_stop.stop_point().y());
  //距停止点的距离stop_distance 就是车辆当前位置到主停止点的位置的距离
  //调用vec2d的成员函数.DistanceTo()计算二维向量之间的距离
  auto stop_distance = stop_pos.DistanceTo(current_car_pos);
  //如果车辆当前位置到主停止点的距离大于平滑最小距离smoother_stop_distance
  //FLAGS_是gflag库用法去apollo\modules\planning\common\planning_gflags.cc取出
  //smoother_stop_distance的值，默认为10m
  //也就是说如果车辆当前位置到主停止点的距离大于10m那么就返回false否则返回true
  if (stop_distance > FLAGS_smoother_stop_distance) {
    ADEBUG << "distance between ADC position and stop position:"
           << stop_distance;
    return false;
  }
  return true;
}

//平滑主函数，输入参数规划周期的历史数据frame_history，当前帧规划所有数据current_frame，平滑结果
//放入current_trajectory_pb
apollo::common::Status Smoother::Smooth(
    const FrameHistory* frame_history, const Frame* current_frame,
    ADCTrajectory* const current_trajectory_pb) {
  //如果规划历史周期数据为空，报error
  if (frame_history == nullptr) {
    const std::string msg = "frame history is null.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  //如果规划模块当前周期所有相关数据为空，报error
  if (current_frame == nullptr) {
    const std::string msg = "frame is null.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  //如果平滑前的轨迹指针为空，报error
  if (current_trajectory_pb == nullptr) {
    const std::string msg = "current trajectory pb is null";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  //主决策main_decision就是当前轨迹访问成员->decision().main_decision()
  //decision主要决策是换道，停车，巡航等行为决策
  //decision详细定义参见modules\planning\proto\decision.proto
  const auto& main_decision = current_trajectory_pb->decision().main_decision();
  //如果主决策没有停车就跳过平滑？
  if (!main_decision.has_stop()) {
    // skip for current planning is not stop.
    ADEBUG << "skip smoothing for current planning is not stop";
    return Status::OK();
  }
  
  //车辆当前状态vehicle_state通过当前帧frame去获得
  const auto& vehicle_state = current_frame->vehicle_state();
  //最大停车速度，即速度小于该值认为车辆已经停住了，实际上是访问配置文件中
  //max_abs_speed_when_stopped，去vehicle_param里modules\common\data\vehicle_param.pb.txt
  //找出max_abs_speed_when_stopped的值，默认为0.2m/s
  const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()
                                        ->GetConfig()
                                        .vehicle_param()
                                        .max_abs_speed_when_stopped();
  //如果车辆当前速度大于停车速度
  //也跳过平滑？
  if (vehicle_state.linear_velocity() > max_adc_stop_speed) {
    ADEBUG << "vehicle speed:" << vehicle_state.linear_velocity()
           << " skip smoothing for non-stop scenario";
    return Status::OK();
  }

  //如果车辆当前位置距离主停车位置不是足够近的话，也跳过平滑？
  if (!IsCloseStop(vehicle_state, main_decision.stop())) {
    ADEBUG << "vehicle state:" << vehicle_state.DebugString()
           << " skip smoothing for ADC is not close to stop point";
    return Status::OK();
  }

  //获取前一帧的规划相关所有数据
  auto previous_frame = frame_history->Latest();
  //如果前一帧规划数据为空。返回警告信息
  if (previous_frame == nullptr) {
    const std::string msg = "previous frame is null";
    AWARN << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  //将之前一帧的规划结果轨迹也就是当前的规划轨迹存放到previous_planning 
  const auto& previous_planning =
      previous_frame->current_frame_planned_trajectory();
  //获取当前轨迹的头部header——包含标题时间戳等?
  auto header = current_trajectory_pb->header();
  //当前的规划轨迹存放到current_trajectory_pb 轨迹指针
  *current_trajectory_pb = previous_planning;
  //将header又copy进去？不是就从里面拿的吗？
  current_trajectory_pb->mutable_header()->CopyFrom(header);
  //smoother_debug为当前规划轨迹的debug下的planning_data下的smoother
  auto smoother_debug = current_trajectory_pb->mutable_debug()
                            ->mutable_planning_data()
                            ->mutable_smoother();
  //设置smoother_debug下的is_smoothed设置为true
  smoother_debug->set_is_smoothed(true);
  //设置smoother_debug下的type设置为SMOOTHER_CLOSE_STOP
  smoother_debug->set_type(
      planning_internal::SmootherDebug::SMOOTHER_CLOSE_STOP);
  return Status::OK();
}

}  // namespace planning
}  // namespace apollo