Skip to content

Commit

Permalink
Merge pull request autowarefoundation#1544 from tier4/fix/typo
Browse files Browse the repository at this point in the history
fix: typo
  • Loading branch information
rej55 authored Sep 19, 2024
2 parents 625605a + 0726690 commit f675b2d
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 25 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -258,10 +258,10 @@ int main(int argc, char ** argv)
const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo();
autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{};
lane_departure_checker.setVehicleInfo(vehicle_info);
autoware::lane_departure_checker::Param lane_depature_checker_params;
lane_depature_checker_params.footprint_extra_margin =
autoware::lane_departure_checker::Param lane_departure_checker_params;
lane_departure_checker_params.footprint_extra_margin =
goal_planner_parameter.lane_departure_check_expansion_margin;
lane_departure_checker.setParam(lane_depature_checker_params);
lane_departure_checker.setParam(lane_departure_checker_params);
auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver(
*node, goal_planner_parameter, lane_departure_checker);
const auto pull_over_path_opt =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -217,31 +217,32 @@ std::optional<size_t> getFirstPointInsidePolygon(
}

void retrievePathsBackward(
const std::vector<std::vector<bool>> & adjacency, const size_t src_ind,
const std::vector<size_t> & visited_inds, std::vector<std::vector<size_t>> & paths)
const std::vector<std::vector<bool>> & adjacency, const size_t src_index,
const std::vector<size_t> & visited_indices, std::vector<std::vector<size_t>> & paths)
{
const auto & nexts = adjacency.at(src_ind);
const bool is_terminal = (std::find(nexts.begin(), nexts.end(), true) == nexts.end());
const auto & next_indices = adjacency.at(src_index);
const bool is_terminal =
(std::find(next_indices.begin(), next_indices.end(), true) == next_indices.end());
if (is_terminal) {
std::vector<size_t> path(visited_inds.begin(), visited_inds.end());
path.push_back(src_ind);
std::vector<size_t> path(visited_indices.begin(), visited_indices.end());
path.push_back(src_index);
paths.emplace_back(std::move(path));
return;
}
for (size_t next = 0; next < nexts.size(); next++) {
if (!nexts.at(next)) {
for (size_t next = 0; next < next_indices.size(); next++) {
if (!next_indices.at(next)) {
continue;
}
if (std::find(visited_inds.begin(), visited_inds.end(), next) != visited_inds.end()) {
if (std::find(visited_indices.begin(), visited_indices.end(), next) != visited_indices.end()) {
// loop detected
std::vector<size_t> path(visited_inds.begin(), visited_inds.end());
path.push_back(src_ind);
std::vector<size_t> path(visited_indices.begin(), visited_indices.end());
path.push_back(src_index);
paths.emplace_back(std::move(path));
continue;
}
auto new_visited_inds = visited_inds;
new_visited_inds.push_back(src_ind);
retrievePathsBackward(adjacency, next, new_visited_inds, paths);
auto new_visited_indices = visited_indices;
new_visited_indices.push_back(src_index);
retrievePathsBackward(adjacency, next, new_visited_indices, paths);
}
return;
}
Expand All @@ -263,10 +264,10 @@ mergeLaneletsByTopologicalSort(
ind2Id[ind] = Id;
Id2lanelet[Id] = lanelet;
}
std::set<size_t> terminal_inds;
std::set<size_t> terminal_indices;
for (const auto & terminal_lanelet : terminal_lanelets) {
if (Id2ind.count(terminal_lanelet.id()) > 0) {
terminal_inds.insert(Id2ind[terminal_lanelet.id()]);
terminal_indices.insert(Id2ind[terminal_lanelet.id()]);
}
}

Expand All @@ -292,7 +293,7 @@ mergeLaneletsByTopologicalSort(
}

std::unordered_map<size_t, std::vector<std::vector<size_t>>> branches;
for (const auto & terminal_ind : terminal_inds) {
for (const auto & terminal_ind : terminal_indices) {
std::vector<std::vector<size_t>> paths;
std::vector<size_t> visited;
retrievePathsBackward(adjacency, terminal_ind, visited, paths);
Expand All @@ -311,11 +312,11 @@ mergeLaneletsByTopologicalSort(
if (sub_branches.size() == 0) {
continue;
}
for (const auto & sub_inds : sub_branches) {
for (const auto & sub_indices : sub_branches) {
lanelet::ConstLanelets to_be_merged;
originals.push_back(lanelet::ConstLanelets({}));
auto & original = originals.back();
for (const auto & sub_ind : sub_inds) {
for (const auto & sub_ind : sub_indices) {
to_be_merged.push_back(Id2lanelet[ind2Id[sub_ind]]);
original.push_back(Id2lanelet[ind2Id[sub_ind]]);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -105,11 +105,11 @@ mergeLaneletsByTopologicalSort(

/**
* @brief this functions retrieves all the paths from the given source to terminal nodes on the tree
@param[in] visited_inds visited node indices excluding src_ind so far
@param[in] visited_indices visited node indices excluding src_ind so far
*/
void retrievePathsBackward(
const std::vector<std::vector<bool>> & adjacency, const size_t src_ind,
const std::vector<size_t> & visited_inds, std::vector<std::vector<size_t>> & paths);
const std::vector<std::vector<bool>> & adjacency, const size_t src_index,
const std::vector<size_t> & visited_indices, std::vector<std::vector<size_t>> & paths);

/**
* @brief find the index of the first point where vehicle footprint intersects with the given
Expand Down

0 comments on commit f675b2d

Please sign in to comment.