diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CMakeLists.txt
index f3b31001978e9..167c7182e8f05 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CMakeLists.txt
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CMakeLists.txt
@@ -28,6 +28,16 @@ target_link_libraries(${PROJECT_NAME}
ament_auto_package(INSTALL_TO_SHARE config)
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+ file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+ ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+ ${TEST_SOURCES}
+ )
+ target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
+endif()
+
install(PROGRAMS
scripts/ttc.py
DESTINATION lib/${PROJECT_NAME}
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml
index 39ed187cd867a..93e1976a251ba 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml
@@ -26,6 +26,7 @@
autoware_planning_msgs
autoware_route_handler
autoware_rtc_interface
+ autoware_test_utils
autoware_universe_utils
autoware_vehicle_info_utils
fmt
@@ -40,6 +41,7 @@
tier4_planning_msgs
visualization_msgs
+ ament_cmake_ros
ament_lint_auto
autoware_lint_common
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp
index 03417a8410d20..8ffb4792f55db 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -590,7 +590,8 @@ class IntersectionModule : public SceneModuleInterface
* @brief generate discretized detection lane linestring.
*/
std::vector generateDetectionLaneDivisions(
- lanelet::ConstLanelets detection_lanelets_all,
+ const lanelet::ConstLanelets & occlusion_detection_lanelets,
+ const lanelet::ConstLanelets & conflicting_detection_lanelets,
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
/** @} */
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp
index 92a8fc57079df..f9ab47ace7d53 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp
@@ -266,8 +266,8 @@ Result IntersectionModule::prepare
if (!occlusion_attention_divisions_) {
occlusion_attention_divisions_ = generateDetectionLaneDivisions(
- intersection_lanelets.occlusion_attention(), routing_graph_ptr,
- planner_data_->occupancy_grid->info.resolution);
+ intersection_lanelets.occlusion_attention(), intersection_lanelets.attention_non_preceding(),
+ routing_graph_ptr, planner_data_->occupancy_grid->info.resolution);
}
if (has_traffic_light_) {
@@ -577,6 +577,73 @@ std::optional IntersectionModule::generateIntersectionSto
return intersection_stoplines;
}
+static std::vector> getPrecedingLaneletsUptoIntersectionRecursive(
+ const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+ const double length, const lanelet::ConstLanelets & exclude_lanelets)
+{
+ std::vector> preceding_lanelet_sequences;
+
+ const auto prev_lanelets = graph->previous(lanelet);
+ const double lanelet_length = lanelet::utils::getLaneletLength3d(lanelet);
+
+ // end condition of the recursive function
+ if (prev_lanelets.empty() || lanelet_length >= length) {
+ preceding_lanelet_sequences.push_back({lanelet});
+ return preceding_lanelet_sequences;
+ }
+
+ for (const auto & prev_lanelet : prev_lanelets) {
+ if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
+ // if prev_lanelet is included in exclude_lanelets,
+ // remove prev_lanelet from preceding_lanelet_sequences
+ continue;
+ }
+ if (const std::string turn_direction = prev_lanelet.attributeOr("turn_direction", "else");
+ turn_direction == "left" || turn_direction == "right") {
+ continue;
+ }
+
+ // get lanelet sequence after prev_lanelet
+ auto tmp_lanelet_sequences = getPrecedingLaneletsUptoIntersectionRecursive(
+ graph, prev_lanelet, length - lanelet_length, exclude_lanelets);
+ for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) {
+ tmp_lanelet_sequence.push_back(lanelet);
+ preceding_lanelet_sequences.push_back(tmp_lanelet_sequence);
+ }
+ }
+
+ if (preceding_lanelet_sequences.empty()) {
+ preceding_lanelet_sequences.push_back({lanelet});
+ }
+ return preceding_lanelet_sequences;
+}
+
+static std::vector getPrecedingLaneletsUptoIntersection(
+ const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+ const double length, const lanelet::ConstLanelets & exclude_lanelets)
+{
+ std::vector lanelet_sequences_vec;
+ const auto prev_lanelets = graph->previous(lanelet);
+ for (const auto & prev_lanelet : prev_lanelets) {
+ if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
+ // if prev_lanelet is included in exclude_lanelets,
+ // remove prev_lanelet from preceding_lanelet_sequences
+ continue;
+ }
+ if (const std::string turn_direction = prev_lanelet.attributeOr("turn_direction", "else");
+ turn_direction == "left" || turn_direction == "right") {
+ continue;
+ }
+ // convert deque into vector
+ const auto lanelet_sequences_deq =
+ getPrecedingLaneletsUptoIntersectionRecursive(graph, prev_lanelet, length, exclude_lanelets);
+ for (const auto & lanelet_sequence : lanelet_sequences_deq) {
+ lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end());
+ }
+ }
+ return lanelet_sequences_vec;
+}
+
IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const lanelet::ConstLanelet assigned_lanelet) const
@@ -706,8 +773,8 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll);
// get preceding lanelets without ego_lanelets
// to prevent the detection area from including the ego lanes and its' preceding lanes.
- const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences(
- routing_graph_ptr, ll, length, ego_lanelets);
+ const auto lanelet_sequences =
+ getPrecedingLaneletsUptoIntersection(routing_graph_ptr, ll, length, ego_lanelets);
for (const auto & ls : lanelet_sequences) {
for (const auto & l : ls) {
const auto & inner_inserted = detection_ids.insert(l.id());
@@ -716,17 +783,10 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
}
}
}
- lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction;
- for (const auto & ll : occlusion_detection_and_preceding_lanelets) {
- const std::string turn_direction = ll.attributeOr("turn_direction", "else");
- if (turn_direction == "left" || turn_direction == "right") {
- continue;
- }
- occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll);
- }
auto [attention_lanelets, original_attention_lanelet_sequences] =
- util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr);
+ util::mergeLaneletsByTopologicalSort(
+ detection_and_preceding_lanelets, detection_lanelets, routing_graph_ptr);
IntersectionLanelets result;
result.attention_ = std::move(attention_lanelets);
@@ -764,8 +824,7 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
// NOTE: occlusion_attention is not inverted here
// TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and
// then trim part of them based on curvature threshold
- result.occlusion_attention_ =
- std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction);
+ result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets);
// NOTE: to properly update(), each element in conflicting_/conflicting_area_,
// attention_non_preceding_/attention_non_preceding_area_ need to be matched
@@ -851,7 +910,8 @@ std::optional IntersectionModule::generatePathLanelets(
}
std::vector IntersectionModule::generateDetectionLaneDivisions(
- lanelet::ConstLanelets detection_lanelets_all,
+ const lanelet::ConstLanelets & occlusion_detection_lanelets,
+ const lanelet::ConstLanelets & conflicting_detection_lanelets,
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const
{
const double curvature_threshold =
@@ -861,9 +921,9 @@ std::vector IntersectionModule::generateDetectionLan
using lanelet::utils::getCenterlineWithOffset;
- // (0) remove left/right lanelet
+ // (0) remove curved
lanelet::ConstLanelets detection_lanelets;
- for (const auto & detection_lanelet : detection_lanelets_all) {
+ for (const auto & detection_lanelet : occlusion_detection_lanelets) {
// TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet
const auto fine_centerline =
lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds);
@@ -875,8 +935,8 @@ std::vector IntersectionModule::generateDetectionLan
}
// (1) tsort detection_lanelets
- const auto [merged_detection_lanelets, originals] =
- util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr);
+ const auto [merged_detection_lanelets, originals] = util::mergeLaneletsByTopologicalSort(
+ detection_lanelets, conflicting_detection_lanelets, routing_graph_ptr);
// (2) merge each branch to one lanelet
// NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp
index 308483e5b0869..f0fbafd75ce5f 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp
@@ -32,12 +32,10 @@
#include
#include
-#include
-#include