apollo
apollo copied to clipboard
question about UpdateLateralBoundsByObstacle in path_time_graph.cc
Why are both start_iter and end_iter found using sl_boundary.start_s()? Is this a bug? Shouldn't end_iter be found using sl_boundary.end_s()?
void PathTimeGraph::UpdateLateralBoundsByObstacle(
const SLBoundary& sl_boundary, const std::vector<double>& discretized_path,
const double s_start, const double s_end,
std::vector<std::pair<double, double>>* const bounds) {
if (sl_boundary.start_s() > s_end || sl_boundary.end_s() < s_start) {
return;
}
auto start_iter = std::lower_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
auto end_iter = std::upper_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
size_t start_index = start_iter - discretized_path.begin();
size_t end_index = end_iter - discretized_path.begin();
if (sl_boundary.end_l() > -FLAGS_numerical_epsilon &&
sl_boundary.start_l() < FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < end_index; ++i) {
bounds->operator[](i).first = -FLAGS_numerical_epsilon;
bounds->operator[](i).second = FLAGS_numerical_epsilon;
}
return;
}
if (sl_boundary.end_l() < FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
bounds->operator[](i).first =
std::max(bounds->operator[](i).first,
sl_boundary.end_l() + FLAGS_nudge_buffer);
}
return;
}
if (sl_boundary.start_l() > -FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
bounds->operator[](i).second =
std::min(bounds->operator[](i).second,
sl_boundary.start_l() - FLAGS_nudge_buffer);
}
return;
}
}