feat(intersection): remove lanelets on path from detection area (auto…

…warefoundation#3655) (#419) remove lanelets on path from detection area Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
tier4 · May 10, 2023 · 8fdc7b1 · 8fdc7b1
1 parent a4a173e
commit 8fdc7b1
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 8 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp
@@ -57,8 +57,8 @@ std::optional<size_t> getDuplicatedPointIdx(
  */
 IntersectionLanelets getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const std::set<int> & assoc_ids,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const int lane_id, const lanelet::ConstLanelets & lanelets_on_path,
+  const std::set<int> & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::pair<size_t, size_t> lane_interval, const double detection_area_length,
   const double occlusion_detection_area_length, const bool tl_arrow_solid_on = false);
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -140,9 +140,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   if (
     !intersection_lanelets_ ||
     intersection_lanelets_.value().tl_arrow_solid_on != tl_arrow_solid_on) {
+    const auto lanelets_on_path = planning_utils::getLaneletsOnPath(
+      *path, lanelet_map_ptr, planner_data_->current_odometry->pose);
     intersection_lanelets_ = util::getObjectiveLanelets(
-      lanelet_map_ptr, routing_graph_ptr, lane_id_, assoc_ids_, path_ip, lane_interval_ip,
-      planner_param_.common.detection_area_length, tl_arrow_solid_on);
+      lanelet_map_ptr, routing_graph_ptr, lane_id_, lanelets_on_path, assoc_ids_, path_ip,
+      lane_interval_ip, planner_param_.common.detection_area_length, tl_arrow_solid_on);
   }
   const auto & detection_lanelets = intersection_lanelets_.value().attention;
   const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent;

diff --git a/...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp
@@ -86,7 +86,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
   /* get detection area */
   if (!intersection_lanelets_.has_value()) {
     intersection_lanelets_ = util::getObjectiveLanelets(
-      lanelet_map_ptr, routing_graph_ptr, lane_id_, {} /* not used here */, path_ip,
+      lanelet_map_ptr, routing_graph_ptr, lane_id_, {}, {} /* not used here */, path_ip,
       lane_interval_ip, planner_param_.detection_area_length,
       false /* tl_arrow_solid on does not matter here*/);
   }

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
@@ -411,8 +411,8 @@ bool getStopLineIndexFromMap(
 
 IntersectionLanelets getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const std::set<int> & assoc_ids,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const int lane_id, const lanelet::ConstLanelets & lanelets_on_path,
+  const std::set<int> & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::pair<size_t, size_t> lane_interval, const double detection_area_length,
   const double occlusion_detection_area_length, const bool tl_arrow_solid_on)
 {
@@ -445,7 +445,7 @@ IntersectionLanelets getObjectiveLanelets(
   }
 
   // get all following lanes of previous lane
-  lanelet::ConstLanelets ego_lanelets{};
+  lanelet::ConstLanelets ego_lanelets = lanelets_on_path;
   for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) {
     ego_lanelets.push_back(previous_lanelet);
     for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) {