diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index bc6b54c23edd6..002c4daf4ec29 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -899,6 +899,7 @@ bool IntersectionModule::checkCollision( const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, planner_param_.common.attention_area_margin); if (is_in_adjacent_lanelets) { continue; @@ -913,12 +914,14 @@ bool IntersectionModule::checkCollision( } else if (util::checkAngleForTargetLanelets( object_direction, attention_area_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, planner_param_.common.attention_area_margin)) { target_objects.objects.push_back(object); } } else if (util::checkAngleForTargetLanelets( object_direction, attention_area_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, planner_param_.common.attention_area_margin)) { // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 51d8e286fe764..8108d0deaae6f 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -446,7 +446,16 @@ IntersectionLanelets getObjectiveLanelets( // get conflicting lanes on assigned lanelet const auto & conflicting_lanelets = lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); - const auto & adjacent_followings = routing_graph_ptr->following(conflicting_lanelets.back()); + std::vector adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } // final objective lanelets lanelet::ConstLanelets detection_lanelets; @@ -467,9 +476,9 @@ IntersectionLanelets getObjectiveLanelets( continue; } detection_lanelets.push_back(conflicting_lanelet); - if (!adjacent_followings.empty()) { - detection_lanelets.push_back(adjacent_followings.front()); - } + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); } } else { // otherwise we need to know the priority from RightOfWay @@ -992,7 +1001,8 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( bool checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const double margin) + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double margin) { for (const auto & ll : target_lanelets) { if (!lanelet::utils::isInLanelet(pose, ll, margin)) { @@ -1001,10 +1011,14 @@ bool checkAngleForTargetLanelets( const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); - if ( - std::fabs(angle_diff) < detection_area_angle_thr / 2 || - std::fabs(angle_diff) > detection_area_angle_thr) { - return true; + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return true; + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return true; + } } } return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 89931a5db10fc..8a539f67f73a6 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -144,7 +144,8 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( bool checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const double margin = 0.0); + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double margin = 0.0); void cutPredictPathWithDuration( autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr,