fix(behavior_velocity_intersection_module): behavior_velocity_interse…

…ction_module dies due empty conflicting_lanelets (autowarefoundation#4191) * fix the range problem Signed-off-by: beyza <bnk@leodrive.ai> * style(pre-commit): autofix * cleanup Signed-off-by: beyza <bnk@leodrive.ai> * add a flag in the checkAngleForTargetLanelets func Signed-off-by: beyza <bnk@leodrive.ai> * change the angle check Signed-off-by: beyza <bnk@leodrive.ai> * style(pre-commit): autofix --------- Signed-off-by: beyza <bnk@leodrive.ai> Co-authored-by: beyza <bnk@leodrive.ai> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
h-ohta · Jul 7, 2023 · 410f10d · 410f10d
1 parent 3b0671a
commit 410f10d
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 10 deletions.
diff --git 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
@@ -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<lanelet::ConstLanelet> 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
@@ -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,