apollo icon indicating copy to clipboard operation
apollo copied to clipboard

question about UpdateLateralBoundsByObstacle in path_time_graph.cc

Open dajiba6 opened this issue 1 year ago • 0 comments

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;
  }
}

dajiba6 avatar Jul 25 '24 06:07 dajiba6