概述

TrajectoryCombiner类是apollo planning模块下modules\planning\lattice\trajectory_generation\trajectory_combiner.cc/.h实现

从类名来看，应该是TrajectoryCombiner横纵向1维轨迹接合器类？

从代码来看TrajectoryCombiner类主要是实现：
结合参考线信息，初始相对时间信息，路径规划/速度规划计算出的横纵向轨迹信息，将其结合成一条离散的规划轨迹(每个轨迹点包含s,v,a,t,x,y,theta,kappa等信息)，其实就是把横纵向轨迹，里的信息提取出来重新组织成一系列的轨迹点数组(一条离散的规划轨迹)。
简而言之，就是完成速度规划和路径规划的结合。

*横向轨迹{(d0,d0’,d0’‘),(d1,d1’,d1’‘)…}
*纵向轨迹{(s0,s0’,s0’‘),(s1,s1’,s1’')…}
*规划轨迹要规划8.0s轨迹，从t=0,0.1,0.2,…7.9,8.0s，按照时间点进行遍历，每个时间点对应输入该类的纵向轨迹里的s(纵向轨迹默认相邻点时间差0.1s?),根据这个s找到输入该类的横向轨迹里的d(横向轨迹默认相邻点纵坐标差1.0m?)，这样就在同一个时间下，把横纵向轨迹匹配到一起了，然后把相应的信息填入待填充的离散轨迹里并返回。

trajectory_combiner.h

#pragma once

#include <vector>

#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/math/curve1d/curve1d.h"

namespace apollo {
namespace planning {

class TrajectoryCombiner {
 public:
 //结合横纵向轨迹的函数Combine
//输入参数参考线(路径点的vector数组)，1维纵向轨迹类的{(t0,s0,v0,a0),(t1,s1,v1,a1),...},1维横向轨迹类的{(s0,d0,d0',d0''),(s1,d1,d1',d1''),...}
//输入参数还有初始的相对时间 init_relative_time
//返回值就是一条离散的轨迹
  static DiscretizedTrajectory Combine(
      const std::vector<common::PathPoint>& reference_line,
      const Curve1d& lon_trajectory, const Curve1d& lat_trajectory,
      const double init_relative_time);
};

}  // namespace planning
}  // namespace apollo

trajectory_combiner.cc

#include "modules/planning/lattice/trajectory_generation/trajectory_combiner.h"

#include <algorithm>

#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/math/path_matcher.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::PathPoint;
using apollo::common::TrajectoryPoint;
using apollo::common::math::CartesianFrenetConverter;
using apollo::common::math::PathMatcher;

//结合横纵向轨迹的函数Combine
//输入参数参考线(路径点的vector数组)，1维纵向轨迹类的{(t0,s0,v0,a0),(t1,s1,v1,a1),...},1维横向轨迹类的{(s0,d0,d0',d0''),(s1,d1,d1',d1''),...}
//输入参数还有初始的相对时间 init_relative_time
//返回值就是一条离散的轨迹
DiscretizedTrajectory TrajectoryCombiner::Combine(
    const std::vector<PathPoint>& reference_line, const Curve1d& lon_trajectory,
    const Curve1d& lat_trajectory, const double init_relative_time) {
  //首先定义一条空的离散轨迹类对象 combined_trajectory
  DiscretizedTrajectory combined_trajectory;

  //获取初始的纵向位置，输入的纵向轨迹0阶插值，取出t=0时对应的s，即s0
  double s0 = lon_trajectory.Evaluate(0, 0.0);
  //获取参考线最后一个路径点的s，参考s的最大值
  double s_ref_max = reference_line.back().s();
  //定义轨迹的累计距离初始为0
  double accumulated_trajectory_s = 0.0;
  //定义一个路径点 先前的轨迹点prev_trajectory_point
  PathPoint prev_trajectory_point;

  //定义上一个s为一个非常小的负数，基本上可以认为就是0
  double last_s = -FLAGS_numerical_epsilon;
  //定义一个时间参数为0
  double t_param = 0.0;
  //如果时间参数小于trajectory_time_length轨迹的时间长度，默认设置为8.0s，apollo规划模块发出的轨迹通常都是8.0s的轨迹，遍历整个轨迹时间8.0s，每次时间参数递增0.1s
  //其实就是按照时间0,0.1,0.2,0.3,...,7.9,8.0s每个时间点重新组织一下横纵下轨迹把他们结合到一起
  while (t_param < FLAGS_trajectory_time_length) {
    // linear extrapolation is handled internally in LatticeTrajectory1d;
    // no worry about t_param > lon_trajectory.ParamLength() situation
    //定义s为纵向轨迹0阶插值时间t_param对应的s
    double s = lon_trajectory.Evaluate(0, t_param);
    //如果上一个点s大于0
    if (last_s > 0.0) {
      //s取为上一个点s, 和当前插值出的s中的较大值
      s = std::max(last_s, s);
    }
    //将当前t_param参数对应的s赋给上一个点的s
    last_s = s;

//计算s'为一个极小值(接近0)，和纵向轨迹t_param参数对应1阶插值出的纵向轨迹的速度
    double s_dot =
        std::max(FLAGS_numerical_epsilon, lon_trajectory.Evaluate(1, t_param));
   //s''同样也是输入的总下个轨迹的二阶导在t_param参数处插值的结果
    double s_ddot = lon_trajectory.Evaluate(2, t_param);
   //如果当前t_param在纵向轨迹上插值处的s大于输入参考线的最大s，那么直接break,终止这个while循环
    if (s > s_ref_max) {
      break;
    }

//计算相对s，就是相对时间t=0的轨迹点s的纵向长度
    double relative_s = s - s0;
    // linear extrapolation is handled internally in LatticeTrajectory1d;
    // no worry about s_param > lat_trajectory.ParamLength() situation
    //根据输入的横向轨迹，分别对其及其一阶导，二阶导插值出t_param时间参数处对应的横向偏移，横向偏移一阶导，二阶导
    double d = lat_trajectory.Evaluate(0, relative_s);
    double d_prime = lat_trajectory.Evaluate(1, relative_s);
    double d_pprime = lat_trajectory.Evaluate(2, relative_s);

    //获取当前遍历的时间参数t_param对应的s在输入的参考线上对应的匹配路径点
    PathPoint matched_ref_point = PathMatcher::MatchToPath(reference_line, s);

//初始定义x,y,theta,kappa,v,a都为0
    double x = 0.0;
    double y = 0.0;
    double theta = 0.0;
    double kappa = 0.0;
    double v = 0.0;
    double a = 0.0;

//定义参考点的s,x,y,theta,kappa,dkappa都为参考线上匹配路径点对应的s,x,y,theta,kappa,dkappa，这些都是参考值
    const double rs = matched_ref_point.s();
    const double rx = matched_ref_point.x();
    const double ry = matched_ref_point.y();
    const double rtheta = matched_ref_point.theta();
    const double rkappa = matched_ref_point.kappa();
    const double rdkappa = matched_ref_point.dkappa();

//定义纵向s条件，参考纵向坐标rs，当前遍历的时间对应的s',s''
    std::array<double, 3> s_conditions = {rs, s_dot, s_ddot};
    //定义横向d条件 ，当前遍历时间对应的d,d',d''
    std::array<double, 3> d_conditions = {d, d_prime, d_pprime};
    //找到参考点xy（匹配的最近点），将frenet系下的横纵向条件及坐标(s,s',s'',d,d',d'')转化为笛卡尔坐标系下(x,y,theta,kappa,v,a)
    CartesianFrenetConverter::frenet_to_cartesian(
        rs, rx, ry, rtheta, rkappa, rdkappa, s_conditions, d_conditions, &x, &y,
        &theta, &kappa, &v, &a);

	//如果前一个点有xy坐标，计算当前循环遍历的时间路径点xy坐标，纵向s坐标相对于上一个循环遍历的时间点的xy增量，s增量，同时计算加上当前循环遍历的时间点后路径长度更新值accumulated_trajectory_s 。计算当前循环遍历的时间路径点的s坐标，也就是纵向累计长度
    if (prev_trajectory_point.has_x() && prev_trajectory_point.has_y()) {
      double delta_x = x - prev_trajectory_point.x();
      double delta_y = y - prev_trajectory_point.y();
      double delta_s = std::hypot(delta_x, delta_y);
      accumulated_trajectory_s += delta_s;
    }

//定义一个空的轨迹点，上面应该是先用一个while循环挨个遍历规划轨迹0-8.0s的每个离散时间，每个离散时间去纵向轨迹上找到对应的s，对应的s去参考线上找到匹配的参考点获取参考点的x,y,theta,kappa,dkappa等，然后通过s坐标去横纵向轨迹上获取横纵向在该离散时间处的终端约束(s,s',s'',d,d',d'')然后全部转化到笛卡尔坐标系下实际规划轨迹点的x,y,theta,kappa,dkappa，s,v,a,t等全部塞入这个空的轨迹点
    TrajectoryPoint trajectory_point;
    trajectory_point.mutable_path_point()->set_x(x);
    trajectory_point.mutable_path_point()->set_y(y);
    trajectory_point.mutable_path_point()->set_s(accumulated_trajectory_s);
    trajectory_point.mutable_path_point()->set_theta(theta);
    trajectory_point.mutable_path_point()->set_kappa(kappa);
    trajectory_point.set_v(v);
    trajectory_point.set_a(a);
    trajectory_point.set_relative_time(t_param + init_relative_time);

//然后在待填充的轨迹对象combined_trajectory塞入上述轨迹点，遍历完0-8s后，整个规划轨迹就全部赛完了
    combined_trajectory.AppendTrajectoryPoint(trajectory_point);

//为了下一次while循环能遍历到下一个离散的时间点，时间参数t_param递增0.1s
    t_param = t_param + FLAGS_trajectory_time_resolution;
//这一时间对应的路径点复制给上一个路径点，在下一个while循环时，又可以计算相邻两个时间对应路径点间s,x,y等变量的增量。
    prev_trajectory_point = trajectory_point.path_point();
  }
  //返回这个结合好的轨迹
  return combined_trajectory;
}

}  // namespace planning
}  // namespace apollo