Skip to content

Commit

Permalink
fix(intersection): additional fix for 8520 (autowarefoundation#8561)
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin @saka1-s
soblin authored and saka1-s committed Nov 27, 2024
1 parent 2ae8bb8 commit f4eeba7
Show file tree
Hide file tree
Showing 4 changed files with 75 additions and 71 deletions.
9 changes: 8 additions & 1 deletion ...er/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -934,6 +934,14 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan
detection_lanelets.push_back(detection_lanelet);
}

std::vector<lanelet::ConstLineString3d> detection_divisions;
if (detection_lanelets.empty()) {
// NOTE(soblin): due to the above filtering detection_lanelets may be empty or do not contain
// conflicting_detection_lanelets
// OK to return empty detction_divsions
return detection_divisions;
}

// (1) tsort detection_lanelets
const auto [merged_detection_lanelets, originals] = util::mergeLaneletsByTopologicalSort(
detection_lanelets, conflicting_detection_lanelets, routing_graph_ptr);
Expand All @@ -952,7 +960,6 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan
}

// (3) discretize each merged lanelet
std::vector<lanelet::ConstLineString3d> detection_divisions;
for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) {
const double length = bg::length(merged_lanelet.centerline());
const double width = area / length;
Expand Down
97 changes: 53 additions & 44 deletions ...ing/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,13 @@

#include <algorithm>
#include <cmath>
#include <limits>
#include <map>
#include <memory>
#include <queue>
#include <set>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace autoware::behavior_velocity_planner::util
Expand Down Expand Up @@ -212,42 +215,35 @@ std::optional<size_t> getFirstPointInsidePolygon(
}
return std::nullopt;
}
std::vector<std::vector<size_t>> retrievePathsBackward(
const std::vector<std::vector<bool>> & adjacency, size_t start_node)

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)
{
std::vector<std::vector<size_t>> paths;
std::vector<size_t> current_path;
std::unordered_set<size_t> visited;

std::function<void(size_t)> retrieve_paths_backward_impl = [&](size_t src_ind) {
current_path.push_back(src_ind);
visited.insert(src_ind);

bool is_terminal = true;
const auto & nexts = adjacency[src_ind];

for (size_t next = 0; next < nexts.size(); ++next) {
if (nexts[next]) {
is_terminal = false;
if (visited.find(next) == visited.end()) {
retrieve_paths_backward_impl(next);
} else {
// Loop detected
paths.push_back(current_path);
}
}
const auto & nexts = adjacency.at(src_ind);
const bool is_terminal = (std::find(nexts.begin(), nexts.end(), true) == nexts.end());
if (is_terminal) {
std::vector<size_t> path(visited_inds.begin(), visited_inds.end());
path.push_back(src_ind);
paths.emplace_back(std::move(path));
return;
}
for (size_t next = 0; next < nexts.size(); next++) {
if (!nexts.at(next)) {
continue;
}

if (is_terminal) {
paths.push_back(current_path);
if (std::find(visited_inds.begin(), visited_inds.end(), next) != visited_inds.end()) {
// loop detected
std::vector<size_t> path(visited_inds.begin(), visited_inds.end());
path.push_back(src_ind);
paths.emplace_back(std::move(path));
continue;
}

current_path.pop_back();
visited.erase(src_ind);
};

retrieve_paths_backward_impl(start_node);
return paths;
auto new_visited_inds = visited_inds;
new_visited_inds.push_back(src_ind);
retrievePathsBackward(adjacency, next, new_visited_inds, paths);
}
return;
}

std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
Expand All @@ -269,13 +265,20 @@ mergeLaneletsByTopologicalSort(
}
std::set<size_t> terminal_inds;
for (const auto & terminal_lanelet : terminal_lanelets) {
terminal_inds.insert(Id2ind[terminal_lanelet.id()]);
if (Id2ind.count(terminal_lanelet.id()) > 0) {
terminal_inds.insert(Id2ind[terminal_lanelet.id()]);
}
}

// create adjacency matrix
const auto n_node = lanelets.size();
std::vector<std::vector<bool>> adjacency(n_node, std::vector<bool>(n_node, false));

std::vector<std::vector<bool>> adjacency(n_node);
for (size_t dst = 0; dst < n_node; ++dst) {
adjacency[dst].resize(n_node);
for (size_t src = 0; src < n_node; ++src) {
adjacency[dst][src] = false;
}
}
// NOTE: this function aims to traverse the detection lanelet in the lane direction, so if lane B
// follows lane A on the routing_graph, adj[A][B] = true
for (const auto & lanelet : lanelets) {
Expand All @@ -290,27 +293,33 @@ mergeLaneletsByTopologicalSort(

std::unordered_map<size_t, std::vector<std::vector<size_t>>> branches;
for (const auto & terminal_ind : terminal_inds) {
branches[terminal_ind] = retrievePathsBackward(adjacency, terminal_ind);
std::vector<std::vector<size_t>> paths;
std::vector<size_t> visited;
retrievePathsBackward(adjacency, terminal_ind, visited, paths);
branches[terminal_ind] = std::move(paths);
}

for (auto & [terminal_ind, paths] : branches) {
for (auto it = branches.begin(); it != branches.end(); it++) {
auto & paths = it->second;
for (auto & path : paths) {
std::reverse(path.begin(), path.end());
}
}
lanelet::ConstLanelets merged;
std::vector<lanelet::ConstLanelets> originals;
for (const auto & [ind, sub_branches] : branches) {
if (sub_branches.empty()) {
if (sub_branches.size() == 0) {
continue;
}
for (const auto & sub_inds : sub_branches) {
lanelet::ConstLanelets to_merge;
lanelet::ConstLanelets to_be_merged;
originals.push_back(lanelet::ConstLanelets({}));
auto & original = originals.back();
for (const auto & sub_ind : sub_inds) {
to_merge.push_back(Id2lanelet[ind2Id[sub_ind]]);
to_be_merged.push_back(Id2lanelet[ind2Id[sub_ind]]);
original.push_back(Id2lanelet[ind2Id[sub_ind]]);
}
originals.push_back(to_merge);
merged.push_back(lanelet::utils::combineLaneletsShape(to_merge));
merged.push_back(lanelet::utils::combineLaneletsShape(to_be_merged));
}
}
return {merged, originals};
Expand Down
28 changes: 8 additions & 20 deletions ...ing/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,34 +110,22 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection(
* @brief this function sorts the set of lanelets topologically using topological sort and merges
* the lanelets from each root to each end. each branch is merged and returned with the original
* lanelets
*
* @param lanelets The set of input lanelets to be processed
* @param terminal_lanelets The set of lanelets considered as terminal
* @param routing_graph_ptr Pointer to the routing graph used for determining lanelet
* connections
*
* @return A pair containing:
* - First: A vector of merged lanelets, where each element represents a merged lanelet from
* a branch
* - Second: A vector of vectors, where each inner vector contains the original lanelets
* that were merged to form the corresponding merged lanelet
* @param[in] lanelets the set of lanelets
* @param[in] routing_graph_ptr the routing graph
* @return the pair of merged lanelets and their corresponding original lanelets
*/
std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
mergeLaneletsByTopologicalSort(
const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & terminal_lanelets,
const lanelet::routing::RoutingGraphPtr routing_graph_ptr);

/**
* @brief Retrieves all paths from the given source to terminal nodes on the tree
*
* @param adjacency A 2D vector representing the adjacency matrix of the graph
* @param src_ind The index of the current source node being processed
*
* @return A vector of vectors, where each inner vector represents a path from the source node to a
* terminal node.
* @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
*/
std::vector<std::vector<size_t>> retrievePathsBackward(
const std::vector<std::vector<bool>> & adjacency, size_t start_node);
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);

/**
* @brief find the index of the first point where vehicle footprint intersects with the given
Expand Down
12 changes: 6 additions & 6 deletions ...havior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,16 +42,16 @@ TEST(TestUtil, retrievePathsBackward)
};
{
const size_t src_ind = 6;
auto paths =
autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind);
std::vector<std::vector<size_t>> paths;
autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind, {}, paths);
EXPECT_EQ(paths.size(), 1);
EXPECT_EQ(paths.at(0).size(), 1);
EXPECT_EQ(paths.at(0).at(0), 6);
}
{
const size_t src_ind = 4;
auto paths =
autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind);
std::vector<std::vector<size_t>> paths;
autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind, {}, paths);
EXPECT_EQ(paths.size(), 2);
// 4 --> 6
EXPECT_EQ(paths.at(0).size(), 2);
Expand All @@ -64,8 +64,8 @@ TEST(TestUtil, retrievePathsBackward)
}
{
const size_t src_ind = 0;
auto paths =
autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind);
std::vector<std::vector<size_t>> paths;
autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind, {}, paths);
EXPECT_EQ(paths.size(), 3);
// 0 --> 1 --> 2 --> 4 --> 6
EXPECT_EQ(paths.at(0).size(), 5);
Expand Down

0 comments on commit f4eeba7

Please sign in to comment.