From 8fdc7b194f378fa489d28479b2fb153854943c20 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 10 May 2023 14:36:35 +0900 Subject: [PATCH] feat(intersection): remove lanelets on path from detection area (#3655) (#419) remove lanelets on path from detection area Signed-off-by: Mamoru Sobue --- .../include/scene_module/intersection/util.hpp | 4 ++-- .../src/scene_module/intersection/scene_intersection.cpp | 6 ++++-- .../intersection/scene_merge_from_private_road.cpp | 2 +- .../src/scene_module/intersection/util.cpp | 6 +++--- 4 files changed, 10 insertions(+), 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 index 4fddd8e15075c..10c06f31376d9 100644 --- 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 getDuplicatedPointIdx( */ IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const std::set & assoc_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const int lane_id, const lanelet::ConstLanelets & lanelets_on_path, + const std::set & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair 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 index e32b6695a0cec..206d3341dc29a 100644 --- 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/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 973b88ae08e59..58040fc18a626 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/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 index bebcaaaa401e9..085503426ed88 100644 --- 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 & assoc_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const int lane_id, const lanelet::ConstLanelets & lanelets_on_path, + const std::set & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair 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)) {