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

概述

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

从类名来看，应该是轨迹缝合类？

从代码来看TrajectoryStitcher类主要是实现：
1.触发轨迹重规划replan时以车辆当前状态/运动学递推0.1s后的状态作为下一段规划起始点；replan时返回的轨迹只有一个起始点？
2.没触发规划轨迹replan时，进行规划轨迹缝合，截取缝合轨迹起点以车辆当前状态在上一帧规划轨迹匹配点(时间，距离匹配点中较小的一个)往回退20个点，截取缝合轨迹终点为到当前时间戳+0.1s在上一帧轨迹上对应点。然后将当前时间戳作为下一段规划轨迹的相对时间为0的点，将当前时间戳往后推0.1s的点对应的s作为下一段规划轨迹的纵坐标起点s=0的点，根据新的时间起点和纵坐标起点更新缝合轨迹。

简而言之，该类就是一是实现特定条件下触发轨迹重规划replan(以车辆当前状态出发进行规划)；二是没有触发replan时进行轨迹缝合(其实也是依赖车辆当前状态去拼接上一帧规划轨迹和下一帧规划轨迹，在上一帧轨迹中找到车辆当前状态最接近的点并在其附近截断，下一帧规划轨迹又以车辆当前状态出发继续规划，车辆当前状态就将上一帧规划轨迹和下一帧规划轨迹连接起来了)，这样规划轨迹始终是比较连续的？

*触发规划轨迹重规划replan的条件：
a.关闭轨迹缝合时；
b.上一帧规划轨迹为空时；
c.车辆不处于完全自动驾驶时；
d.上一帧规划轨迹点数量为0时；
e.当前时间戳小于上一帧轨迹的起点的时间；
f.当前时间戳超出了上一帧轨迹的最后一个点的时间；
g.先前的轨迹无路径点；
h.横向误差超过0.5m 或纵向误差超过2.5m

trajectory_stitcher.h

#pragma once

#include 
#include 
#include 

#include "modules/common/proto/pnc_point.pb.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/planning/common/trajectory/publishable_trajectory.h"
#include "modules/planning/reference_line/reference_line.h"

namespace apollo {
namespace planning {

class TrajectoryStitcher {
 public:
 //禁用默认构造函数
  TrajectoryStitcher() = delete;

// only used in navigation mode
// 仅仅在导航模式下使用，导航模式是事先录制一条轨迹作为参考线？
// 默认是不使用导航模式的，跳过这段
  static void TransformLastPublishedTrajectory(
      const double x_diff, const double y_diff, const double theta_diff,
      PublishableTrajectory* prev_trajectory);

/* 从车辆当前状态开始规划，当：
   1. 自驾模式关闭时
   (或) 2. 我们没有上个周期的规划轨迹时
   (或) 3. 实际和目标的位置偏差太大
*/
//计算缝合轨迹函数
//输入参数：车辆状态vehicle_state
//输入参数：当前的时间戳current_timestamp
//输入参数：规划周期planning_cycle_time
//输入参数：preserved_points_num 保留的点的数量？
//输入参数：replan_by_offset 布尔量，是否因为offset而replan?
//输入参数：prev_trajectory先前的轨迹？
//输入参数: replan_reason重规划原因？存放函数执行后的结果，设置重规划原因
//返回值：返回的是轨迹点类的vector数组
//如果触发replan的条件就返回replan的轨迹(里面就一个点车辆当前状态轨迹点/或运动学推导0.1s后对应车辆状态的轨迹点)
//如果没触发replan就进行轨迹缝合，从上一次的规划轨迹中从车辆当前状态时间最近点/距离最近点(取较小的那个)往前截取20个点开始，到(当前时间戳+规划周期0.1s在上一段规划轨迹对应的点)，这一段轨迹作为缝合轨迹，这个缝合轨迹的最后一个点作为下一段规划轨迹的纵向起点s=0的点，时间戳以当前时间戳作为下段规划轨迹时间起点
  static std::vector<common::TrajectoryPoint> ComputeStitchingTrajectory(
      const common::VehicleState& vehicle_state, const double current_timestamp,
      const double planning_cycle_time, const size_t preserved_points_num,
      const bool replan_by_offset, const PublishableTrajectory* prev_trajectory,
      std::string* replan_reason);

//计算重新初始缝合轨迹函数？这个函数就是如果车速加速度够小，以当前状态为0.1s后规划轨迹点；若车速和加速度不够小就以运动学模型预测0.1s后的车辆状态作为规划轨迹的0.1s处的点
//这个0.1s处的点就是replan时的缝合点？
//输入参数：planning_cycle_time规划周期，apollo里为0.1s
//输入参数：vehicle_state车辆状态
//返回值：是一个轨迹点的vector数组
//其实就是计算replan轨迹？但是最后返回的replan轨迹里只有一个点？
  static std::vector<common::TrajectoryPoint> ComputeReinitStitchingTrajectory(
      const double planning_cycle_time,
      const common::VehicleState& vehicle_state);

 private:
 //计算位置投影？
//输入参数：x,y,还有一个轨迹点p?
//其实就是将一个x,y坐标投影到轨迹点p上转化为sd坐标，s代表纵向，d代表横向
  static std::pair<double, double> ComputePositionProjection(
      const double x, const double y,
      const common::TrajectoryPoint& matched_trajectory_point);

//从车辆状态计算轨迹点函数？
//输入参数：planning_cycle_time规划周期，apollo里面一般是0.1s
//输入参数：vehicle_state车辆状态
//其实该函数就是把车辆状态当前的s,x,y,z,theta,kappa,v,a设置到一个轨迹点，并将车辆当前状态的这个轨迹点相对时间设置为0.1s(考虑的规划执行周期)，然后返回这个轨迹点(s,x,y,z,theta,kappa,v,a,relative_time=0.1s)
  static common::TrajectoryPoint ComputeTrajectoryPointFromVehicleState(
      const double planning_cycle_time,
      const common::VehicleState& vehicle_state);
};

}  // namespace planning
}  // namespace apollo

trajectory_stitcher.cc

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

#include 

#include "absl/strings/str_cat.h"

#include "cyber/common/log.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/math/angle.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/util.h"
#include "modules/common/vehicle_model/vehicle_model.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleModel;
using apollo::common::VehicleState;
using apollo::common::math::Vec2d;

//从车辆状态计算轨迹点函数？
//输入参数：planning_cycle_time规划周期，apollo里面一般是0.1s
//输入参数：vehicle_state车辆状态
//其实该函数就是把车辆状态当前的s,x,y,z,theta,kappa,v,a设置到一个轨迹点，并将车辆当前状态的这个轨迹点相对时间设置为0.1s(考虑的规划执行周期)，然后返回这个轨迹点(s,x,y,z,theta,kappa,v,a,relative_time=0.1s)
TrajectoryPoint TrajectoryStitcher::ComputeTrajectoryPointFromVehicleState(
    const double planning_cycle_time, const VehicleState& vehicle_state) {
  //初始定义个空的轨迹点point
  TrajectoryPoint point; 
  //设置轨迹点point里路径点纵向坐标s为0.0，可以理解为是把车辆当前状态当成轨迹起始点s=0处
  point.mutable_path_point()->set_s(0.0);
  //设置轨迹点point里路径点的x,y,z,theta,kappa信息为车辆的当前状态
  point.mutable_path_point()->set_x(vehicle_state.x());
  point.mutable_path_point()->set_y(vehicle_state.y());
  point.mutable_path_point()->set_z(vehicle_state.z());
  point.mutable_path_point()->set_theta(vehicle_state.heading());
  //vehcile_state的kappa需要看相关代码 由 dpsi/v估算
  point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  //设置轨迹点point里的v,a,relative_time为0.1s，考虑到规划执行的时间，把当前状态往前提0.1s
  point.set_v(vehicle_state.linear_velocity());
  point.set_a(vehicle_state.linear_acceleration());
  point.set_relative_time(planning_cycle_time);
  //返回这个设置的轨迹点
  return point;
}

//计算重新初始缝合轨迹函数？这个函数就是如果车速加速度够小，以当前状态为0.1s后规划轨迹点；若车速和加速度不够小就以运动学模型预测0.1s后的车辆状态作为规划轨迹的0.1s处的点
//这个0.1s处的点就是replan时的缝合点？
//输入参数：planning_cycle_time规划周期，apollo里为0.1s
//输入参数：vehicle_state车辆状态
//返回值：是一个轨迹点的vector数组
//其实就是计算replan轨迹？但是最后返回的replan轨迹里只有一个点？
std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const double planning_cycle_time, const VehicleState& vehicle_state) {
  //定义一个重新初始轨迹点reinit_point
  TrajectoryPoint reinit_point;
  //定义了两个速度和加速度的阈值常数kEpsilon_v  0.1m/s  kEpsilon_a 0.4m/s^2
  static constexpr double kEpsilon_v = 0.1;
  static constexpr double kEpsilon_a = 0.4;
  //Apollo原注释，需要调整常数阈值当修正后的IMU加速度被提供？
  // TODO(Jinyun/Yu): adjust kEpsilon if corrected IMU acceleration provided
  //如果车纵向速度的绝对值<0.1m/s^2 且 车辆纵向加速度绝对值 < 0.4m/s^2
  //那么就调用上面的ComputeTrajectoryPointFromVehicleState()函数，把车辆当前状态作为0.1s后的轨迹点reinit_point 
  //意思就是车速/加速度若很小时，直接将车辆当前状态的点作为缝合后轨迹相对时间0.1s的点
  if (std::abs(vehicle_state.linear_velocity()) < kEpsilon_v &&
      std::abs(vehicle_state.linear_acceleration()) < kEpsilon_a) {
    reinit_point = ComputeTrajectoryPointFromVehicleState(planning_cycle_time,
                                                          vehicle_state);
  } else {
  //那么如果车速和加速度不够小时，预测车辆的状态predicted_vehicle_state
    VehicleState predicted_vehicle_state;
    //根据车辆运动学模型预测车辆一个规划周期0.1s后的车辆状态
    predicted_vehicle_state =
        VehicleModel::Predict(planning_cycle_time, vehicle_state);
    //以这个预测的车辆状态点去重新计算 重新初始化点reinit_point 
    //就是将运动学模型预测的点作为相对时间0.1s后的点，然后再计算车辆规划轨迹
    reinit_point = ComputeTrajectoryPointFromVehicleState(
        planning_cycle_time, predicted_vehicle_state);
  }

//返回的轨迹里就1个这个0.1s处1的这个缝合点？
  return std::vector<TrajectoryPoint>(1, reinit_point);
}

// only used in navigation mode
// 仅仅在导航模式下使用，导航模式是事先录制一条轨迹作为参考线？
// 默认是不使用导航模式的，跳过这段
void TrajectoryStitcher::TransformLastPublishedTrajectory(
    const double x_diff, const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {
  if (!prev_trajectory) {
    return;
  }

  // R^-1
  double cos_theta = std::cos(theta_diff);
  double sin_theta = -std::sin(theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  std::for_each(prev_trajectory->begin(), prev_trajectory->end(),
                [&cos_theta, &sin_theta, &tx, &ty,
                 &theta_diff](common::TrajectoryPoint& p) {
                  auto x = p.path_point().x();
                  auto y = p.path_point().y();
                  auto theta = p.path_point().theta();

                  auto x_new = cos_theta * x - sin_theta * y + tx;
                  auto y_new = sin_theta * x + cos_theta * y + ty;
                  auto theta_new =
                      common::math::NormalizeAngle(theta - theta_diff);

                  p.mutable_path_point()->set_x(x_new);
                  p.mutable_path_point()->set_y(y_new);
                  p.mutable_path_point()->set_theta(theta_new);
                });
}

/* 从车辆当前状态开始规划，当：
   1. 自驾模式关闭时
   (或) 2. 我们没有上个周期的规划轨迹时
   (或) 3. 实际和目标的位置偏差太大
*/
//计算缝合轨迹函数
//输入参数：车辆状态vehicle_state
//输入参数：当前的时间戳current_timestamp
//输入参数：规划周期planning_cycle_time
//输入参数：preserved_points_num 保留的点的数量？
//输入参数：replan_by_offset 布尔量，是否因为offset而replan?
//输入参数：prev_trajectory先前的轨迹？
//输入参数: replan_reason重规划原因？存放函数执行后的结果，设置重规划原因
//返回值：返回的是轨迹点类的vector数组
//如果触发replan的条件就返回replan的轨迹(里面就一个点车辆当前状态轨迹点/或运动学推导0.1s后对应车辆状态的轨迹点)
//如果没触发replan就进行轨迹缝合，从上一次的规划轨迹中从车辆当前状态时间最近点/距离最近点(取较小的那个)往前截取20个点开始，到(当前时间戳+规划周期0.1s在上一段规划轨迹对应的点)，这一段轨迹作为缝合轨迹，这个缝合轨迹的最后一个点作为下一段规划轨迹的纵向起点s=0的点，时间戳以当前时间戳作为下段规划轨迹时间起点
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time, const size_t preserved_points_num,
    const bool replan_by_offset, const PublishableTrajectory* prev_trajectory,
    std::string* replan_reason) {
  //enable_trajectory_stitcher在planning_gflags.cc里定义为true，默认打开轨迹缝合使能
  if (!FLAGS_enable_trajectory_stitcher) { //不打开才执行这段
    *replan_reason = "stitch is disabled by gflag.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }
  //如果先前的规划轨迹为空指针
  if (!prev_trajectory) {
  //重规划原因设置为 "无先前的规划轨迹"
    *replan_reason = "replan for no previous trajectory.";
    //返回的就是replan后的轨迹 ，里面目前只有一个0.1s后的replan 缝合点(车辆当前状态点/或运动学模型预测点)？
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

//如果车辆状态不为智驾模式，报replan的原因，同时返回的就是replan后的轨迹 ，里面目前只有一个0.1s后的replan 缝合点(车辆当前状态点/或运动学模型预测点)？
  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    *replan_reason = "replan for manual mode.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

//先前的轨迹点的数量
  size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

//先前的轨迹点的数量为0，报replan的原因，同时返回的就是replan后的轨迹 ，里面目前只有一个0.1s后的replan 缝合点(车辆当前状态点/或运动学模型预测点)？
  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    *replan_reason = "replan for empty previous trajectory.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

//获取当前时间戳于相对上一段轨迹的header时间的相对时间，当前的时间戳-先前轨迹的Header时间戳
  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

//获取匹配的时间下标，把当前时间相对上一段轨迹的header时间去上一段轨迹上找到时间匹配点下标
  size_t time_matched_index =
      prev_trajectory->QueryLowerBoundPoint(veh_rel_time);

//如果当前时间戳在先前轨迹的时间匹配点下标为0 且 /获取当前时间戳于相对上一段轨迹的header时间的相对时间小于先前轨迹的第一个轨迹点的相对时间，当前相对时间比上一段轨迹起始时间还小？，报警告信息，报replan的原因，同时返回的就是replan后的轨迹 ，里面目前只有一个0.1s后的replan 缝合点(车辆当前状态点/或运动学模型预测点)？
  if (time_matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    *replan_reason =
        "replan for current time smaller than the previous trajectory's first "
        "time.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }
  //如果当前时间戳在先前轨迹的时间匹配点下标 + 1甚至超过了上一段规划轨迹的里面的轨迹点的个数，报警告，设置replan原因，同时返回的就是replan后的轨迹 ，里面目前只有一个0.1s后的replan 缝合点(车辆当前状态点/或运动学模型预测点)？
  if (time_matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    *replan_reason =
        "replan for current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

//获取当前时间戳在先前轨迹上的时间匹配点time_matched_point 
  auto time_matched_point = prev_trajectory->TrajectoryPointAt(
      static_cast<uint32_t>(time_matched_index));

//如果时间匹配点在上一段轨迹上没有路径点，设置replan原因，同时返回的就是replan后的轨迹 ，里面目前只有一个0.1s后的replan 缝合点(车辆当前状态点/或运动学模型预测点)？
  if (!time_matched_point.has_path_point()) {
    *replan_reason = "replan for previous trajectory missed path point";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

//获取车辆当前状态在上一段轨迹上的距离最近点 position_matched_index 
  size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer(
      {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);
//获取车辆当前状态在上一段轨迹上的距离最近点 position_matched_index转化为fenet系下的坐标frenet_sd 也就是sd坐标(横，纵向坐标)
  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(),
      prev_trajectory->TrajectoryPointAt(
          static_cast<uint32_t>(position_matched_index)));

//如果因为offset导致replan，也就是因为横纵向误差导致replan
  if (replan_by_offset) {
    //纵向误差=时间匹配点纵向坐标-当前车辆状态s坐标
    auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;
    //横向误差=车辆当前状态sd坐标里的d坐标，也就是相对于参考线的横向坐标
    auto lat_diff = frenet_sd.second;

//打印横纵向误差用于debug
    ADEBUG << "Control lateral diff: " << lat_diff
           << ", longitudinal diff: " << lon_diff;

//如果横向误差绝对值超过设定的阈值，replan_lateral_distance_threshold在planning_gflags.cc里设定为0.5m，那么就进行replan，调用ComputeReinitStitchingTrajectory，从车辆当前状态出发重新规划轨迹，但目前这个返回的轨迹里面只有一个车辆当前状态(或者用运动学推算0.1s后的状态)轨迹点？
    if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold) {
      const std::string msg = absl::StrCat(
          "the distance between matched point and actual position is too "
          "large. Replan is triggered. lat_diff = ",
          lat_diff);
      AERROR << msg;
      *replan_reason = msg;
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }

//如果纵向误差绝对值超过设定的阈值，replan_longitudinal_distance_threshold在planning_gflags.cc里设定为2.5m，那么就进行replan，调用ComputeReinitStitchingTrajectory，从车辆当前状态出发重新规划轨迹，但目前这个返回的轨迹里面只有一个车辆当前状态(或者用运动学推算0.1s后的状态)轨迹点？
    if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) {
      const std::string msg = absl::StrCat(
          "the distance between matched point and actual position is too "
          "large. Replan is triggered. lon_diff = ",
          lon_diff);
      AERROR << msg;
      *replan_reason = msg;
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }
  } else {
    ADEBUG << "replan according to certain amount of lat and lon offset is "
              "disabled";
  }

//定义一个向前的相对时间forward_rel_time 
//车辆当前的相对时间向前推一个规划周期0.1s后的相对时间,也是相对于上一段轨迹headertime的相对时间
  double forward_rel_time = veh_rel_time + planning_cycle_time;

//向前时间小标，其实就是找到上一段轨迹上相对时间forward_rel_time的时间最近点
  size_t forward_time_index =
      prev_trajectory->QueryLowerBoundPoint(forward_rel_time);

//打印一些debug信息
  ADEBUG << "Position matched index:\t" << position_matched_index;
  ADEBUG << "Time matched index:\t" << time_matched_index;

//当前时间在上一段轨迹上的时间最近点下标，当前车辆状态在上一段轨迹上的距离最近点下标
//matched_index =时间最近点下标和距离最近点下标里更小的那个
  auto matched_index = std::min(time_matched_index, position_matched_index);
//定义一个轨迹点的vector数组对象stitching_trajectory也就是缝合轨迹
//matched_index 就是车辆当前状态在上一段规划轨迹里的时间最近点下标和距离最近点下标中的较小值
//preserved_points_num保留的点的个数？
//forward_rel_time 车辆当前的相对时间向前推一个规划周期0.1s后的相对时间，也是相对于上一段轨迹headertime的相对时间
//preserved_points_num是来源于这个函数的输入，on_lane_planning.cc里调用ComputeStitchingTrajectory该函数时，该值被planning_gflags.cc里定义为trajectory_stitching_preserved_length 20，所以replan时，默认保留之前的20个点？
//没太看明白
//在上一帧的轨迹prev_trajectory上起点截取出往前20个点preserved_points_num开始，至forward_rel_time的一段轨迹，作为
//stitching_trajectory。最后，遍历这段轨迹，对其relative_time和s进行赋值。最终，使
//用stitching_trajectory的最后一个点，作为当前帧轨迹规划的起始点。
  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->begin() +
          std::max(0, static_cast<int>(matched_index - preserved_points_num)),
      prev_trajectory->begin() + forward_time_index + 1);
  ADEBUG << "stitching_trajectory size: " << stitching_trajectory.size();
//matched_index - preserved_points_num代表车辆状态在上一段轨迹上的时间最近点/距离最近点(选小的一个)往回找20个点开始，到当前时间戳在上一段轨迹上往后推0.1s的点作为stitching_trajectory
//定义 zero_s为缝合轨迹stitching_trajectory里的最后一个点对应的路径点的s，作为下一次规划轨迹的纵向坐标的起点
  const double zero_s = stitching_trajectory.back().path_point().s();
  //遍历缝合轨迹上的每个轨迹点，若有轨迹点无路径点又replan并且返回replan轨迹
  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      *replan_reason = "replan for previous trajectory missed path point";
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }
    //设置缝合轨迹上轨迹点相对时间=缝合轨迹点的相对时间 + 上一段轨迹的header时间，不就得到绝对时间吗，然后减去当前的时间戳，其实就是以当前时间戳为起点开始计算相对时间？
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    //设置缝合轨迹上轨迹点tp的路径点的s为 上一段的s-zero_s，就转换为相对于zero_s的相对纵坐标
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  //最后返回这个缝合的轨迹
  return stitching_trajectory;
}

//计算位置投影？
//输入参数：x,y,还有一个轨迹点p?
//其实就是将一个x,y坐标投影到轨迹点p上转化为sd坐标，s代表纵向，d代表横向
//其实上面调用的时候就是将车辆当前状态的x,y坐标，投影到轨迹上距离最近点处的横纵向相对坐标？但是返回的,返回的d就是当前的横向误差，s还会叠加上最近点s转化为纵向坐标s（其与时间最近点的偏差即为纵向误差）
std::pair<double, double> TrajectoryStitcher::ComputePositionProjection(
    const double x, const double y, const TrajectoryPoint& p) {
  Vec2d v(x - p.path_point().x(), y - p.path_point().y());
  Vec2d n(std::cos(p.path_point().theta()), std::sin(p.path_point().theta()));

  std::pair<double, double> frenet_sd;
  frenet_sd.first = v.InnerProd(n) + p.path_point().s();
  frenet_sd.second = v.CrossProd(n);
  return frenet_sd;
}

}  // namespace planning
}  // namespace apollo