apollo::planning::OpenSpaceTrajectoryOptimizer类 参考

#include <open_space_trajectory_optimizer.h>

apollo::planning::OpenSpaceTrajectoryOptimizer 的协作图:

Public 成员函数
	OpenSpaceTrajectoryOptimizer (const OpenSpaceTrajectoryOptimizerConfig &config)
virtual	~OpenSpaceTrajectoryOptimizer ()=default
common::Status	Plan (const std::vector< common::TrajectoryPoint > &stitching_trajectory, const std::vector< double > &end_pose, const std::vector< double > &XYbounds, double rotate_angle, const common::math::Vec2d &translate_origin, const Eigen::MatrixXi &obstacles_edges_num, const Eigen::MatrixXd &obstacles_A, const Eigen::MatrixXd &obstacles_b, const std::vector< std::vector< common::math::Vec2d > > &obstacles_vertices_vec, double *time_latency)
void	GetStitchingTrajectory (std::vector< common::TrajectoryPoint > *stitching_trajectory)
void	GetOptimizedTrajectory (DiscretizedTrajectory *optimized_trajectory)
void	RecordDebugInfo (const common::TrajectoryPoint &trajectory_stitching_point, const common::math::Vec2d &translate_origin, const double rotate_angle, const std::vector< double > &end_pose, const Eigen::MatrixXd &xWS, const Eigen::MatrixXd &uWs, const Eigen::MatrixXd &l_warm_up, const Eigen::MatrixXd &n_warm_up, const Eigen::MatrixXd &dual_l_result_ds, const Eigen::MatrixXd &dual_n_result_ds, const Eigen::MatrixXd &state_result_ds, const Eigen::MatrixXd &control_result_ds, const Eigen::MatrixXd &time_result_ds, const std::vector< double > &XYbounds, const std::vector< std::vector< common::math::Vec2d > > &obstacles_vertices_vec)
void	UpdateDebugInfo (::apollo::planning_internal::OpenSpaceDebug *open_space_debug)
apollo::planning_internal::OpenSpaceDebug *	mutable_open_space_debug ()

详细描述

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

构造及析构函数说明

OpenSpaceTrajectoryOptimizer()

apollo::planning::OpenSpaceTrajectoryOptimizer::OpenSpaceTrajectoryOptimizer

(

const OpenSpaceTrajectoryOptimizerConfig &

config

)

在文件 open_space_trajectory_optimizer.cc 第 33 行定义.

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

~OpenSpaceTrajectoryOptimizer()

virtual apollo::planning::OpenSpaceTrajectoryOptimizer::~OpenSpaceTrajectoryOptimizer ( )

virtualdefault

成员函数说明

GetOptimizedTrajectory()

void apollo::planning::OpenSpaceTrajectoryOptimizer::GetOptimizedTrajectory ( DiscretizedTrajectory *  optimized_trajectory )

inline

在文件 open_space_trajectory_optimizer.h 第 69 行定义.

                                                                           {
    optimized_trajectory->clear();
    *optimized_trajectory = optimized_trajectory_;
  }

GetStitchingTrajectory()

void apollo::planning::OpenSpaceTrajectoryOptimizer::GetStitchingTrajectory ( std::vector< common::TrajectoryPoint > *  stitching_trajectory )

inline

在文件 open_space_trajectory_optimizer.h 第 63 行定义.

                                                              {
    stitching_trajectory->clear();
    *stitching_trajectory = stitching_trajectory_;
  }

mutable_open_space_debug()

apollo::planning_internal::OpenSpaceDebug * apollo::planning::OpenSpaceTrajectoryOptimizer::mutable_open_space_debug ( )

inline

在文件 open_space_trajectory_optimizer.h 第 91 行定义.

                                                                    {
    return &open_space_debug_;
  }

Plan()

Status apollo::planning::OpenSpaceTrajectoryOptimizer::Plan	(	const std::vector< common::TrajectoryPoint > &	stitching_trajectory,
		const std::vector< double > &	end_pose,
		const std::vector< double > &	XYbounds,
		double	rotate_angle,
		const common::math::Vec2d &	translate_origin,
		const Eigen::MatrixXi &	obstacles_edges_num,
		const Eigen::MatrixXd &	obstacles_A,
		const Eigen::MatrixXd &	obstacles_b,
		const std::vector< std::vector< common::math::Vec2d > > &	obstacles_vertices_vec,
		double *	time_latency
	)

在文件 open_space_trajectory_optimizer.cc 第 54 行定义.

                          {
  if (XYbounds.empty() || end_pose.empty() || obstacles_edges_num.cols() == 0 ||
      obstacles_A.cols() == 0 || obstacles_b.cols() == 0) {
    ADEBUG << "OpenSpaceTrajectoryOptimizer input data not ready";
    AERROR << XYbounds.empty() << end_pose.empty() << obstacles_edges_num.cols()
           << obstacles_A.cols() << obstacles_b.cols();
    return Status(ErrorCode::PLANNING_ERROR,
                  "OpenSpaceTrajectoryOptimizer input data not ready");
  }
  CHECK(!stitching_trajectory.empty());
  // Generate Stop trajectory if init point close to destination
  if (IsInitPointNearDestination(stitching_trajectory.back(), end_pose,
                                 rotate_angle, translate_origin)) {
    AINFO << "Planning init point is close to destination, skip new "
             "trajectory generation";
    return Status(ErrorCode::OK,
                  "Planning init point is close to destination, skip new "
                  "trajectory generation");
  }
 
  const auto start_timestamp = std::chrono::system_clock::now();
 
  // Initiate initial states
  stitching_trajectory_ = stitching_trajectory;
 
  // Init trajectory point is the stitching point from last trajectory
  const common::TrajectoryPoint trajectory_stitching_point =
      stitching_trajectory.back();
 
  // init x, y, z would be rotated.
  double init_x = trajectory_stitching_point.path_point().x();
  double init_y = trajectory_stitching_point.path_point().y();
  double init_phi = trajectory_stitching_point.path_point().theta();
  AINFO << "origin_point: (" << std::setprecision(9) << translate_origin.x()
        << "," << translate_origin.y() << ")";
  AINFO << "origin_heading:" << std::setprecision(9) << rotate_angle << ",";
  AINFO << "init_point: ( " << std::setprecision(9) << init_x << "," << init_y
        << ")";
  AINFO << "end_pose: (" << std::setprecision(9) << end_pose[0] << "," << end_pose[1]
        << ")";
 
  // Rotate and scale the state
  PathPointNormalizing(rotate_angle, translate_origin, &init_x, &init_y,
                       &init_phi);
 
  // Result container for warm start (initial velocity is assumed to be 0 for
  // now)
  HybridAStartResult result;
 
  if (warm_start_->Plan(init_x, init_y, init_phi, end_pose[0], end_pose[1],
                        end_pose[2], XYbounds, obstacles_vertices_vec,
                        &result)) {
    AINFO << "State warm start problem solved successfully!";
  } else {
    AERROR << "State warm start problem failed to solve";
    return Status(ErrorCode::PLANNING_ERROR,
                  "State warm start problem failed to solve");
  }
 
  // 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<HybridAStartResult> partition_trajectories;
    if (!warm_start_->TrajectoryPartition(result, &partition_trajectories)) {
      return Status(ErrorCode::PLANNING_ERROR, "Hybrid Astar partition failed");
    }
    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) {
      LoadHybridAstarResultInEigen(&partition_trajectories[i], &xWS_vec[i],
                                   &uWS_vec[i]);
      Eigen::MatrixXd last_time_u(2, 1);
      double init_v = 0.0;
      // Stitching point control and velocity is set for first piece of
      // trajectories. In the next ones, control and velocity are assumed to be
      // zero as the next trajectories always start from vehicle static state
      if (i == 0) {
        const double init_steer = trajectory_stitching_point.steer();
        const double init_a = trajectory_stitching_point.a();
        last_time_u << init_steer, init_a;
        init_v = trajectory_stitching_point.v();
      } else {
        last_time_u << 0.0, 0.0;
        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";
    LoadHybridAstarResultInEigen(&result, &xWS, &uWS);
 
    const double init_steer = trajectory_stitching_point.steer();
    const double init_a = trajectory_stitching_point.a();
    Eigen::MatrixXd last_time_u(2, 1);
    last_time_u << init_steer, init_a;
 
    const double init_v = trajectory_stitching_point.v();
    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(trajectory_stitching_point, translate_origin, rotate_angle,
                    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) {
    PathPointDeNormalizing(rotate_angle, translate_origin,
                           &(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.";
  *time_latency = diff.count() * 1000.0;
 
  return Status::OK();
}

RecordDebugInfo()

void apollo::planning::OpenSpaceTrajectoryOptimizer::RecordDebugInfo	(	const common::TrajectoryPoint &	trajectory_stitching_point,
		const common::math::Vec2d &	translate_origin,
		const double	rotate_angle,
		const std::vector< double > &	end_pose,
		const Eigen::MatrixXd &	xWS,
		const Eigen::MatrixXd &	uWs,
		const Eigen::MatrixXd &	l_warm_up,
		const Eigen::MatrixXd &	n_warm_up,
		const Eigen::MatrixXd &	dual_l_result_ds,
		const Eigen::MatrixXd &	dual_n_result_ds,
		const Eigen::MatrixXd &	state_result_ds,
		const Eigen::MatrixXd &	control_result_ds,
		const Eigen::MatrixXd &	time_result_ds,
		const std::vector< double > &	XYbounds,
		const std::vector< std::vector< common::math::Vec2d > > &	obstacles_vertices_vec
	)

在文件 open_space_trajectory_optimizer.cc 第 293 行定义.

                                                               {
  // load information about trajectory stitching point
 
  open_space_debug_.mutable_trajectory_stitching_point()->CopyFrom(
      trajectory_stitching_point);
  // load translation origin and heading angle
  auto* roi_shift_point = open_space_debug_.mutable_roi_shift_point();
  // pathpoint
  roi_shift_point->mutable_path_point()->set_x(translate_origin.x());
  roi_shift_point->mutable_path_point()->set_y(translate_origin.y());
  roi_shift_point->mutable_path_point()->set_theta(rotate_angle);
 
  // load end_pose into debug
  auto* end_point = open_space_debug_.mutable_end_point();
  end_point->mutable_path_point()->set_x(end_pose[0]);
  end_point->mutable_path_point()->set_y(end_pose[1]);
  end_point->mutable_path_point()->set_theta(end_pose[2]);
  end_point->set_v(end_pose[3]);
 
  // load warm start trajectory
  size_t horizon = xWS.cols() - 1;
  auto* warm_start_trajectory =
      open_space_debug_.mutable_warm_start_trajectory();
  for (size_t i = 0; i < horizon; ++i) {
    auto* warm_start_point = warm_start_trajectory->add_vehicle_motion_point();
    warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_x(
        xWS(0, i));
    warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_y(
        xWS(1, i));
    warm_start_point->mutable_trajectory_point()
        ->mutable_path_point()
        ->set_theta(xWS(2, i));
    warm_start_point->mutable_trajectory_point()->set_v(xWS(3, i));
    warm_start_point->set_steer(uWS(0, i));
    warm_start_point->mutable_trajectory_point()->set_a(uWS(1, i));
  }
  auto* warm_start_point = warm_start_trajectory->add_vehicle_motion_point();
  warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_x(
      xWS(0, horizon));
  warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_y(
      xWS(1, horizon));
  warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_theta(
      xWS(2, horizon));
  warm_start_point->mutable_trajectory_point()->set_v(xWS(3, horizon));
 
  // load warm start dual variables
  size_t l_warm_up_rows = l_warm_up.rows();
  for (size_t i = 0; i < horizon; ++i) {
    for (size_t j = 0; j < l_warm_up_rows; j++) {
      open_space_debug_.add_warm_start_dual_lambda(l_warm_up(j, i));
    }
  }
  size_t n_warm_up_rows = n_warm_up.rows();
  for (size_t i = 0; i < horizon; ++i) {
    for (size_t j = 0; j < n_warm_up_rows; j++) {
      open_space_debug_.add_warm_start_dual_miu(n_warm_up(j, i));
    }
  }
 
  // load optimized dual variables
  size_t dual_l_result_ds_rows = dual_l_result_ds.rows();
  for (size_t i = 0; i < horizon; ++i) {
    for (size_t j = 0; j < dual_l_result_ds_rows; j++) {
      open_space_debug_.add_optimized_dual_lambda(dual_l_result_ds(j, i));
    }
  }
  size_t dual_n_result_ds_rows = dual_n_result_ds.rows();
  for (size_t i = 0; i < horizon; ++i) {
    for (size_t j = 0; j < dual_n_result_ds_rows; j++) {
      open_space_debug_.add_optimized_dual_miu(dual_n_result_ds(j, i));
    }
  }
 
  double relative_time = 0;
 
  // load smoothed trajectory
  horizon = state_result_ds.cols() - 1;
  auto* smoothed_trajectory = open_space_debug_.mutable_smoothed_trajectory();
  for (size_t i = 0; i < horizon; ++i) {
    auto* smoothed_point = smoothed_trajectory->add_vehicle_motion_point();
    smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_x(
        state_result_ds(0, i));
    smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_y(
        state_result_ds(1, i));
    smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_theta(
        state_result_ds(2, i));
    smoothed_point->mutable_trajectory_point()->set_v(state_result_ds(3, i));
    smoothed_point->set_steer(control_result_ds(0, i));
    smoothed_point->mutable_trajectory_point()->set_a(control_result_ds(1, i));
    relative_time += time_result_ds(0, i);
    smoothed_point->mutable_trajectory_point()->set_relative_time(
        relative_time);
  }
  auto* smoothed_point = smoothed_trajectory->add_vehicle_motion_point();
  smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_x(
      state_result_ds(0, horizon));
  smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_y(
      state_result_ds(1, horizon));
  smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_theta(
      state_result_ds(2, horizon));
  smoothed_point->mutable_trajectory_point()->set_v(
      state_result_ds(3, horizon));
 
  // load xy boundary (xmin, xmax, ymin, ymax)
  open_space_debug_.add_xy_boundary(XYbounds[0]);
  open_space_debug_.add_xy_boundary(XYbounds[1]);
  open_space_debug_.add_xy_boundary(XYbounds[2]);
  open_space_debug_.add_xy_boundary(XYbounds[3]);
 
  // load obstacles
  for (const auto& obstacle_vertices : obstacles_vertices_vec) {
    auto* obstacle_ptr = open_space_debug_.add_obstacles();
    for (const auto& vertex : obstacle_vertices) {
      obstacle_ptr->add_vertices_x_coords(vertex.x());
      obstacle_ptr->add_vertices_y_coords(vertex.y());
    }
  }
}

UpdateDebugInfo()

void apollo::planning::OpenSpaceTrajectoryOptimizer::UpdateDebugInfo

(

::apollo::planning_internal::OpenSpaceDebug *

open_space_debug

)

在文件 open_space_trajectory_optimizer.cc 第 422 行定义.

                                                       {
  open_space_debug->MergeFrom(open_space_debug_);
}

---

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

• modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_optimizer.h
• modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_optimizer.cc