Apollo 11.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 行定义.

31 {
32 planner_open_space_config_ = planner_open_space_config;
33 distance_approach_config_ = distance_approach_config_;
34}

◆ ~DistanceApproachProblem()

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

◆ DistanceApproachProblem() [2/2]

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

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

36 {
37 distance_approach_config_ = distance_approach_config;
38}

成员函数说明

◆ 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 第 40 行定义.

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