apollo::planning::TrajectoryStitcher类 参考

#include <trajectory_stitcher.h>

apollo::planning::TrajectoryStitcher 的协作图:

Public 成员函数
	TrajectoryStitcher ()=delete

静态 Public 成员函数
static void	TransformLastPublishedTrajectory (const double x_diff, const double y_diff, const double theta_diff, PublishableTrajectory *prev_trajectory)
static std::vector< common::TrajectoryPoint >	ComputeStitchingTrajectory (const canbus::Chassis &vehicle_chassis, 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, const control::ControlInteractiveMsg &control_interactive_msg)
static std::vector< common::TrajectoryPoint >	ComputeReinitStitchingTrajectory (const double planning_cycle_time, const common::VehicleState &vehicle_state)
static bool	need_replan_by_necessary_check (const common::VehicleState &vehicle_state, const double current_timestamp, const PublishableTrajectory *prev_trajectory, std::string *replan_reason, size_t *time_matched_index)
static bool	need_replan_by_control_interactive (const double current_timestamp, std::string *replan_reason, const control::ControlInteractiveMsg &control_interactive_msg)
static std::vector< common::TrajectoryPoint >	ComputeControlInteractiveStitchingTrajectory (const double planning_cycle_time, const common::VehicleState &vehicle_state, const common::TrajectoryPoint &time_match_point, const control::ControlInteractiveMsg &control_interactive_msg)

详细描述

在文件 trajectory_stitcher.h 第 37 行定义.

构造及析构函数说明

TrajectoryStitcher()

apollo::planning::TrajectoryStitcher::TrajectoryStitcher ( )

delete

成员函数说明

ComputeControlInteractiveStitchingTrajectory()

std::vector< common::TrajectoryPoint > apollo::planning::TrajectoryStitcher::ComputeControlInteractiveStitchingTrajectory ( const double  planning_cycle_time, const common::VehicleState &  vehicle_state, const common::TrajectoryPoint &  time_match_point, const control::ControlInteractiveMsg &  control_interactive_msg  )

static

在文件 trajectory_stitcher.cc 第 357 行定义.

                                                                 {
  if (control_interactive_msg.replan_req_reason_code() ==
          control::ReplanRequestReasonCode::REPLAN_REQ_ALL_REPLAN ||
      control_interactive_msg.replan_req_reason_code() ==
          control::ReplanRequestReasonCode::REPLAN_REQ_STATION_REPLAN) {
    AINFO << "control_interactive_msg replan, all replan";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  } else {
    AINFO << "control_interactive_msg replan, speed replan";
    VehicleState vehicle_state_tmp = vehicle_state;
    vehicle_state_tmp.set_x(time_match_point.path_point().x());
    vehicle_state_tmp.set_y(time_match_point.path_point().y());
    vehicle_state_tmp.set_z(time_match_point.path_point().z());
    vehicle_state_tmp.set_heading(time_match_point.path_point().theta());
    vehicle_state_tmp.set_kappa(time_match_point.path_point().kappa());
    return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                            vehicle_state_tmp);
  }
}

ComputeReinitStitchingTrajectory()

std::vector< TrajectoryPoint > apollo::planning::TrajectoryStitcher::ComputeReinitStitchingTrajectory ( const double  planning_cycle_time, const common::VehicleState &  vehicle_state  )

static

在文件 trajectory_stitcher.cc 第 59 行定义.

                                                                         {
  TrajectoryPoint reinit_point;
  static constexpr double kEpsilon_v = 0.1;
  static constexpr double kEpsilon_a = 0.4;
  // TODO(Jinyun/Yu): adjust kEpsilon if corrected IMU acceleration provided
  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 {
    VehicleState predicted_vehicle_state;
    predicted_vehicle_state =
        VehicleModel::Predict(planning_cycle_time, vehicle_state);
    reinit_point = ComputeTrajectoryPointFromVehicleState(
        planning_cycle_time, predicted_vehicle_state);
  }
 
  return std::vector<TrajectoryPoint>(1, reinit_point);
}

ComputeStitchingTrajectory()

std::vector< TrajectoryPoint > apollo::planning::TrajectoryStitcher::ComputeStitchingTrajectory ( const canbus::Chassis &  vehicle_chassis, 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, const control::ControlInteractiveMsg &  control_interactive_msg  )

static

在文件 trajectory_stitcher.cc 第 119 行定义.

                                                                 {
  // 1.check replan not by offset
  size_t time_matched_index = 0;
  if (need_replan_by_necessary_check(vehicle_state, current_timestamp,
                                     prev_trajectory, replan_reason,
                                     &time_matched_index)) {
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }
 
  // 2. check replan by GEAR switch
  if (vehicle_chassis.has_gear_location()) {
    static canbus::Chassis::GearPosition gear_pos =
        canbus::Chassis::GEAR_NEUTRAL;
    if (gear_pos == canbus::Chassis::GEAR_NEUTRAL &&
        vehicle_chassis.gear_location() == canbus::Chassis::GEAR_DRIVE) {
      gear_pos = vehicle_chassis.gear_location();
      const std::string msg =
          "gear change from n to d, replan to avoid large station error";
      AERROR << msg;
      *replan_reason = msg;
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }
    gear_pos = vehicle_chassis.gear_location();
  }
 
  auto time_matched_point = prev_trajectory->TrajectoryPointAt(
      static_cast<uint32_t>(time_matched_index));
 
  // 3.check replan by control_interactive_msg
  if (need_replan_by_control_interactive(current_timestamp, replan_reason,
                                         control_interactive_msg)) {
    return ComputeControlInteractiveStitchingTrajectory(
        planning_cycle_time, vehicle_state, time_matched_point,
        control_interactive_msg);
  }
 
  size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer(
      {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);
 
  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(),
      prev_trajectory->TrajectoryPointAt(
          static_cast<uint32_t>(position_matched_index)));
 
  // 4.check replan by offset
  if (replan_by_offset) {
    if (vehicle_chassis.has_parking_brake()) {
      static bool parking_brake = true;
      if (parking_brake && !vehicle_chassis.parking_brake()) {
        parking_brake = vehicle_chassis.parking_brake();
        const std::string msg =
            "parking brake off, ego move, replan to avoid large station error";
        AERROR << msg;
        *replan_reason = msg;
        return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                                vehicle_state);
      }
      parking_brake = vehicle_chassis.parking_brake();
    }
 
    auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;
    auto lat_diff = frenet_sd.second;
    double time_diff =
        time_matched_point.relative_time() -
        prev_trajectory
            ->TrajectoryPointAt(static_cast<uint32_t>(position_matched_index))
            .relative_time();
 
    ADEBUG << "Control lateral diff: " << lat_diff
           << ", longitudinal diff: " << lon_diff
           << ", time diff: " << time_diff;
 
    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);
    }
 
    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);
    }
 
    if (std::fabs(time_diff) > FLAGS_replan_time_threshold) {
      const std::string msg = absl::StrCat(
          "the difference between time matched point relative time and "
          "actual position corresponding relative time is too "
          "large. Replan is triggered. time_diff = ",
          time_diff);
      AERROR << msg;
      *replan_reason = msg;
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }
  } else {
    ADEBUG << "replan according to certain amount of "
           << "lat、lon and time offset is disabled";
  }
 
  // 4.stitching_trajectory
  double forward_rel_time =
      current_timestamp - prev_trajectory->header_time() + planning_cycle_time;
 
  size_t forward_time_index =
      prev_trajectory->QueryLowerBoundPoint(forward_rel_time);
 
  ADEBUG << "Position matched index:\t" << position_matched_index;
  ADEBUG << "Time matched index:\t" << time_matched_index;
 
  auto matched_index = std::min(time_matched_index, position_matched_index);
 
  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();
 
  const double zero_s = stitching_trajectory.back().path_point().s();
  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);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  return stitching_trajectory;
}

need_replan_by_control_interactive()

bool apollo::planning::TrajectoryStitcher::need_replan_by_control_interactive ( const double  current_timestamp, std::string *  replan_reason, const control::ControlInteractiveMsg &  control_interactive_msg  )

static

在文件 trajectory_stitcher.cc 第 337 行定义.

                                                                 {
  const double rel_time =
      current_timestamp - control_interactive_msg.header().timestamp_sec();
  if (rel_time > 0.5) {
    AINFO << "control_interactive_msg time out, skip replay by control "
             "interactive";
    return false;
  }
  if (control_interactive_msg.has_replan_request() &&
      control_interactive_msg.replan_request() == true) {
    *replan_reason = "replan for control_interactive_msg, " +
                     control_interactive_msg.replan_request_reason();
    return true;
  }
  return false;
}

need_replan_by_necessary_check()

bool apollo::planning::TrajectoryStitcher::need_replan_by_necessary_check ( const common::VehicleState &  vehicle_state, const double  current_timestamp, const PublishableTrajectory *  prev_trajectory, std::string *  replan_reason, size_t *  time_matched_index  )

static

在文件 trajectory_stitcher.cc 第 278 行定义.

                                {
  if (!FLAGS_enable_trajectory_stitcher) {
    *replan_reason = "stitch is disabled by gflag.";
    return true;
  }
  if (!prev_trajectory) {
    *replan_reason = "replan for no previous trajectory.";
    return true;
  }
 
  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    *replan_reason = "replan for manual mode.";
    return true;
  }
 
  size_t prev_trajectory_size = prev_trajectory->NumOfPoints();
 
  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 true;
  }
 
  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();
 
  *time_matched_index = prev_trajectory->QueryLowerBoundPoint(veh_rel_time);
 
  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 true;
  }
 
  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 true;
  }
 
  auto time_matched_point = prev_trajectory->TrajectoryPointAt(
      static_cast<uint32_t>(*time_matched_index));
 
  if (!time_matched_point.has_path_point()) {
    *replan_reason = "replan for previous trajectory missed path point";
    return true;
  }
  return false;
}

TransformLastPublishedTrajectory()

void apollo::planning::TrajectoryStitcher::TransformLastPublishedTrajectory ( const double  x_diff, const double  y_diff, const double  theta_diff, PublishableTrajectory *  prev_trajectory  )

static

在文件 trajectory_stitcher.cc 第 81 行定义.

                                            {
  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);
                });
}

---

该类的文档由以下文件生成:

• modules/planning/planning_base/common/trajectory_stitcher.h
• modules/planning/planning_base/common/trajectory_stitcher.cc