apollo::planning::Optimizer类 参考

#include <optimizer.h>

apollo::planning::Optimizer 的协作图:

Public 成员函数
	Optimizer (const OpenSpaceTrajectoryOptimizerParkConfig &config)
virtual	~Optimizer ()=default
common::Status	Plan (const OpenSpaceInfo open_space_info)
void	GetOptimizedTrajectory (DiscretizedTrajectory &optimized_trajectory)
void	UpdateDebugInfo (planning_internal::OpenSpaceDebug *open_space_debug)

详细描述

在文件 optimizer.h 第 46 行定义.

构造及析构函数说明

Optimizer()

apollo::planning::Optimizer::Optimizer

(

const OpenSpaceTrajectoryOptimizerParkConfig &

config

)

在文件 optimizer.cc 第 34 行定义.

    : config_(config) {
  // Load config
  AINFO << config_.DebugString();
 
  // Initialize dual variable warm start class pointer
  dual_variable_warm_start_.reset(
      new DualVariableWarmStartProblem(
          config.dual_variable_warm_start_config()));
 
  // Initialize distance approach trajectory smootherclass pointer
  distance_approach_.reset(
      new DistanceApproachProblem(config.distance_approach_config()));
 
  // Initialize iterative anchoring smoother config class pointer
  iterative_anchoring_smoother_.reset(
      new IterativeAnchoringSmoother(
          config.iterative_anchoring_smoother_config()));
}

~Optimizer()

virtual apollo::planning::Optimizer::~Optimizer ( )

virtualdefault

成员函数说明

GetOptimizedTrajectory()

void apollo::planning::Optimizer::GetOptimizedTrajectory ( DiscretizedTrajectory &  optimized_trajectory )

inline

在文件 optimizer.h 第 55 行定义.

                                                                           {
    optimized_trajectory.clear();
    optimized_trajectory = optimized_trajectory_;
  }

Plan()

Status apollo::planning::Optimizer::Plan

(

const OpenSpaceInfo

open_space_info

)

在文件 optimizer.cc 第 55 行定义.

                                                          {
  // const auto start_timestamp = std::chrono::system_clock::now();
 
  const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec =
      open_space_info.obstacles_vertices_vec();
  const std::vector<double>& XYbounds = open_space_info.ROI_xy_boundary();
  const Eigen::MatrixXi& obstacles_edges_num =
      open_space_info.obstacles_edges_num();
  const Eigen::MatrixXd& obstacles_A = open_space_info.obstacles_A();
  const Eigen::MatrixXd& obstacles_b = open_space_info.obstacles_b();
 
  DiscretizedTrajectory warm_start_trajectory =
      open_space_info.path_planning_trajectory_result();
  const double origin_angle = open_space_info.origin_heading();
  const Vec2d origin_point = open_space_info.origin_point();
  for (size_t i = 0; i < warm_start_trajectory.size(); i++) {
    double x = warm_start_trajectory[i].path_point().x();
    double y = warm_start_trajectory[i].path_point().y();
    double theta = warm_start_trajectory[i].path_point().theta();
    OpenSpaceTrajectoryOptimizerUtil::PathPointNormalizing(origin_angle,
                                                           origin_point,
                                                           &x,
                                                           &y,
                                                           &theta);
    warm_start_trajectory[i].mutable_path_point()->set_x(x);
    warm_start_trajectory[i].mutable_path_point()->set_y(y);
    warm_start_trajectory[i].mutable_path_point()->set_theta(theta);
  }
 
  // Containers for distance approach trajectory smoothing problem
  Eigen::MatrixXd xWS;
  Eigen::MatrixXd uWS;
  Eigen::MatrixXd state_result_ds;
  Eigen::MatrixXd control_result_ds;
  Eigen::MatrixXd time_result_ds;
  Eigen::MatrixXd l_warm_up;
  Eigen::MatrixXd n_warm_up;
  Eigen::MatrixXd dual_l_result_ds;
  Eigen::MatrixXd dual_n_result_ds;
 
  if (FLAGS_enable_parallel_trajectory_smoothing) {
    std::vector<DiscretizedTrajectory> partition_trajectories;
 
    PartitionTrajectory(warm_start_trajectory, &partition_trajectories);
 
    size_t size = partition_trajectories.size();
    std::vector<Eigen::MatrixXd> xWS_vec;
    std::vector<Eigen::MatrixXd> uWS_vec;
    std::vector<Eigen::MatrixXd> state_result_ds_vec;
    std::vector<Eigen::MatrixXd> control_result_ds_vec;
    std::vector<Eigen::MatrixXd> time_result_ds_vec;
    std::vector<Eigen::MatrixXd> l_warm_up_vec;
    std::vector<Eigen::MatrixXd> n_warm_up_vec;
    std::vector<Eigen::MatrixXd> dual_l_result_ds_vec;
    std::vector<Eigen::MatrixXd> dual_n_result_ds_vec;
    xWS_vec.resize(size);
    uWS_vec.resize(size);
    state_result_ds_vec.resize(size);
    control_result_ds_vec.resize(size);
    time_result_ds_vec.resize(size);
    l_warm_up_vec.resize(size);
    n_warm_up_vec.resize(size);
    dual_l_result_ds_vec.resize(size);
    dual_n_result_ds_vec.resize(size);
 
    // In for loop
    ADEBUG << "Trajectories size in smoother is " << size;
    for (size_t i = 0; i < size; ++i) {
      LoadTrajectoryInEigen(partition_trajectories[i], xWS_vec[i],
                                   uWS_vec[i]);
      Eigen::MatrixXd last_time_u(2, 1);
      last_time_u << 0.0, 0.0;
      double init_v = 0.0;
 
      // TODO(Jinyun): Further testing
      const auto smoother_start_timestamp = std::chrono::system_clock::now();
      if (FLAGS_use_iterative_anchoring_smoother) {
        AINFO << "use iterative anchoring smoother";
        if (!GenerateDecoupledTraj(
                xWS_vec[i], last_time_u(1, 0), init_v, obstacles_vertices_vec,
                &state_result_ds_vec[i], &control_result_ds_vec[i],
                &time_result_ds_vec[i])) {
          AERROR << "Smoother fail at " << i << "th trajectory";
          AERROR << i << "th trajectory size is " << xWS_vec[i].cols();
          return Status(
              ErrorCode::PLANNING_ERROR,
              "iterative anchoring smoothing problem failed to solve");
        }
      } else {
        const double start_system_timestamp =
            std::chrono::duration<double>(
                std::chrono::system_clock::now().time_since_epoch())
                .count();
        AINFO << "use distance approach parallel smoother";
        if (!GenerateDistanceApproachTraj(
                xWS_vec[i], uWS_vec[i], XYbounds, obstacles_edges_num,
                obstacles_A, obstacles_b, obstacles_vertices_vec, last_time_u,
                init_v, &state_result_ds_vec[i], &control_result_ds_vec[i],
                &time_result_ds_vec[i], &l_warm_up_vec[i], &n_warm_up_vec[i],
                &dual_l_result_ds_vec[i], &dual_n_result_ds_vec[i])) {
          AERROR << "Smoother fail at " << i
                 << "th trajectory with index starts from 0";
          AERROR << i << "th trajectory size is " << xWS_vec[i].cols();
          AERROR << "State matrix: " << xWS_vec[i];
          AERROR << "Control matrix: " << uWS_vec[i];
          return Status(ErrorCode::PLANNING_ERROR,
                        "distance approach smoothing problem failed to solve");
        }
        const auto end_system_timestamp =
            std::chrono::duration<double>(
                std::chrono::system_clock::now().time_since_epoch())
                .count();
        const auto time_diff_ms =
            (end_system_timestamp - start_system_timestamp) * 1000;
        ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";
        ADEBUG << i << "th trajectory size is " << xWS_vec[i].cols();
        ADEBUG << "average time spend: " << time_diff_ms / xWS_vec[i].cols()
               << " ms per point.";
        ADEBUG << "average time spend after smooth: "
               << time_diff_ms / state_result_ds_vec[i].cols()
               << " ms per point.";
        ADEBUG << i << "th smoothed trajectory size is "
               << state_result_ds_vec[i].cols();
      }
      const auto smoother_end_timestamp = std::chrono::system_clock::now();
      std::chrono::duration<double> smoother_diff =
          smoother_end_timestamp - smoother_start_timestamp;
      AINFO << "Open space trajectory smoothing total time: "
            << smoother_diff.count() * 1000.0 << " ms at the " << i
            << "th trajectory.";
      AINFO << "The " << i << "th trajectory pre-smoothing size is "
            << xWS_vec[i].cols() << "; post-smoothing size is "
            << state_result_ds_vec[i].cols();
    }
 
    // Retrive the trajectory in one piece
    CombineTrajectories(xWS_vec, uWS_vec, state_result_ds_vec,
                        control_result_ds_vec, time_result_ds_vec,
                        l_warm_up_vec, n_warm_up_vec, dual_l_result_ds_vec,
                        dual_n_result_ds_vec, &xWS, &uWS, &state_result_ds,
                        &control_result_ds, &time_result_ds, &l_warm_up,
                        &n_warm_up, &dual_l_result_ds, &dual_n_result_ds);
 
  } else {
    AINFO << "use distance approach smoother";
    LoadTrajectoryInEigen(warm_start_trajectory, xWS, uWS);
 
    Eigen::MatrixXd last_time_u(2, 1);
    last_time_u << 0, 0;
 
    const double init_v = 0;
    FLAGS_use_iterative_anchoring_smoother = false;
    if (!GenerateDistanceApproachTraj(
            xWS, uWS, XYbounds, obstacles_edges_num, obstacles_A, obstacles_b,
            obstacles_vertices_vec, last_time_u, init_v, &state_result_ds,
            &control_result_ds, &time_result_ds, &l_warm_up, &n_warm_up,
            &dual_l_result_ds, &dual_n_result_ds)) {
      return Status(ErrorCode::PLANNING_ERROR,
                    "distance approach smoothing problem failed to solve");
    }
  }
 
  // record debug info
  if (FLAGS_enable_record_debug) {
    open_space_debug_.Clear();
    RecordDebugInfo(origin_point, origin_angle,
                    open_space_info.open_space_end_pose(),
                    xWS, uWS, l_warm_up, n_warm_up, dual_l_result_ds,
                    dual_n_result_ds, state_result_ds, control_result_ds,
                    time_result_ds, XYbounds, obstacles_vertices_vec);
  }
 
  // rescale the states to the world frame
  size_t state_size = state_result_ds.cols();
  for (size_t i = 0; i < state_size; ++i) {
    OpenSpaceTrajectoryOptimizerUtil::PathPointDeNormalizing(
        origin_angle, origin_point,
        &(state_result_ds(0, i)), &(state_result_ds(1, i)),
        &(state_result_ds(2, i)));
  }
 
  LoadTrajectory(state_result_ds, control_result_ds, time_result_ds);
 
  // const auto end_timestamp = std::chrono::system_clock::now();
  // std::chrono::duration<double> diff = end_timestamp - start_timestamp;
  // ADEBUG << "open space trajectory smoother total time: "
  //        << diff.count() * 1000.0 << " ms.";
  // double temp = diff.count() * 1000.0;
 
  return Status::OK();
}

UpdateDebugInfo()

void apollo::planning::Optimizer::UpdateDebugInfo

(

planning_internal::OpenSpaceDebug *

open_space_debug

)

在文件 optimizer.cc 第 851 行定义.

                                                       {
  open_space_debug->MergeFrom(open_space_debug_);
}

---

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

• modules/planning/tasks/open_space_trajectory_optimizer_park/optimizer.h
• modules/planning/tasks/open_space_trajectory_optimizer_park/optimizer.cc