Apollo 10.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::planning::DistanceApproachProblem类 参考

#include <distance_approach_problem.h>

apollo::planning::DistanceApproachProblem 的协作图:

Public 成员函数

 DistanceApproachProblem (const PlannerOpenSpaceConfig &planner_open_space_config)
 
virtual ~DistanceApproachProblem ()=default
 
bool Solve (const Eigen::MatrixXd &x0, const Eigen::MatrixXd &xF, const Eigen::MatrixXd &last_time_u, const size_t horizon, const double ts, const Eigen::MatrixXd &ego, const Eigen::MatrixXd &xWS, const Eigen::MatrixXd &uWS, const Eigen::MatrixXd &l_warm_up, const Eigen::MatrixXd &n_warm_up, const Eigen::MatrixXd &s_warm_up, const std::vector< double > &XYbounds, const size_t obstacles_num, const Eigen::MatrixXi &obstacles_edges_num, const Eigen::MatrixXd &obstacles_A, const Eigen::MatrixXd &obstacles_b, Eigen::MatrixXd *state_result, Eigen::MatrixXd *control_result, Eigen::MatrixXd *time_result, Eigen::MatrixXd *dual_l_result, Eigen::MatrixXd *dual_n_result)
 
 DistanceApproachProblem (const DistanceApproachConfig &distance_approach_config)
 

详细描述

在文件 distance_approach_problem.h 第 43 行定义.

构造及析构函数说明

◆ DistanceApproachProblem() [1/2]

apollo::planning::DistanceApproachProblem::DistanceApproachProblem ( const PlannerOpenSpaceConfig &  planner_open_space_config)
explicit

在文件 distance_approach_problem.cc 第 31 行定义.

32 {
33 planner_open_space_config_ = planner_open_space_config;
34}

◆ ~DistanceApproachProblem()

virtual apollo::planning::DistanceApproachProblem::~DistanceApproachProblem ( )
virtualdefault

◆ DistanceApproachProblem() [2/2]

apollo::planning::DistanceApproachProblem::DistanceApproachProblem ( const DistanceApproachConfig &  distance_approach_config)
explicit

成员函数说明

◆ Solve()

bool apollo::planning::DistanceApproachProblem::Solve ( const Eigen::MatrixXd &  x0,
const Eigen::MatrixXd &  xF,
const Eigen::MatrixXd &  last_time_u,
const size_t  horizon,
const double  ts,
const Eigen::MatrixXd &  ego,
const Eigen::MatrixXd &  xWS,
const Eigen::MatrixXd &  uWS,
const Eigen::MatrixXd &  l_warm_up,
const Eigen::MatrixXd &  n_warm_up,
const Eigen::MatrixXd &  s_warm_up,
const std::vector< double > &  XYbounds,
const size_t  obstacles_num,
const Eigen::MatrixXi &  obstacles_edges_num,
const Eigen::MatrixXd &  obstacles_A,
const Eigen::MatrixXd &  obstacles_b,
Eigen::MatrixXd *  state_result,
Eigen::MatrixXd *  control_result,
Eigen::MatrixXd *  time_result,
Eigen::MatrixXd *  dual_l_result,
Eigen::MatrixXd *  dual_n_result 
)

在文件 distance_approach_problem.cc 第 36 行定义.

47 {
48 // TODO(QiL) : evaluate whether need to new it everytime
49 PERF_BLOCK_START();
50
51 DistanceApproachInterface* ptop = nullptr;
52
53 if (planner_open_space_config_.distance_approach_config()
54 .distance_approach_mode() == DISTANCE_APPROACH_IPOPT) {
55 ptop = new DistanceApproachIPOPTInterface(
56 horizon, ts, ego, xWS, uWS, l_warm_up, n_warm_up, x0, xF, last_time_u,
57 XYbounds, obstacles_edges_num, obstacles_num, obstacles_A, obstacles_b,
58 planner_open_space_config_);
59 } else if (planner_open_space_config_.distance_approach_config()
60 .distance_approach_mode() ==
61 DISTANCE_APPROACH_IPOPT_FIXED_TS) {
62 ptop = new DistanceApproachIPOPTFixedTsInterface(
63 horizon, ts, ego, xWS, uWS, l_warm_up, n_warm_up, x0, xF, last_time_u,
64 XYbounds, obstacles_edges_num, obstacles_num, obstacles_A, obstacles_b,
65 planner_open_space_config_);
66 } else if (planner_open_space_config_.distance_approach_config()
67 .distance_approach_mode() == DISTANCE_APPROACH_IPOPT_CUDA) {
68 ptop = new DistanceApproachIPOPTCUDAInterface(
69 horizon, ts, ego, xWS, uWS, l_warm_up, n_warm_up, x0, xF, last_time_u,
70 XYbounds, obstacles_edges_num, obstacles_num, obstacles_A, obstacles_b,
71 planner_open_space_config_);
72 } else if (planner_open_space_config_.distance_approach_config()
73 .distance_approach_mode() ==
74 DISTANCE_APPROACH_IPOPT_FIXED_DUAL) {
75 ptop = new DistanceApproachIPOPTFixedDualInterface(
76 horizon, ts, ego, xWS, uWS, l_warm_up, n_warm_up, x0, xF, last_time_u,
77 XYbounds, obstacles_edges_num, obstacles_num, obstacles_A, obstacles_b,
78 planner_open_space_config_);
79 } else if (planner_open_space_config_.distance_approach_config()
80 .distance_approach_mode() ==
81 DISTANCE_APPROACH_IPOPT_RELAX_END) {
82 ptop = new DistanceApproachIPOPTRelaxEndInterface(
83 horizon, ts, ego, xWS, uWS, l_warm_up, n_warm_up, x0, xF, last_time_u,
84 XYbounds, obstacles_edges_num, obstacles_num, obstacles_A, obstacles_b,
85 planner_open_space_config_);
86 } else if (planner_open_space_config_.distance_approach_config()
87 .distance_approach_mode() ==
88 DISTANCE_APPROACH_IPOPT_RELAX_END_SLACK) {
89 ptop = new DistanceApproachIPOPTRelaxEndSlackInterface(
90 horizon, ts, ego, xWS, uWS, l_warm_up, n_warm_up, s_warm_up, x0, xF,
91 last_time_u, XYbounds, obstacles_edges_num, obstacles_num, obstacles_A,
92 obstacles_b, planner_open_space_config_);
93 }
94
95 Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;
96
97 // Create an instance of the IpoptApplication
98 Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
99
100 app->Options()->SetIntegerValue(
101 "print_level", planner_open_space_config_.distance_approach_config()
102 .ipopt_config()
103 .ipopt_print_level());
104 app->Options()->SetIntegerValue(
105 "mumps_mem_percent", planner_open_space_config_.distance_approach_config()
106 .ipopt_config()
107 .mumps_mem_percent());
108 app->Options()->SetNumericValue(
109 "mumps_pivtol", planner_open_space_config_.distance_approach_config()
110 .ipopt_config()
111 .mumps_pivtol());
112 app->Options()->SetIntegerValue(
113 "max_iter", planner_open_space_config_.distance_approach_config()
114 .ipopt_config()
115 .ipopt_max_iter());
116 app->Options()->SetNumericValue(
117 "tol", planner_open_space_config_.distance_approach_config()
118 .ipopt_config()
119 .ipopt_tol());
120 app->Options()->SetNumericValue(
121 "acceptable_constr_viol_tol",
122 planner_open_space_config_.distance_approach_config()
123 .ipopt_config()
124 .ipopt_acceptable_constr_viol_tol());
125 app->Options()->SetNumericValue(
126 "min_hessian_perturbation",
127 planner_open_space_config_.distance_approach_config()
128 .ipopt_config()
129 .ipopt_min_hessian_perturbation());
130 app->Options()->SetNumericValue(
131 "jacobian_regularization_value",
132 planner_open_space_config_.distance_approach_config()
133 .ipopt_config()
134 .ipopt_jacobian_regularization_value());
135 app->Options()->SetStringValue(
136 "print_timing_statistics",
137 planner_open_space_config_.distance_approach_config()
138 .ipopt_config()
139 .ipopt_print_timing_statistics());
140 app->Options()->SetStringValue(
141 "alpha_for_y", planner_open_space_config_.distance_approach_config()
142 .ipopt_config()
143 .ipopt_alpha_for_y());
144 app->Options()->SetStringValue(
145 "recalc_y", planner_open_space_config_.distance_approach_config()
146 .ipopt_config()
147 .ipopt_recalc_y());
148 app->Options()->SetNumericValue(
149 "mu_init", planner_open_space_config_.distance_approach_config()
150 .ipopt_config()
151 .ipopt_mu_init());
152
153 Ipopt::ApplicationReturnStatus status = app->Initialize();
154 if (status != Ipopt::Solve_Succeeded) {
155 AERROR << "*** Distance Approach problem error during initialization!";
156 return false;
157 }
158
159 status = app->OptimizeTNLP(problem);
160
161 if (status == Ipopt::Solve_Succeeded ||
162 status == Ipopt::Solved_To_Acceptable_Level) {
163 // Retrieve some statistics about the solve
164 Ipopt::Index iter_count = app->Statistics()->IterationCount();
165 ADEBUG << "*** The problem solved in " << iter_count << " iterations!";
166
167 Ipopt::Number final_obj = app->Statistics()->FinalObjective();
168 ADEBUG << "*** The final value of the objective function is " << final_obj
169 << '.';
170
171 PERF_BLOCK_END("DistanceApproachProblemSolving");
172 } else {
173 /*
174 return detailed failure information,
175 reference resource: Ipopt::ApplicationReturnStatus, https://
176 www.coin-or.org/Doxygen/CoinAll/_ip_return_codes__inc_8h-source.html
177 */
178 std::unordered_map<int, std::string> failure_status = {
179 {0, "Solve_Succeeded"},
180 {1, "Solved_To_Acceptable_Level"},
181 {2, "Infeasible_Problem_Detected"},
182 {3, "Search_Direction_Becomes_Too_Small"},
183 {4, "Diverging_Iterates"},
184 {5, "User_Requested_Stop"},
185 {6, "Feasible_Point_Found"},
186 {-1, "Maximum_Iterations_Exceeded"},
187 {-2, "Restoration_Failed"},
188 {-3, "Error_In_Step_Computation"},
189 {-10, "Not_Enough_Degrees_Of_Freedom"},
190 {-11, "Invalid_Problem_Definition"},
191 {-12, "Invalid_Option"},
192 {-13, "Invalid_Number_Detected"},
193 {-100, "Unrecoverable_Exception"},
194 {-101, "NonIpopt_Exception_Thrown"},
195 {-102, "Insufficient_Memory"},
196 {-199, "Internal_Error"}};
197
198 if (!failure_status.count(static_cast<size_t>(status))) {
199 AINFO << "Solver ends with unknown failure code: "
200 << static_cast<int>(status);
201 } else {
202 AINFO << "Solver failure case: "
203 << failure_status[static_cast<size_t>(status)];
204 }
205 }
206
207 ptop->get_optimization_results(state_result, control_result, time_result,
208 dual_l_result, dual_n_result);
209
210 return status == Ipopt::Solve_Succeeded ||
211 status == Ipopt::Solved_To_Acceptable_Level;
212}
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
PERF_BLOCK_START
#define PERF_BLOCK_START()
Definition perf_util.h:126
PERF_BLOCK_END
#define PERF_BLOCK_END
Definition profiler.h:53
apollo::planning::PlannerOpenSpaceConfig::distance_approach_config
optional DistanceApproachConfig distance_approach_config
Definition planner_open_space_config.proto:32
该类的文档由以下文件生成: