Apollo 11.0
自动驾驶开放平台
path_optimizer_util.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2023 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17#include "modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.h"
18
19#include <utility>
20#include <vector>
21
22#include "modules/common/configs/vehicle_config_helper.h"
23#include "modules/common/math/math_utils.h"
24#include "modules/planning/planning_base/common/speed/speed_data.h"
25#include "modules/planning/planning_base/common/trajectory1d/piecewise_jerk_trajectory1d.h"
26#include "modules/planning/planning_base/common/util/print_debug_info.h"
27#include "modules/planning/planning_base/gflags/planning_gflags.h"
28#include "modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_path_problem.h"
29namespace apollo {
30namespace planning {
31
32FrenetFramePath PathOptimizerUtil::ToPiecewiseJerkPath(
33 const std::vector<double>& x, const std::vector<double>& dx,
34 const std::vector<double>& ddx, const double delta_s,
35 const double start_s) {
36 ACHECK(!x.empty());
37 ACHECK(!dx.empty());
38 ACHECK(!ddx.empty());
39
40 PiecewiseJerkTrajectory1d piecewise_jerk_traj(x.front(), dx.front(),
41 ddx.front());
42
43 for (std::size_t i = 1; i < x.size(); ++i) {
44 const auto dddl = (ddx[i] - ddx[i - 1]) / delta_s;
45 piecewise_jerk_traj.AppendSegment(dddl, delta_s);
46 }
47
48 std::vector<common::FrenetFramePoint> frenet_frame_path;
49 double accumulated_s = 0.0;
50 while (accumulated_s < piecewise_jerk_traj.ParamLength()) {
51 double l = piecewise_jerk_traj.Evaluate(0, accumulated_s);
52 double dl = piecewise_jerk_traj.Evaluate(1, accumulated_s);
53 double ddl = piecewise_jerk_traj.Evaluate(2, accumulated_s);
54
55 common::FrenetFramePoint frenet_frame_point;
56 frenet_frame_point.set_s(accumulated_s + start_s);
57 frenet_frame_point.set_l(l);
58 frenet_frame_point.set_dl(dl);
59 frenet_frame_point.set_ddl(ddl);
60 frenet_frame_path.push_back(std::move(frenet_frame_point));
61
62 accumulated_s += FLAGS_trajectory_space_resolution;
63 }
64
65 return FrenetFramePath(std::move(frenet_frame_path));
66}
67
68double PathOptimizerUtil::EstimateJerkBoundary(const double vehicle_speed) {
69 const auto& veh_param =
70 common::VehicleConfigHelper::GetConfig().vehicle_param();
71 const double axis_distance = veh_param.wheel_base();
72 const double max_yaw_rate =
73 veh_param.max_steer_angle_rate() / veh_param.steer_ratio();
74 return max_yaw_rate / axis_distance / vehicle_speed;
75}
76
77std::vector<common::PathPoint>
78PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe(
79 const PathData& path_data) {
80 std::vector<common::PathPoint> ret;
81 double front_to_rear_axe_distance =
82 apollo::common::VehicleConfigHelper::GetConfig()
83 .vehicle_param()
84 .wheel_base();
85 for (auto path_point : path_data.discretized_path()) {
86 common::PathPoint new_path_point = path_point;
87 new_path_point.set_x(path_point.x() - front_to_rear_axe_distance *
88 std::cos(path_point.theta()));
89 new_path_point.set_y(path_point.y() - front_to_rear_axe_distance *
90 std::sin(path_point.theta()));
91 ret.push_back(new_path_point);
92 }
93 return ret;
94}
95
96bool PathOptimizerUtil::OptimizePath(
97 const SLState& init_state, const std::array<double, 3>& end_state,
98 std::vector<double> l_ref, std::vector<double> l_ref_weight,
99 const PathBoundary& path_boundary,
100 const std::vector<std::pair<double, double>>& ddl_bounds, double dddl_bound,
101 const PiecewiseJerkPathConfig& config, std::vector<double>* x,
102 std::vector<double>* dx, std::vector<double>* ddx) {
103 // num of knots
104 const auto& lat_boundaries = path_boundary.boundary();
105 const size_t kNumKnots = lat_boundaries.size();
106 AINFO << "kNumKnots: " << kNumKnots;
107
108 double delta_s = path_boundary.delta_s();
109 PiecewiseJerkPathProblem piecewise_jerk_problem(kNumKnots, delta_s,
110 init_state.second);
111
112 const auto& adc_vertex_constraints = path_boundary.adc_vertex_bound();
113 const auto& extra_bound = path_boundary.extra_path_bound();
114 // get reference towing l
115 std::vector<double> towing_l_ref;
116 for (auto& path_boundary_pt : path_boundary) {
117 towing_l_ref.emplace_back(path_boundary_pt.towing_l);
118 }
119 PrintCurves print_curve;
120 for (size_t i = 0; i < adc_vertex_constraints.size(); i++) {
121 print_curve.AddPoint(path_boundary.label() + "_vertex_l_lower",
122 adc_vertex_constraints[i].rear_axle_s,
123 adc_vertex_constraints[i].lower_bound);
124 print_curve.AddPoint(path_boundary.label() + "_vertex_l_upper",
125 adc_vertex_constraints[i].rear_axle_s,
126 adc_vertex_constraints[i].upper_bound);
127 }
128 for (size_t i = 0; i < extra_bound.size(); i++) {
129 print_curve.AddPoint(path_boundary.label() + "_conner_l_lower",
130 extra_bound[i].rear_axle_s,
131 extra_bound[i].lower_bound);
132 print_curve.AddPoint(path_boundary.label() + "_conner_l_upper",
133 extra_bound[i].rear_axle_s,
134 extra_bound[i].upper_bound);
135 }
136 // double adc_half_width =
137 // apollo::common::VehicleConfigHelper::GetConfig().vehicle_param().width()
138 // / 2.0;
139 for (size_t i = 0; i < kNumKnots; i++) {
140 double s = i * path_boundary.delta_s() + path_boundary.start_s();
141 print_curve.AddPoint(path_boundary.label() + "_ref_l", s, l_ref[i]);
142 print_curve.AddPoint(path_boundary.label() + "_towing_ref_l", s,
143 towing_l_ref[i]);
144 print_curve.AddPoint(path_boundary.label() + "_ref_l_weight", s,
145 l_ref_weight[i]);
146 print_curve.AddPoint(path_boundary.label() + "_l_lower", s,
147 lat_boundaries[i].first);
148 print_curve.AddPoint(path_boundary.label() + "_l_upper", s,
149 lat_boundaries[i].second);
150 print_curve.AddPoint(path_boundary.label() + "_ddl_lower", s,
151 ddl_bounds[i].first);
152 print_curve.AddPoint(path_boundary.label() + "_ddl_upper", s,
153 ddl_bounds[i].second);
154 }
155 print_curve.AddPoint(path_boundary.label() + "_opt_l",
156 path_boundary.start_s(), init_state.second[0]);
157 print_curve.AddPoint(path_boundary.label() + "_opt_dl",
158 path_boundary.start_s(), init_state.second[1]);
159 print_curve.AddPoint(path_boundary.label() + "_opt_ddl",
160 path_boundary.start_s(), init_state.second[2]);
161 // TODO(Hongyi): update end_state settings
162 std::array<double, 3U> end_state_weight = {config.weight_end_state_l(),
163 config.weight_end_state_dl(),
164 config.weight_end_state_ddl()};
165 piecewise_jerk_problem.set_end_state_ref(end_state_weight, end_state);
166 piecewise_jerk_problem.set_x_ref(std::move(l_ref_weight), l_ref);
167 piecewise_jerk_problem.set_towing_x_ref(config.l_weight(), towing_l_ref);
168
169 // for debug:here should use std::move
170 piecewise_jerk_problem.set_weight_x(config.l_weight());
171 piecewise_jerk_problem.set_weight_dx(config.dl_weight());
172 piecewise_jerk_problem.set_weight_ddx(config.ddl_weight());
173 piecewise_jerk_problem.set_weight_dddx(config.dddl_weight());
174
175 piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});
176 piecewise_jerk_problem.set_extra_constraints(extra_bound);
177 piecewise_jerk_problem.set_vertex_constraints(adc_vertex_constraints);
178 auto start_time = std::chrono::system_clock::now();
179
180 piecewise_jerk_problem.set_x_bounds(lat_boundaries);
181 piecewise_jerk_problem.set_dx_bounds(
182 -config.lateral_derivative_bound_default(),
183 config.lateral_derivative_bound_default());
184 piecewise_jerk_problem.set_ddx_bounds(ddl_bounds);
185
186 piecewise_jerk_problem.set_dddx_bound(dddl_bound);
187
188 bool success = piecewise_jerk_problem.Optimize(config.max_iteration());
189
190 auto end_time = std::chrono::system_clock::now();
191 std::chrono::duration<double> diff = end_time - start_time;
192 ADEBUG << "Path Optimizer used time: " << diff.count() * 1000 << " ms.";
193
194 if (!success) {
195 AERROR << path_boundary.label() << "piecewise jerk path optimizer failed";
196 AINFO << "init s(" << init_state.first[0] << "," << init_state.first[1]
197 << "," << init_state.first[2] << ") l (" << init_state.second[0]
198 << "," << init_state.second[1] << "," << init_state.second[2];
199 AINFO << "dx bound" << config.lateral_derivative_bound_default();
200 AINFO << "jerk bound" << dddl_bound;
201 print_curve.PrintToLog();
202 return false;
203 }
204
205 *x = piecewise_jerk_problem.opt_x();
206 *dx = piecewise_jerk_problem.opt_dx();
207 *ddx = piecewise_jerk_problem.opt_ddx();
208 PrintBox print_box("opt_l_box");
209 for (size_t i = 0; i < kNumKnots; i++) {
210 double s = i * path_boundary.delta_s() + path_boundary.start_s();
211 print_curve.AddPoint(path_boundary.label() + "_opt_l", s, (*x)[i]);
212 print_curve.AddPoint(path_boundary.label() + "_opt_dl", s, (*dx)[i]);
213 print_curve.AddPoint(path_boundary.label() + "_opt_ddl", s, (*ddx)[i]);
214 print_box.AddAdcBox(s, (*x)[i], std::atan((*dx)[i]), true);
215 }
216 print_curve.PrintToLog();
217 // print_box.PrintToLog();
218 return true;
219}
220
221bool PathOptimizerUtil::OptimizePathWithTowingPoints(
222 const SLState& init_state, const std::array<double, 3>& end_state,
223 std::vector<double> l_ref, std::vector<double> l_ref_weight,
224 std::vector<double> towing_l_ref, std::vector<double> towing_l_ref_weight,
225 const PathBoundary& path_boundary,
226 const std::vector<std::pair<double, double>>& ddl_bounds, double dddl_bound,
227 const PiecewiseJerkPathConfig& config, std::vector<double>* x,
228 std::vector<double>* dx, std::vector<double>* ddx) {
229 // num of knots
230 const auto& lat_boundaries = path_boundary.boundary();
231 const size_t kNumKnots = lat_boundaries.size();
232
233 double delta_s = path_boundary.delta_s();
234 PiecewiseJerkPathProblem piecewise_jerk_problem(kNumKnots, delta_s,
235 init_state.second);
236 const auto& adc_vertex_constraints = path_boundary.adc_vertex_bound();
237 const auto& extra_bound = path_boundary.extra_path_bound();
238 PrintCurves print_curve;
239 for (size_t i = 0; i < adc_vertex_constraints.size(); i++) {
240 print_curve.AddPoint(path_boundary.label() + "_vertex_l_lower",
241 adc_vertex_constraints[i].rear_axle_s,
242 adc_vertex_constraints[i].lower_bound);
243 print_curve.AddPoint(path_boundary.label() + "_vertex_l_upper",
244 adc_vertex_constraints[i].rear_axle_s,
245 adc_vertex_constraints[i].upper_bound);
246 }
247 for (size_t i = 0; i < extra_bound.size(); i++) {
248 print_curve.AddPoint(path_boundary.label() + "_conner_l_lower",
249 extra_bound[i].rear_axle_s,
250 extra_bound[i].lower_bound);
251 print_curve.AddPoint(path_boundary.label() + "_conner_l_upper",
252 extra_bound[i].rear_axle_s,
253 extra_bound[i].upper_bound);
254 }
255
256 for (size_t i = 0; i < kNumKnots; i++) {
257 double s = i * path_boundary.delta_s() + path_boundary.start_s();
258 print_curve.AddPoint(path_boundary.label() + "_ref_l", s, l_ref[i]);
259 print_curve.AddPoint(path_boundary.label() + "_towing_ref_l", s,
260 towing_l_ref[i]);
261 print_curve.AddPoint(path_boundary.label() + "_ref_l_weight", s,
262 l_ref_weight[i]);
263 print_curve.AddPoint(path_boundary.label() + "_l_lower", s,
264 lat_boundaries[i].first);
265 print_curve.AddPoint(path_boundary.label() + "_l_upper", s,
266 lat_boundaries[i].second);
267 print_curve.AddPoint(path_boundary.label() + "_dl_lower", s,
268 -config.lateral_derivative_bound_default());
269 print_curve.AddPoint(path_boundary.label() + "_dl_upper", s,
270 config.lateral_derivative_bound_default());
271 print_curve.AddPoint(path_boundary.label() + "_ddl_lower", s,
272 ddl_bounds[i].first);
273 print_curve.AddPoint(path_boundary.label() + "_ddl_upper", s,
274 ddl_bounds[i].second);
275 }
276 print_curve.AddPoint(path_boundary.label() + "_opt_l",
277 path_boundary.start_s(), init_state.second[0]);
278 print_curve.AddPoint(path_boundary.label() + "_opt_dl",
279 path_boundary.start_s(), init_state.second[1]);
280 print_curve.AddPoint(path_boundary.label() + "_opt_ddl",
281 path_boundary.start_s(), init_state.second[2]);
282 // TODO(Hongyi): update end_state settings
283 // std::array<double, 3U> end_state_weight
284 // = {config.weight_end_state_l(), config.weight_end_state_dl(),
285 // config.weight_end_state_ddl()};
286 // piecewise_jerk_problem.set_end_state_ref(end_state_weight, end_state);
287 piecewise_jerk_problem.set_x_ref(std::move(l_ref_weight), l_ref);
288 piecewise_jerk_problem.set_towing_x_ref(std::move(towing_l_ref_weight),
289 towing_l_ref);
290 // for debug:here should use std::move
291 piecewise_jerk_problem.set_weight_x(config.l_weight());
292 piecewise_jerk_problem.set_weight_dx(config.dl_weight());
293 piecewise_jerk_problem.set_weight_ddx(config.ddl_weight());
294 piecewise_jerk_problem.set_weight_dddx(config.dddl_weight());
295
296 piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});
297 piecewise_jerk_problem.set_extra_constraints(extra_bound);
298 piecewise_jerk_problem.set_vertex_constraints(adc_vertex_constraints);
299 auto start_time = std::chrono::system_clock::now();
300
301 piecewise_jerk_problem.set_x_bounds(lat_boundaries);
302 piecewise_jerk_problem.set_dx_bounds(
303 -config.lateral_derivative_bound_default(),
304 config.lateral_derivative_bound_default());
305 piecewise_jerk_problem.set_ddx_bounds(ddl_bounds);
306
307 piecewise_jerk_problem.set_dddx_bound(dddl_bound);
308
309 bool success = piecewise_jerk_problem.Optimize(config.max_iteration());
310
311 auto end_time = std::chrono::system_clock::now();
312 std::chrono::duration<double> diff = end_time - start_time;
313 ADEBUG << "Path Optimizer used time: " << diff.count() * 1000 << " ms.";
314
315 if (!success) {
316 AERROR << path_boundary.label() << "piecewise jerk path optimizer failed";
317 AINFO << "init s(" << init_state.first[0] << "," << init_state.first[1]
318 << "," << init_state.first[2] << ") l (" << init_state.second[0]
319 << "," << init_state.second[1] << "," << init_state.second[2];
320 AINFO << "dx bound" << config.lateral_derivative_bound_default();
321 AINFO << "jerk bound" << dddl_bound;
322 print_curve.PrintToLog();
323 return false;
324 }
325
326 *x = piecewise_jerk_problem.opt_x();
327 *dx = piecewise_jerk_problem.opt_dx();
328 *ddx = piecewise_jerk_problem.opt_ddx();
329 for (size_t i = 0; i < kNumKnots; i++) {
330 double s = i * path_boundary.delta_s() + path_boundary.start_s();
331 print_curve.AddPoint(path_boundary.label() + "_opt_l", s, (*x)[i]);
332 print_curve.AddPoint(path_boundary.label() + "_opt_dl", s, (*dx)[i]);
333 print_curve.AddPoint(path_boundary.label() + "_opt_ddl", s, (*ddx)[i]);
334 }
335 print_curve.PrintToLog();
336 return true;
337}
338
339void PathOptimizerUtil::UpdatePathRefWithBound(
340 const PathBoundary& path_boundary, double weight,
341 std::vector<double>* ref_l, std::vector<double>* weight_ref_l) {
342 ref_l->resize(path_boundary.size());
343 weight_ref_l->resize(path_boundary.size());
344 const double kEpison = 1e-2;
345 bool need_update_by_left_boundary = false;
346 bool need_update_by_right_boundary = false;
347 for (size_t i = 0; i < ref_l->size(); i++) {
348 need_update_by_left_boundary =
349 path_boundary[i].l_upper.type == BoundType::OBSTACLE &&
350 path_boundary[i].l_upper.l < path_boundary[i].towing_l + kEpison;
351 need_update_by_right_boundary =
352 path_boundary[i].l_lower.type == BoundType::OBSTACLE &&
353 path_boundary[i].l_lower.l > path_boundary[i].towing_l - kEpison;
354
355 if (!need_update_by_left_boundary && !need_update_by_right_boundary) {
356 weight_ref_l->at(i) = 0;
357 continue;
358 }
359
360 double center_ref_l =
361 (path_boundary[i].l_lower.l + path_boundary[i].l_upper.l) / 2.0;
362 weight_ref_l->at(i) = weight;
363
364 if ((path_boundary[i].l_upper.l - path_boundary[i].l_lower.l) <
365 (2 * FLAGS_path_obs_ref_shift_distance + kEpison)) {
366 ref_l->at(i) = center_ref_l;
367 continue;
368 }
369
370 if (need_update_by_left_boundary &&
371 std::fabs(path_boundary[i].l_upper.l -
372 FLAGS_path_obs_ref_shift_distance - ref_l->at(i)) <
373 std::fabs(center_ref_l - ref_l->at(i))) {
374 center_ref_l =
375 path_boundary[i].l_upper.l - FLAGS_path_obs_ref_shift_distance;
376 }
377 if (need_update_by_right_boundary &&
378 std::fabs(path_boundary[i].l_lower.l +
379 FLAGS_path_obs_ref_shift_distance - ref_l->at(i)) <
380 std::fabs(center_ref_l - ref_l->at(i))) {
381 center_ref_l =
382 path_boundary[i].l_lower.l + FLAGS_path_obs_ref_shift_distance;
383 }
384 ref_l->at(i) = center_ref_l;
385
386 AINFO << "need_update_path_ref: s: " << path_boundary[i].s
387 << ", l: " << ref_l->at(i);
388 }
389}
390
391void PathOptimizerUtil::UpdatePathRefWithBound(
392 const PathBoundary& path_boundary, double weight,
393 const std::vector<double>& towing_ref_l, std::vector<double>* ref_l,
394 std::vector<double>* weight_ref_l) {
395 ref_l->resize(path_boundary.size());
396 weight_ref_l->resize(path_boundary.size());
397 const double kEpison = 1e-2;
398 for (size_t i = 0; i < ref_l->size(); i++) {
399 bool is_need_update_path_ref =
400 (path_boundary[i].l_lower.type == BoundType::OBSTACLE ||
401 path_boundary[i].l_upper.type == BoundType::OBSTACLE) &&
402 (path_boundary[i].l_lower.l > towing_ref_l[i] - kEpison ||
403 path_boundary[i].l_upper.l < towing_ref_l[i] + kEpison);
404 if (is_need_update_path_ref) {
405 ref_l->at(i) =
406 (path_boundary[i].l_lower.l + path_boundary[i].l_upper.l) / 2.0;
407 weight_ref_l->at(i) = weight;
408 AINFO << "need_update_path_ref: s: " << path_boundary[i].s
409 << ", l: " << ref_l->at(i);
410 } else {
411 weight_ref_l->at(i) = 0;
412 }
413 }
414}
415
416void PathOptimizerUtil::UpdatePathRefWithBoundInSidePassDirection(
417 const PathBoundary& path_boundary, double weight,
418 std::vector<double>* ref_l, std::vector<double>* weight_ref_l,
419 bool is_left_side_pass) {
420 ref_l->resize(path_boundary.size());
421 weight_ref_l->resize(path_boundary.size());
422 const double kEpison = 1e-2;
423 for (size_t i = 0; i < ref_l->size(); i++) {
424 bool is_need_update_path_ref =
425 ((path_boundary[i].l_lower.type == BoundType::OBSTACLE &&
426 is_left_side_pass) ||
427 (path_boundary[i].l_upper.type == BoundType::OBSTACLE &&
428 !is_left_side_pass)) &&
429 (path_boundary[i].l_lower.l > path_boundary[i].towing_l - kEpison ||
430 path_boundary[i].l_upper.l < path_boundary[i].towing_l + kEpison);
431
432 if (is_need_update_path_ref) {
433 ref_l->at(i) =
434 (path_boundary[i].l_lower.l + path_boundary[i].l_upper.l) / 2.0;
435 weight_ref_l->at(i) = weight;
436 AINFO << "need_update_path_ref: s: " << path_boundary[i].s
437 << ", l: " << ref_l->at(i);
438 } else {
439 weight_ref_l->at(i) = 0;
440 }
441 }
442}
443
444void PathOptimizerUtil::CalculateAccBound(
445 const PathBoundary& path_boundary, const ReferenceLine& reference_line,
446 std::vector<std::pair<double, double>>* ddl_bounds) {
447 const auto& veh_param =
448 common::VehicleConfigHelper::GetConfig().vehicle_param();
449 const double lat_acc_bound =
450 std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) /
451 veh_param.wheel_base();
452 size_t path_boundary_size = path_boundary.boundary().size();
453 for (size_t i = 0; i < path_boundary_size; ++i) {
454 double s = static_cast<double>(i) * path_boundary.delta_s() +
455 path_boundary.start_s();
456 double kappa = reference_line.GetNearestReferencePoint(s).kappa();
457 ddl_bounds->emplace_back(-lat_acc_bound - kappa, lat_acc_bound - kappa);
458 }
459}
460
461} // namespace planning
462} // namespace apollo
apollo::common::VehicleConfigHelper::GetConfig
static const VehicleConfig & GetConfig()
Get the current vehicle configuration.
Definition vehicle_config_helper.cc:47
apollo::planning::PathBoundary::extra_path_bound
const ObsCornerConstraints & extra_path_bound() const
Definition path_boundary.h:135
apollo::planning::PathBoundary::label
const std::string & label() const
Definition path_boundary.cc:51
apollo::planning::PathBoundary::adc_vertex_bound
const ADCVertexConstraints & adc_vertex_bound() const
Definition path_boundary.h:141
apollo::planning::PathBoundary::boundary
std::vector< std::pair< double, double > > boundary() const
Definition path_boundary.cc:41
apollo::planning::PathData::discretized_path
const DiscretizedPath & discretized_path() const
Definition path_data.cc:90
apollo::planning::PathOptimizerUtil::OptimizePathWithTowingPoints
static bool OptimizePathWithTowingPoints(const SLState &init_state, const std::array< double, 3 > &end_state, std::vector< double > l_ref, std::vector< double > l_ref_weight, std::vector< double > towing_l_ref, std::vector< double > towing_l_ref_weight, const PathBoundary &path_boundary, const std::vector< std::pair< double, double > > &ddl_bounds, double dddl_bound, const PiecewiseJerkPathConfig &config, std::vector< double > *x, std::vector< double > *dx, std::vector< double > *ddx)
Definition path_optimizer_util.cc:221
apollo::planning::PathOptimizerUtil::UpdatePathRefWithBoundInSidePassDirection
static void UpdatePathRefWithBoundInSidePassDirection(const PathBoundary &path_boundary, double weight, std::vector< double > *ref_l, std::vector< double > *weight_ref_l, bool is_left_side_pass)
Definition path_optimizer_util.cc:416
apollo::planning::PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe
static std::vector< common::PathPoint > ConvertPathPointRefFromFrontAxeToRearAxe(const PathData &path_data)
Definition path_optimizer_util.cc:78
apollo::planning::PathOptimizerUtil::CalculateAccBound
static void CalculateAccBound(const PathBoundary &path_boundary, const ReferenceLine &reference_line, std::vector< std::pair< double, double > > *ddl_bounds)
calculate ddl bound by referenceline kappa and adc lat accleration
Definition path_optimizer_util.cc:444
apollo::planning::PathOptimizerUtil::ToPiecewiseJerkPath
static FrenetFramePath ToPiecewiseJerkPath(const std::vector< double > &x, const std::vector< double > &dx, const std::vector< double > &ddx, const double delta_s, const double start_s)
Data format tramform from raw data to FrenetFramePath
Definition path_optimizer_util.cc:32
apollo::planning::PathOptimizerUtil::EstimateJerkBoundary
static double EstimateJerkBoundary(const double vehicle_speed)
Calculation of jerk boundary based on vehicle kinematics model.
Definition path_optimizer_util.cc:68
apollo::planning::PathOptimizerUtil::OptimizePath
static bool OptimizePath(const SLState &init_state, const std::array< double, 3 > &end_state, std::vector< double > l_ref, std::vector< double > l_ref_weight, const PathBoundary &path_boundary, const std::vector< std::pair< double, double > > &ddl_bounds, double dddl_bound, const PiecewiseJerkPathConfig &config, std::vector< double > *x, std::vector< double > *dx, std::vector< double > *ddx)
Piecewise jerk path optimizer.
Definition path_optimizer_util.cc:96
apollo::planning::PathOptimizerUtil::UpdatePathRefWithBound
static void UpdatePathRefWithBound(const PathBoundary &path_boundary, double weight, std::vector< double > *ref_l, std::vector< double > *weight_ref_l)
If ref_l is below or above path boundary, will update its values and weights
Definition path_optimizer_util.cc:339
apollo::planning::PiecewiseJerkPathProblem::set_vertex_constraints
void set_vertex_constraints(const ADCVertexConstraints &vertexs)
Definition piecewise_jerk_path_problem.h:59
apollo::planning::PiecewiseJerkPathProblem::set_extra_constraints
void set_extra_constraints(const ObsCornerConstraints &extra_constraints)
Definition piecewise_jerk_path_problem.h:55
apollo::planning::PiecewiseJerkProblem::set_x_ref
void set_x_ref(const double weight_x_ref, std::vector< double > x_ref)
Set the x ref object and the uniform x_ref weighting
Definition piecewise_jerk_problem.cc:313
apollo::planning::PiecewiseJerkProblem::opt_x
const std::vector< double > & opt_x() const
Definition piecewise_jerk_problem.h:125
apollo::planning::PiecewiseJerkProblem::set_scale_factor
void set_scale_factor(const std::array< double, 3 > &scale_factor)
Definition piecewise_jerk_problem.h:87
apollo::planning::PiecewiseJerkProblem::set_end_state_ref
void set_end_state_ref(const std::array< double, 3 > &weight_end_state, const std::array< double, 3 > &end_state_ref)
Definition piecewise_jerk_problem.cc:352
apollo::planning::PiecewiseJerkProblem::set_dddx_bound
void set_dddx_bound(const double dddx_bound)
Definition piecewise_jerk_problem.h:69
apollo::planning::PiecewiseJerkProblem::set_x_bounds
void set_x_bounds(std::vector< std::pair< double, double > > x_bounds)
Definition piecewise_jerk_problem.cc:271
apollo::planning::PiecewiseJerkProblem::Optimize
virtual bool Optimize(const int max_iter=4000)
Definition piecewise_jerk_problem.cc:90
apollo::planning::PiecewiseJerkProblem::set_weight_ddx
void set_weight_ddx(const double weight_ddx)
Definition piecewise_jerk_problem.h:83
apollo::planning::PiecewiseJerkProblem::set_dx_bounds
void set_dx_bounds(std::vector< std::pair< double, double > > dx_bounds)
Definition piecewise_jerk_problem.cc:277
apollo::planning::PiecewiseJerkProblem::opt_dx
const std::vector< double > & opt_dx() const
Definition piecewise_jerk_problem.h:127
apollo::planning::PiecewiseJerkProblem::opt_ddx
const std::vector< double > & opt_ddx() const
Definition piecewise_jerk_problem.h:129
apollo::planning::PiecewiseJerkProblem::set_towing_x_ref
void set_towing_x_ref(const double weight_towing_x_ref, std::vector< double > towing_x_ref)
Set towing x ref object and piecewised towing_x_ref weightings
Definition piecewise_jerk_problem.cc:333
apollo::planning::PiecewiseJerkProblem::set_weight_dddx
void set_weight_dddx(const double weight_dddx)
Definition piecewise_jerk_problem.h:85
apollo::planning::PiecewiseJerkProblem::set_ddx_bounds
void set_ddx_bounds(std::vector< std::pair< double, double > > ddx_bounds)
Definition piecewise_jerk_problem.cc:283
apollo::planning::PiecewiseJerkTrajectory1d::AppendSegment
void AppendSegment(const double jerk, const double param)
Definition piecewise_jerk_trajectory1d.cc:40
apollo::planning::PiecewiseJerkTrajectory1d::Evaluate
double Evaluate(const std::uint32_t order, const double param) const
Definition piecewise_jerk_trajectory1d.cc:55
apollo::planning::PrintBox::AddAdcBox
void AddAdcBox(double x, double y, double heading, bool is_rear_axle_point=true)
Definition print_debug_info.cc:87
apollo::planning::PrintCurves::AddPoint
void AddPoint(std::string key, double x, double y)
add point to curve key
Definition print_debug_info.cc:48
apollo::planning::ReferenceLine::GetNearestReferencePoint
ReferencePoint GetNearestReferencePoint(const common::math::Vec2d &xy) const
Definition reference_line.cc:130
planning
Planning module main class.
math_utils.h
Math-related util functions.
ACHECK
#define ACHECK(cond)
Definition log.h:80
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
apollo::planning::SLState
std::pair< std::array< double, 3 >, std::array< double, 3 > > SLState
Definition path_bounds_decider_util.h:34
apollo
class register implement
Definition arena_queue.h:37
path_optimizer_util.h
piecewise_jerk_path_problem.h
piecewise_jerk_trajectory1d.h
planning_gflags.h
print_debug_info.h
speed_data.h
apollo::common::VehicleConfig::vehicle_param
optional VehicleParam vehicle_param
Definition vehicle_config.proto:99
vehicle_config_helper.h