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

#include <path_optimizer_util.h>

apollo::planning::PathOptimizerUtil 的协作图:

静态 Public 成员函数

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
 
static double EstimateJerkBoundary (const double vehicle_speed)
 Calculation of jerk boundary based on vehicle kinematics model.
 
static std::vector< common::PathPoint > ConvertPathPointRefFromFrontAxeToRearAxe (const PathData &path_data)
 
static void FormulateExtraConstraints (PathBound extra_path_bound, const PathBoundary &path_boundary, ObsCornerConstraints *extra_constraints)
 
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.
 
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)
 
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
 
static void UpdatePathRefWithBound (const PathBoundary &path_boundary, double weight, const std::vector< double > &towing_ref_l, std::vector< double > *ref_l, std::vector< double > *weight_ref_l)
 
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)
 
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
 

详细描述

在文件 path_optimizer_util.h 第 30 行定义.

成员函数说明

◆ CalculateAccBound()

void apollo::planning::PathOptimizerUtil::CalculateAccBound ( const PathBoundary &  path_boundary,
const ReferenceLine &  reference_line,
std::vector< std::pair< double, double > > *  ddl_bounds 
)
static

calculate ddl bound by referenceline kappa and adc lat accleration

在文件 path_optimizer_util.cc 第 444 行定义.

446 {
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}
apollo::common::VehicleConfigHelper::GetConfig
static const VehicleConfig & GetConfig()
Get the current vehicle configuration.
Definition vehicle_config_helper.cc:47
apollo::common::VehicleConfig::vehicle_param
optional VehicleParam vehicle_param
Definition vehicle_config.proto:99

◆ ConvertPathPointRefFromFrontAxeToRearAxe()

std::vector< common::PathPoint > apollo::planning::PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe ( const PathData &  path_data)
static

在文件 path_optimizer_util.cc 第 78 行定义.

79 {
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}

◆ EstimateJerkBoundary()

double apollo::planning::PathOptimizerUtil::EstimateJerkBoundary ( const double  vehicle_speed)
static

Calculation of jerk boundary based on vehicle kinematics model.

在文件 path_optimizer_util.cc 第 68 行定义.

68 {
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}

◆ FormulateExtraConstraints()

static void apollo::planning::PathOptimizerUtil::FormulateExtraConstraints ( PathBound  extra_path_bound,
const PathBoundary &  path_boundary,
ObsCornerConstraints *  extra_constraints 
)
static

◆ OptimizePath()

bool apollo::planning::PathOptimizerUtil::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 
)
static

Piecewise jerk path optimizer.

在文件 path_optimizer_util.cc 第 96 行定义.

102 {
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}
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
apollo::perception::lidar::diff
float diff(Image< float > *I, int x1, int y1, int x2, int y2)
Dissimilarity measure between pixels
Definition segment_image.h:55

◆ OptimizePathWithTowingPoints()

bool apollo::planning::PathOptimizerUtil::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 
)
static

在文件 path_optimizer_util.cc 第 221 行定义.

228 {
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}

◆ ToPiecewiseJerkPath()

FrenetFramePath apollo::planning::PathOptimizerUtil::ToPiecewiseJerkPath ( const std::vector< double > &  x,
const std::vector< double > &  dx,
const std::vector< double > &  ddx,
const double  delta_s,
const double  start_s 
)
static

Data format tramform from raw data to FrenetFramePath

在文件 path_optimizer_util.cc 第 32 行定义.

35 {
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}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ UpdatePathRefWithBound() [1/2]

void apollo::planning::PathOptimizerUtil::UpdatePathRefWithBound ( const PathBoundary &  path_boundary,
double  weight,
const std::vector< double > &  towing_ref_l,
std::vector< double > *  ref_l,
std::vector< double > *  weight_ref_l 
)
static

在文件 path_optimizer_util.cc 第 391 行定义.

394 {
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}

◆ UpdatePathRefWithBound() [2/2]

void apollo::planning::PathOptimizerUtil::UpdatePathRefWithBound ( const PathBoundary &  path_boundary,
double  weight,
std::vector< double > *  ref_l,
std::vector< double > *  weight_ref_l 
)
static

If ref_l is below or above path boundary, will update its values and weights

在文件 path_optimizer_util.cc 第 339 行定义.

341 {
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}

◆ UpdatePathRefWithBoundInSidePassDirection()

void apollo::planning::PathOptimizerUtil::UpdatePathRefWithBoundInSidePassDirection ( const PathBoundary &  path_boundary,
double  weight,
std::vector< double > *  ref_l,
std::vector< double > *  weight_ref_l,
bool  is_left_side_pass 
)
static

在文件 path_optimizer_util.cc 第 416 行定义.

419 {
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}
该类的文档由以下文件生成: