distance_approach_problem_wrapper.cc

浏览该文件的文档.

/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#include <future>
 
#include "cyber/common/file.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/planning_open_space/coarse_trajectory_generator/hybrid_a_star.h"
#include "modules/planning/planning_open_space/trajectory_smoother/distance_approach_problem.h"
#include "modules/planning/planning_open_space/trajectory_smoother/dual_variable_warm_start_problem.h"
 
namespace apollo {
namespace planning {
 
using apollo::common::math::Vec2d;
 
class ObstacleContainer {
 public:
  ObstacleContainer() = default;
 
  bool VPresentationObstacle(
      const double* ROI_distance_approach_parking_boundary) {
    obstacles_num_ = 4;
    obstacles_edges_num_.resize(4, 1);
    obstacles_edges_num_ << 2, 1, 2, 1;
    size_t index = 0;
    for (size_t i = 0; i < obstacles_num_; i++) {
      std::vector<Vec2d> vertices_cw;
      for (int j = 0; j < obstacles_edges_num_(i, 0) + 1; j++) {
        Vec2d vertice =
            Vec2d(ROI_distance_approach_parking_boundary[index],
                  ROI_distance_approach_parking_boundary[index + 1]);
        index += 2;
        vertices_cw.emplace_back(vertice);
      }
      obstacles_vertices_vec_.emplace_back(vertices_cw);
    }
    return true;
  }
 
  bool HPresentationObstacle() {
    obstacles_A_ = Eigen::MatrixXd::Zero(obstacles_edges_num_.sum(), 2);
    obstacles_b_ = Eigen::MatrixXd::Zero(obstacles_edges_num_.sum(), 1);
    // vertices using H-representation
    if (!ObsHRep(obstacles_num_, obstacles_edges_num_, obstacles_vertices_vec_,
                 &obstacles_A_, &obstacles_b_)) {
      AINFO << "Fail to present obstacle in hyperplane";
      return false;
    }
    return true;
  }
 
  bool ObsHRep(const size_t obstacles_num,
               const Eigen::MatrixXi& obstacles_edges_num,
               const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec,
               Eigen::MatrixXd* A_all, Eigen::MatrixXd* b_all) {
    if (obstacles_num != obstacles_vertices_vec.size()) {
      AINFO << "obstacles_num != obstacles_vertices_vec.size()";
      return false;
    }
 
    A_all->resize(obstacles_edges_num.sum(), 2);
    b_all->resize(obstacles_edges_num.sum(), 1);
 
    int counter = 0;
    double kEpsilon = 1.0e-5;
    // start building H representation
    for (size_t i = 0; i < obstacles_num; ++i) {
      size_t current_vertice_num = obstacles_edges_num(i, 0);
      Eigen::MatrixXd A_i(current_vertice_num, 2);
      Eigen::MatrixXd b_i(current_vertice_num, 1);
 
      // take two subsequent vertices, and computer hyperplane
      for (size_t j = 0; j < current_vertice_num; ++j) {
        Vec2d v1 = obstacles_vertices_vec[i][j];
        Vec2d v2 = obstacles_vertices_vec[i][j + 1];
 
        Eigen::MatrixXd A_tmp(2, 1), b_tmp(1, 1), ab(2, 1);
        // find hyperplane passing through v1 and v2
        if (std::abs(v1.x() - v2.x()) < kEpsilon) {
          if (v2.y() < v1.y()) {
            A_tmp << 1, 0;
            b_tmp << v1.x();
          } else {
            A_tmp << -1, 0;
            b_tmp << -v1.x();
          }
        } else if (std::abs(v1.y() - v2.y()) < kEpsilon) {
          if (v1.x() < v2.x()) {
            A_tmp << 0, 1;
            b_tmp << v1.y();
          } else {
            A_tmp << 0, -1;
            b_tmp << -v1.y();
          }
        } else {
          Eigen::MatrixXd tmp1(2, 2);
          tmp1 << v1.x(), 1, v2.x(), 1;
          Eigen::MatrixXd tmp2(2, 1);
          tmp2 << v1.y(), v2.y();
          ab = tmp1.inverse() * tmp2;
          double a = ab(0, 0);
          double b = ab(1, 0);
 
          if (v1.x() < v2.x()) {
            A_tmp << -a, 1;
            b_tmp << b;
          } else {
            A_tmp << a, -1;
            b_tmp << -b;
          }
        }
 
        // store vertices
        A_i.block(j, 0, 1, 2) = A_tmp.transpose();
        b_i.block(j, 0, 1, 1) = b_tmp;
      }
 
      A_all->block(counter, 0, A_i.rows(), 2) = A_i;
      b_all->block(counter, 0, b_i.rows(), 1) = b_i;
      counter += static_cast<int>(current_vertice_num);
    }
    return true;
  }
 
  void AddObstacle(const double* ROI_distance_approach_parking_boundary) {
    // the obstacles are hard coded into vertice sets of 3, 2, 3, 2
    if (!(VPresentationObstacle(ROI_distance_approach_parking_boundary) &&
          HPresentationObstacle())) {
      AINFO << "obstacle presentation fails";
    }
  }
 
  const std::vector<std::vector<Vec2d>>& GetObstacleVec() const {
    return obstacles_vertices_vec_;
  }
  const Eigen::MatrixXd& GetAMatrix() const { return obstacles_A_; }
  const Eigen::MatrixXd& GetbMatrix() const { return obstacles_b_; }
  size_t GetObstaclesNum() const { return obstacles_num_; }
  const Eigen::MatrixXi& GetObstaclesEdgesNum() const {
    return obstacles_edges_num_;
  }
 
 private:
  size_t obstacles_num_ = 0;
  Eigen::MatrixXi obstacles_edges_num_;
  std::vector<std::vector<Vec2d>> obstacles_vertices_vec_;
  Eigen::MatrixXd obstacles_A_;
  Eigen::MatrixXd obstacles_b_;
};
 
class ResultContainer {
 public:
  ResultContainer() = default;
  void LoadHybridAResult() {
    x_ = std::move(result_.x);
    y_ = std::move(result_.y);
    phi_ = std::move(result_.phi);
    v_ = std::move(result_.v);
    a_ = std::move(result_.a);
    steer_ = std::move(result_.steer);
  }
  std::vector<double>* GetX() { return &x_; }
  std::vector<double>* GetY() { return &y_; }
  std::vector<double>* GetPhi() { return &phi_; }
  std::vector<double>* GetV() { return &v_; }
  std::vector<double>* GetA() { return &a_; }
  std::vector<double>* GetSteer() { return &steer_; }
  HybridAStartResult* PrepareHybridAResult() { return &result_; }
  Eigen::MatrixXd* PrepareStateResult() { return &state_result_ds_; }
  Eigen::MatrixXd* PrepareControlResult() { return &control_result_ds_; }
  Eigen::MatrixXd* PrepareTimeResult() { return &time_result_ds_; }
  Eigen::MatrixXd* PrepareLResult() { return &dual_l_result_ds_; }
  Eigen::MatrixXd* PrepareNResult() { return &dual_n_result_ds_; }
  double* GetHybridTime() { return &hybrid_time_; }
  double* GetDualTime() { return &dual_time_; }
  double* GetIpoptTime() { return &ipopt_time_; }
 
 private:
  HybridAStartResult result_;
  std::vector<double> x_;
  std::vector<double> y_;
  std::vector<double> phi_;
  std::vector<double> v_;
  std::vector<double> a_;
  std::vector<double> steer_;
  Eigen::MatrixXd state_result_ds_;
  Eigen::MatrixXd control_result_ds_;
  Eigen::MatrixXd time_result_ds_;
  Eigen::MatrixXd dual_l_result_ds_;
  Eigen::MatrixXd dual_n_result_ds_;
  double hybrid_time_;
  double dual_time_;
  double ipopt_time_;
};
 
extern "C" {
HybridAStar* CreateHybridAPtr() {
  apollo::planning::PlannerOpenSpaceConfig planner_open_space_config_;
  ACHECK(apollo::cyber::common::GetProtoFromFile(
      FLAGS_planner_open_space_config_filename, &planner_open_space_config_))
      << "Failed to load open space config file "
      << FLAGS_planner_open_space_config_filename;
  return new HybridAStar(planner_open_space_config_);
}
ObstacleContainer* DistanceCreateObstaclesPtr() {
  return new ObstacleContainer();
}
ResultContainer* DistanceCreateResultPtr() { return new ResultContainer(); }
 
void AddObstacle(ObstacleContainer* obstacles_ptr,
                 const double* ROI_distance_approach_parking_boundary) {
  obstacles_ptr->AddObstacle(ROI_distance_approach_parking_boundary);
}
 
double InterpolateUsingLinearApproximation(const double p0, const double p1,
                                           const double w) {
  return p0 * (1.0 - w) + p1 * w;
}
 
std::vector<double> VectorLinearInterpolation(const std::vector<double>& x,
                                              int extend_size) {
  // interplation example:
  // x: [x0, x1, x2], extend_size: 3
  // output: [y0(x0), y1, y2, y3(x1), y4, y5, y6(x2)]
  size_t origin_last = x.size() - 1;
  std::vector<double> res(origin_last * extend_size + 1, 0.0);
 
  for (size_t i = 0; i < origin_last * extend_size; ++i) {
    size_t idx0 = i / extend_size;
    size_t idx1 = idx0 + 1;
    double w =
        static_cast<double>(i % extend_size) / static_cast<double>(extend_size);
    res[i] = InterpolateUsingLinearApproximation(x[idx0], x[idx1], w);
  }
 
  res.back() = x.back();
  return res;
}
 
bool DistanceSmoothing(
    const apollo::planning::PlannerOpenSpaceConfig& planner_open_space_config,
    const ObstacleContainer& obstacles, double sx, double sy, double sphi,
    double ex, double ey, double ephi, const std::vector<double>& XYbounds,
    HybridAStartResult* hybrid_a_star_result, Eigen::MatrixXd* state_result_ds_,
    Eigen::MatrixXd* control_result_ds_, Eigen::MatrixXd* time_result_ds_,
    Eigen::MatrixXd* dual_l_result_ds_, Eigen::MatrixXd* dual_n_result_ds_,
    double& dual_time, double& ipopt_time) {
  // load Warm Start result(horizon is the "N", not the size of step points)
  size_t horizon_ = hybrid_a_star_result->x.size() - 1;
  // nominal sampling time
  float ts_ = planner_open_space_config.delta_t();
 
  Eigen::VectorXd x;
  Eigen::VectorXd y;
  Eigen::VectorXd phi;
  Eigen::VectorXd v;
  Eigen::VectorXd steer;
  Eigen::VectorXd a;
 
  // TODO(Runxin): extend logics in future
  if (horizon_ <= 10 && horizon_ > 2 &&
      planner_open_space_config.enable_linear_interpolation()) {
    // TODO(Runxin): extend this number
    int extend_size = 5;
    // modify state and control vectors sizes
    horizon_ = extend_size * horizon_;
    // modify delta t
    ts_ = ts_ / static_cast<float>(extend_size);
 
    std::vector<double> x_extend =
        VectorLinearInterpolation(hybrid_a_star_result->x, extend_size);
    x = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(x_extend.data(),
                                                      horizon_ + 1);
 
    std::vector<double> y_extend =
        VectorLinearInterpolation(hybrid_a_star_result->y, extend_size);
    y = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(y_extend.data(),
                                                      horizon_ + 1);
 
    std::vector<double> phi_extend =
        VectorLinearInterpolation(hybrid_a_star_result->phi, extend_size);
    phi = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(phi_extend.data(),
                                                        horizon_ + 1);
 
    std::vector<double> v_extend =
        VectorLinearInterpolation(hybrid_a_star_result->v, extend_size);
    v = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(v_extend.data(),
                                                      horizon_ + 1);
 
    steer = Eigen::VectorXd(horizon_);
    a = Eigen::VectorXd(horizon_);
    for (size_t i = 0; i < static_cast<size_t>(horizon_); ++i) {
      steer[i] = hybrid_a_star_result->steer[i / extend_size];
      a[i] = hybrid_a_star_result->a[i / extend_size];
    }
    ADEBUG << "hybrid A x: ";
    for (size_t i = 0; i < hybrid_a_star_result->x.size(); ++i) {
      ADEBUG << "i: " << i << ", val: " << hybrid_a_star_result->x[i];
    }
    ADEBUG << "interpolated x: \n" << x;
 
    ADEBUG << "hybrid A steer: ";
    for (size_t i = 0; i < hybrid_a_star_result->steer.size(); ++i) {
      ADEBUG << "i: " << i << ", val: " << hybrid_a_star_result->steer[i];
    }
    ADEBUG << "interpolated steer: \n" << steer;
  } else {
    x = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
        hybrid_a_star_result->x.data(), horizon_ + 1);
    y = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
        hybrid_a_star_result->y.data(), horizon_ + 1);
    phi = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
        hybrid_a_star_result->phi.data(), horizon_ + 1);
    v = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
        hybrid_a_star_result->v.data(), horizon_ + 1);
    steer = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
        hybrid_a_star_result->steer.data(), horizon_);
    a = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
        hybrid_a_star_result->a.data(), horizon_);
  }
 
  Eigen::MatrixXd xWS = Eigen::MatrixXd::Zero(4, horizon_ + 1);
  Eigen::MatrixXd uWS = Eigen::MatrixXd::Zero(2, horizon_);
  xWS.row(0) = x;
  xWS.row(1) = y;
  xWS.row(2) = phi;
  xWS.row(3) = v;
  uWS.row(0) = steer;
  uWS.row(1) = a;
 
  Eigen::MatrixXd x0(4, 1);
  x0 << sx, sy, sphi, 0.0;
 
  Eigen::MatrixXd xF(4, 1);
  xF << ex, ey, ephi, 0.0;
 
  Eigen::MatrixXd last_time_u(2, 1);
  last_time_u << 0.0, 0.0;
 
  common::VehicleParam vehicle_param_ =
      common::VehicleConfigHelper::GetConfig().vehicle_param();
 
  // load vehicle configuration
  Eigen::MatrixXd ego_(4, 1);
  double front_to_center = vehicle_param_.front_edge_to_center();
  double back_to_center = vehicle_param_.back_edge_to_center();
  double left_to_center = vehicle_param_.left_edge_to_center();
  double right_to_center = vehicle_param_.right_edge_to_center();
  ego_ << front_to_center, right_to_center, back_to_center, left_to_center;
 
  // result for distance approach problem
  Eigen::MatrixXd l_warm_up;
  Eigen::MatrixXd n_warm_up;
  Eigen::MatrixXd s_warm_up =
      Eigen::MatrixXd::Zero(obstacles.GetObstaclesNum(), horizon_ + 1);
 
  DualVariableWarmStartProblem* dual_variable_warm_start_ptr =
      new DualVariableWarmStartProblem(planner_open_space_config);
 
  const auto t1 = std::chrono::system_clock::now();
  if (FLAGS_use_dual_variable_warm_start) {
    bool dual_variable_warm_start_status = dual_variable_warm_start_ptr->Solve(
        horizon_, ts_, ego_, obstacles.GetObstaclesNum(),
        obstacles.GetObstaclesEdgesNum(), obstacles.GetAMatrix(),
        obstacles.GetbMatrix(), xWS, &l_warm_up, &n_warm_up, &s_warm_up);
 
    if (dual_variable_warm_start_status) {
      AINFO << "Dual variable problem solved successfully!";
    } else {
      AERROR << "Dual variable problem solving failed";
      return false;
    }
  } else {
    l_warm_up = Eigen::MatrixXd::Zero(obstacles.GetObstaclesEdgesNum().sum(),
                                      horizon_ + 1);
    n_warm_up =
        Eigen::MatrixXd::Zero(4 * obstacles.GetObstaclesNum(), horizon_ + 1);
  }
  const auto t2 = std::chrono::system_clock::now();
  dual_time = std::chrono::duration<double>(t2 - t1).count() * 1000;
 
  DistanceApproachProblem* distance_approach_ptr =
      new DistanceApproachProblem(planner_open_space_config);
 
  bool status = distance_approach_ptr->Solve(
      x0, xF, last_time_u, horizon_, ts_, ego_, xWS, uWS, l_warm_up, n_warm_up,
      s_warm_up, XYbounds, obstacles.GetObstaclesNum(),
      obstacles.GetObstaclesEdgesNum(), obstacles.GetAMatrix(),
      obstacles.GetbMatrix(), state_result_ds_, control_result_ds_,
      time_result_ds_, dual_l_result_ds_, dual_n_result_ds_);
  const auto t3 = std::chrono::system_clock::now();
  ipopt_time = std::chrono::duration<double>(t3 - t2).count() * 1000;
 
  if (!status) {
    AERROR << "Distance fail";
    return false;
  }
  return true;
}
 
bool DistancePlan(HybridAStar* hybridA_ptr, ObstacleContainer* obstacles_ptr,
                  ResultContainer* result_ptr, double sx, double sy,
                  double sphi, double ex, double ey, double ephi,
                  double* XYbounds) {
  apollo::planning::PlannerOpenSpaceConfig planner_open_space_config_;
  ACHECK(apollo::cyber::common::GetProtoFromFile(
      FLAGS_planner_open_space_config_filename, &planner_open_space_config_))
      << "Failed to load open space config file "
      << FLAGS_planner_open_space_config_filename;
  AINFO << "FLAGS_planner_open_space_config_filename: "
        << FLAGS_planner_open_space_config_filename;
 
  double hybrid_total = 0.0;
  double dual_total = 0.0;
  double ipopt_total = 0.0;
 
  std::string flag_file_path =
      "modules/planning/planning_component/conf/planning.conf";
  google::SetCommandLineOption("flagfile", flag_file_path.c_str());
 
  HybridAStartResult hybrid_astar_result;
  std::vector<double> XYbounds_(XYbounds, XYbounds + 4);
  std::vector<std::vector<common::math::Vec2d>> soft_boundary_vertices_vec;
 
  const auto start_timestamp = std::chrono::system_clock::now();
  if (!hybridA_ptr->Plan(sx, sy, sphi, ex, ey, ephi, XYbounds_,
                         obstacles_ptr->GetObstacleVec(),
                         &hybrid_astar_result,
                         soft_boundary_vertices_vec, false)) {
    AINFO << "Hybrid A Star fails";
    return false;
  }
  const auto end_timestamp = std::chrono::system_clock::now();
  std::chrono::duration<double> time_diff = end_timestamp - start_timestamp;
  hybrid_total = time_diff.count() * 1000;
 
  if (FLAGS_enable_parallel_trajectory_smoothing) {
    std::vector<HybridAStartResult> partition_trajectories;
    if (!hybridA_ptr->TrajectoryPartition(hybrid_astar_result,
                                          &partition_trajectories)) {
      AERROR << "TrajectoryPartition failed";
      *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
      return false;
    }
    size_t size = partition_trajectories.size();
    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> dual_l_result_ds_vec;
    std::vector<Eigen::MatrixXd> dual_n_result_ds_vec;
    state_result_ds_vec.resize(size);
    control_result_ds_vec.resize(size);
    time_result_ds_vec.resize(size);
    dual_l_result_ds_vec.resize(size);
    dual_n_result_ds_vec.resize(size);
    std::vector<std::future<bool>> results;
 
    // In parallel
    // TODO(Jinyun): fix memory issue
    // for (size_t i = 0; i < size; ++i) {
    //   double piece_wise_sx = partition_trajectories[i].x.front();
    //   double piece_wise_sy = partition_trajectories[i].y.front();
    //   double piece_wise_sphi = partition_trajectories[i].phi.front();
    //   double piece_wise_ex = partition_trajectories[i].x.back();
    //   double piece_wise_ey = partition_trajectories[i].y.back();
    //   double piece_wise_ephi = partition_trajectories[i].phi.back();
    //   auto* ith_trajectories = &partition_trajectories[i];
    //   auto* ith_state_result = &state_result_ds_vec[i];
    //   auto* ith_control_result = &control_result_ds_vec[i];
    //   auto* ith_time_result = &time_result_ds_vec[i];
    //   auto* ith_dual_l_result = &dual_l_result_ds_vec[i];
    //   auto* ith_dual_n_result = &dual_n_result_ds_vec[i];
    //   results.push_back(
    //       cyber::Async(&DistanceSmoothing,
    //       std::ref(planner_open_space_config_),
    //                    std::ref(*obstacles_ptr), piece_wise_sx,
    //                    piece_wise_sy, piece_wise_sphi, piece_wise_ex,
    //                    piece_wise_ey, piece_wise_ephi, std::ref(XYbounds_),
    //                    ith_trajectories, ith_state_result,
    //                    ith_control_result, ith_time_result,
    //                    ith_dual_l_result, ith_dual_n_result));
    // }
    // for (auto& result : results) {
    //   if (!result.get()) {
    //     AERROR << "Failure in a piece of trajectory.";
    //     return false;
    //   }
    // }
 
    // In for loop
    double dual_tmp = 0.0;
    double ipopt_tmp = 0.0;
    for (size_t i = 0; i < size; ++i) {
      double piece_wise_sx = partition_trajectories[i].x.front();
      double piece_wise_sy = partition_trajectories[i].y.front();
      double piece_wise_sphi = partition_trajectories[i].phi.front();
      double piece_wise_ex = partition_trajectories[i].x.back();
      double piece_wise_ey = partition_trajectories[i].y.back();
      double piece_wise_ephi = partition_trajectories[i].phi.back();
 
      if (!DistanceSmoothing(planner_open_space_config_, *obstacles_ptr,
                             piece_wise_sx, piece_wise_sy, piece_wise_sphi,
                             piece_wise_ex, piece_wise_ey, piece_wise_ephi,
                             XYbounds_, &partition_trajectories[i],
                             &state_result_ds_vec[i], &control_result_ds_vec[i],
                             &time_result_ds_vec[i], &dual_l_result_ds_vec[i],
                             &dual_n_result_ds_vec[i], dual_tmp, ipopt_tmp)) {
        *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
        return false;
      }
      dual_total += dual_tmp;
      ipopt_total += ipopt_tmp;
    }
 
    // Retrieve result in one single trajectory
    size_t trajectory_point_size = 0;
    for (size_t i = 0; i < size; ++i) {
      if (state_result_ds_vec[i].cols() < 2) {
        AERROR << "state horizon smaller than 2";
        return false;
      }
      AINFO << "trajectory idx: "
            << "idx range: " << trajectory_point_size << ", "
            << trajectory_point_size +
                   static_cast<size_t>(state_result_ds_vec[i].cols()) - 1;
      trajectory_point_size +=
          static_cast<size_t>(state_result_ds_vec[i].cols()) - 1;
    }
    ++trajectory_point_size;
 
    const uint64_t state_dimension = state_result_ds_vec.front().rows();
    Eigen::MatrixXd state_result_ds;
    state_result_ds.resize(state_dimension, trajectory_point_size);
    uint64_t k = 0;
    for (size_t i = 0; i < size; ++i) {
      // leave out the last repeated point so set column minus one
      uint64_t state_col_num = state_result_ds_vec[i].cols() - 1;
      for (uint64_t j = 0; j < state_col_num; ++j) {
        state_result_ds.col(k) = state_result_ds_vec[i].col(j);
        ++k;
      }
    }
    state_result_ds.col(k) =
        state_result_ds_vec.back().col(state_result_ds_vec.back().cols() - 1);
 
    const uint64_t control_dimension = control_result_ds_vec.front().rows();
    Eigen::MatrixXd control_result_ds;
    control_result_ds.resize(control_dimension, trajectory_point_size - 1);
    k = 0;
 
    for (size_t i = 0; i < size; ++i) {
      uint64_t control_col_num = control_result_ds_vec[i].cols() - 1;
      for (uint64_t j = 0; j < control_col_num; ++j) {
        control_result_ds.col(k) = control_result_ds_vec[i].col(j);
        ++k;
      }
    }
 
    const uint64_t time_dimension = time_result_ds_vec.front().rows();
    Eigen::MatrixXd time_result_ds;
    time_result_ds.resize(time_dimension, trajectory_point_size - 1);
    k = 0;
    for (size_t i = 0; i < size; ++i) {
      uint64_t time_col_num = time_result_ds_vec[i].cols() - 1;
      for (uint64_t j = 0; j < time_col_num; ++j) {
        time_result_ds.col(k) = time_result_ds_vec[i].col(j);
        ++k;
      }
    }
 
    *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
    *(result_ptr->PrepareStateResult()) = state_result_ds;
    *(result_ptr->PrepareControlResult()) = control_result_ds;
    *(result_ptr->PrepareTimeResult()) = time_result_ds;
    *(result_ptr->GetHybridTime()) = hybrid_total;
    *(result_ptr->GetDualTime()) = dual_total;
    *(result_ptr->GetIpoptTime()) = ipopt_total;
  } else {
    Eigen::MatrixXd state_result_ds;
    Eigen::MatrixXd control_result_ds;
    Eigen::MatrixXd time_result_ds;
    Eigen::MatrixXd dual_l_result_ds;
    Eigen::MatrixXd dual_n_result_ds;
    if (!DistanceSmoothing(planner_open_space_config_, *obstacles_ptr, sx, sy,
                           sphi, ex, ey, ephi, XYbounds_, &hybrid_astar_result,
                           &state_result_ds, &control_result_ds,
                           &time_result_ds, &dual_l_result_ds,
                           &dual_n_result_ds, dual_total, ipopt_total)) {
      AERROR << "DistanceSmoothing failed";
      *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
      return false;
    }
    *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
    *(result_ptr->PrepareStateResult()) = state_result_ds;
    *(result_ptr->PrepareControlResult()) = control_result_ds;
    *(result_ptr->PrepareTimeResult()) = time_result_ds;
    *(result_ptr->PrepareLResult()) = dual_l_result_ds;
    *(result_ptr->PrepareNResult()) = dual_n_result_ds;
    *(result_ptr->GetHybridTime()) = hybrid_total;
    *(result_ptr->GetDualTime()) = dual_total;
    *(result_ptr->GetIpoptTime()) = ipopt_total;
  }
  return true;
}
 
void DistanceGetResult(ResultContainer* result_ptr,
                       ObstacleContainer* obstacles_ptr, double* x, double* y,
                       double* phi, double* v, double* a, double* steer,
                       double* opt_x, double* opt_y, double* opt_phi,
                       double* opt_v, double* opt_a, double* opt_steer,
                       double* opt_time, double* opt_dual_l, double* opt_dual_n,
                       size_t* output_size, double* hybrid_time,
                       double* dual_time, double* ipopt_time) {
  result_ptr->LoadHybridAResult();
  size_t size = result_ptr->GetX()->size();
  size_t size_by_distance = result_ptr->PrepareStateResult()->cols();
  AERROR_IF(size != size_by_distance)
      << "sizes by hybrid A and distance approach not consistent";
  for (size_t i = 0; i < size; ++i) {
    x[i] = result_ptr->GetX()->at(i);
    y[i] = result_ptr->GetY()->at(i);
    phi[i] = result_ptr->GetPhi()->at(i);
    v[i] = result_ptr->GetV()->at(i);
  }
  for (size_t i = 0; i + 1 < size; ++i) {
    a[i] = result_ptr->GetA()->at(i);
    steer[i] = result_ptr->GetSteer()->at(i);
  }
  output_size[0] = size;
 
  size_t obstacles_edges_sum = obstacles_ptr->GetObstaclesEdgesNum().sum();
  size_t obstacles_num_to_car = 4 * obstacles_ptr->GetObstaclesNum();
  for (size_t i = 0; i < size_by_distance; ++i) {
    opt_x[i] = (*(result_ptr->PrepareStateResult()))(0, i);
    opt_y[i] = (*(result_ptr->PrepareStateResult()))(1, i);
    opt_phi[i] = (*(result_ptr->PrepareStateResult()))(2, i);
    opt_v[i] = (*(result_ptr->PrepareStateResult()))(3, i);
  }
 
  if (result_ptr->PrepareTimeResult() != 0) {
    for (size_t i = 0; i + 1 < size_by_distance; ++i) {
      opt_time[i] = (*(result_ptr->PrepareTimeResult()))(0, i);
    }
  }
 
  if (result_ptr->PrepareLResult()->cols() != 0 &&
      result_ptr->PrepareNResult() != 0) {
    for (size_t i = 0; i + 1 < size_by_distance; ++i) {
      for (size_t j = 0; j < obstacles_edges_sum; ++j) {
        opt_dual_l[i * obstacles_edges_sum + j] =
            (*(result_ptr->PrepareLResult()))(j, i);
      }
      for (size_t k = 0; k < obstacles_num_to_car; ++k) {
        opt_dual_n[i * obstacles_num_to_car + k] =
            (*(result_ptr->PrepareNResult()))(k, i);
      }
    }
  }
  for (size_t i = 0; i + 1 < size_by_distance; ++i) {
    opt_a[i] = (*(result_ptr->PrepareControlResult()))(1, i);
    opt_steer[i] = (*(result_ptr->PrepareControlResult()))(0, i);
  }
 
  hybrid_time[0] = *(result_ptr->GetHybridTime());
  dual_time[0] = *(result_ptr->GetDualTime());
  ipopt_time[0] = *(result_ptr->GetIpoptTime());
}
};
 
}  // namespace planning
}  // namespace apollo