feat(intersection): consider abnormal violating vehicles

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -291,6 +291,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       &debug_marker_array);
   }
 
+  if (debug_data_.yield_area) {
+    appendMarkerArray(
+      ::createLaneletPolygonsMarkerArray(
+        debug_data_.yield_area.value(), "yield_area", lane_id_, 0.6588235, 0.34509, 0.6588235),
+      &debug_marker_array);
+  }
+
   if (debug_data_.ego_lane) {
     appendMarkerArray(
       ::createLaneletPolygonsMarkerArray(

planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp

@@ -52,20 +52,29 @@ struct IntersectionLanelets
   {
     return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_;
   }
+  const lanelet::ConstLanelets & attention_non_preceding() const
+  {
+    return attention_non_preceding_;
+  }
   const lanelet::ConstLanelets & conflicting() const { return conflicting_; }
   const lanelet::ConstLanelets & adjacent() const { return adjacent_; }
   const lanelet::ConstLanelets & occlusion_attention() const
   {
     return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_;
   }
-  const lanelet::ConstLanelets & attention_non_preceding() const
+  const lanelet::ConstLanelets & yield() const { return yield_; }
+  const std::vector<std::optional<lanelet::ConstLineString3d>> & yield_stoplines() const
   {
-    return attention_non_preceding_;
+    return yield_stoplines_;
   }
   const std::vector<lanelet::CompoundPolygon3d> & attention_area() const
   {
     return is_prioritized_ ? attention_non_preceding_area_ : attention_area_;
   }
+  const std::optional<lanelet::CompoundPolygon3d> & first_attention_area() const
+  {
+    return first_attention_area_;
+  }
   const std::vector<lanelet::CompoundPolygon3d> & conflicting_area() const
   {
     return conflicting_area_;
@@ -75,6 +84,7 @@ struct IntersectionLanelets
   {
     return occlusion_attention_area_;
   }
+  const std::vector<lanelet::CompoundPolygon3d> & yield_area() const { return yield_area_; }
   const std::optional<lanelet::ConstLanelet> & first_conflicting_lane() const
   {
     return first_conflicting_lane_;
@@ -87,10 +97,6 @@ struct IntersectionLanelets
   {
     return first_attention_lane_;
   }
-  const std::optional<lanelet::CompoundPolygon3d> & first_attention_area() const
-  {
-    return first_attention_area_;
-  }
   const std::optional<lanelet::ConstLanelet> & second_attention_lane() const
   {
     return second_attention_lane_;
@@ -103,48 +109,55 @@ struct IntersectionLanelets
   /**
    * the set of attention lanelets which is topologically merged
    */
-  lanelet::ConstLanelets attention_;
-  std::vector<lanelet::CompoundPolygon3d> attention_area_;
+  lanelet::ConstLanelets attention_{};
+  std::vector<lanelet::CompoundPolygon3d> attention_area_{};
 
   /**
    * the stop lines for each attention_lanelets associated with traffic lights. At intersection
    * without traffic lights, each value is null
    */
-  std::vector<std::optional<lanelet::ConstLineString3d>> attention_stoplines_;
+  std::vector<std::optional<lanelet::ConstLineString3d>> attention_stoplines_{};
 
   /**
    * the conflicting part of attention lanelets
    */
-  lanelet::ConstLanelets attention_non_preceding_;
-  std::vector<lanelet::CompoundPolygon3d> attention_non_preceding_area_;
+  lanelet::ConstLanelets attention_non_preceding_{};
+  std::vector<lanelet::CompoundPolygon3d> attention_non_preceding_area_{};
 
   /**
    * the stop lines for each attention_non_preceding_
    */
-  std::vector<std::optional<lanelet::ConstLineString3d>> attention_non_preceding_stoplines_;
+  std::vector<std::optional<lanelet::ConstLineString3d>> attention_non_preceding_stoplines_{};
 
   /**
    * the conflicting lanelets of the objective intersection lanelet
    */
-  lanelet::ConstLanelets conflicting_;
-  std::vector<lanelet::CompoundPolygon3d> conflicting_area_;
+  lanelet::ConstLanelets conflicting_{};
+  std::vector<lanelet::CompoundPolygon3d> conflicting_area_{};
 
   /**
    *
    */
-  lanelet::ConstLanelets adjacent_;
-  std::vector<lanelet::CompoundPolygon3d> adjacent_area_;
+  lanelet::ConstLanelets adjacent_{};
+  std::vector<lanelet::CompoundPolygon3d> adjacent_area_{};
 
   /**
    * the set of attention lanelets for occlusion detection which is topologically merged
    */
-  lanelet::ConstLanelets occlusion_attention_;
-  std::vector<lanelet::CompoundPolygon3d> occlusion_attention_area_;
+  lanelet::ConstLanelets occlusion_attention_{};
+  std::vector<lanelet::CompoundPolygon3d> occlusion_attention_area_{};
+
+  /**
+   * the set of lanelets that the objective intersection lanelet has RightOfWay over
+   */
+  lanelet::ConstLanelets yield_{};
+  std::vector<std::optional<lanelet::ConstLineString3d>> yield_stoplines_{};
+  std::vector<lanelet::CompoundPolygon3d> yield_area_{};
 
   /**
    * the vector of sum of each occlusion_attention lanelet
    */
-  std::vector<double> occlusion_attention_size_;
+  std::vector<double> occlusion_attention_size_{};
 
   /**
    * the first conflicting lanelet which ego path points intersect for the first time

planning/behavior_velocity_intersection_module/src/object_manager.cpp

@@ -197,38 +197,47 @@ bool ObjectInfo::before_stopline_by(const double margin) const
 }
 
 std::shared_ptr<ObjectInfo> ObjectInfoManager::registerObject(
-  const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-  const bool belong_intersection_area, const bool is_parked_vehicle)
+  const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area,
+  const bool belong_attention_area, const bool belong_intersection_area,
+  const bool is_parked_vehicle)
 {
   if (objects_info_.count(uuid) == 0) {
     auto object = std::make_shared<intersection::ObjectInfo>(uuid);
     objects_info_[uuid] = object;
   }
   auto object = objects_info_[uuid];
-  if (belong_attention_area) {
-    attention_area_objects_.push_back(object);
-  } else if (belong_intersection_area) {
-    intersection_area_objects_.push_back(object);
-  }
-  if (is_parked_vehicle) {
-    parked_objects_.push_back(object);
+  if (belong_violating_area) {
+    violating_objects_.push_back(object);
+  } else {
+    if (belong_attention_area) {
+      attention_area_objects_.push_back(object);
+    } else if (belong_intersection_area) {
+      intersection_area_objects_.push_back(object);
+    }
+    if (is_parked_vehicle) {
+      parked_objects_.push_back(object);
+    }
   }
   return object;
 }
 
 void ObjectInfoManager::registerExistingObject(
-  const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-  const bool belong_intersection_area, const bool is_parked_vehicle,
-  std::shared_ptr<intersection::ObjectInfo> object)
+  const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area,
+  const bool belong_attention_area, const bool belong_intersection_area,
+  const bool is_parked_vehicle, std::shared_ptr<intersection::ObjectInfo> object)
 {
   objects_info_[uuid] = object;
-  if (belong_attention_area) {
-    attention_area_objects_.push_back(object);
-  } else if (belong_intersection_area) {
-    intersection_area_objects_.push_back(object);
-  }
-  if (is_parked_vehicle) {
-    parked_objects_.push_back(object);
+  if (belong_violating_area) {
+    violating_objects_.push_back(object);
+  } else {
+    if (belong_attention_area) {
+      attention_area_objects_.push_back(object);
+    } else if (belong_intersection_area) {
+      intersection_area_objects_.push_back(object);
+    }
+    if (is_parked_vehicle) {
+      parked_objects_.push_back(object);
+    }
   }
 }
 
@@ -240,7 +249,7 @@ void ObjectInfoManager::clearObjects()
   parked_objects_.clear();
 };
 
-std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects() const
+std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allAttentionObjects() const
 {
   std::vector<std::shared_ptr<ObjectInfo>> all_objects = attention_area_objects_;
   all_objects.insert(

planning/behavior_velocity_intersection_module/src/object_manager.hpp

@@ -229,12 +229,12 @@ class ObjectInfoManager
 {
 public:
   std::shared_ptr<ObjectInfo> registerObject(
-    const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-    const bool belong_intersection_area, const bool is_parked);
+    const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area,
+    const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked);
 
   void registerExistingObject(
-    const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
-    const bool belong_intersection_area, const bool is_parked,
+    const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area,
+    const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked,
     std::shared_ptr<intersection::ObjectInfo> object);
 
   void clearObjects();
@@ -244,9 +244,17 @@ class ObjectInfoManager
     return attention_area_objects_;
   }
 
-  const std::vector<std::shared_ptr<ObjectInfo>> & parkedObjects() const { return parked_objects_; }
+  const std::vector<std::shared_ptr<ObjectInfo>> & parkedAttentionObjects() const
+  {
+    return parked_objects_;
+  }
+
+  const std::vector<std::shared_ptr<ObjectInfo>> & violatingObjects() const
+  {
+    return violating_objects_;
+  }
 
-  std::vector<std::shared_ptr<ObjectInfo>> allObjects() const;
+  std::vector<std::shared_ptr<ObjectInfo>> allAttentionObjects() const;
 
   const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
   getObjectsMap()
@@ -275,6 +283,9 @@ class ObjectInfoManager
   //! parked objects on attention_area/intersection_area
   std::vector<std::shared_ptr<ObjectInfo>> parked_objects_;
 
+  //! objects violating right of way
+  std::vector<std::shared_ptr<ObjectInfo>> violating_objects_;
+
   std::optional<rclcpp::Time> passed_1st_judge_line_first_time_{std::nullopt};
   std::optional<rclcpp::Time> passed_2nd_judge_line_first_time_{std::nullopt};
 };

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -224,6 +224,7 @@ class IntersectionModule : public SceneModuleInterface
     std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
     std::optional<geometry_msgs::msg::Polygon> stuck_vehicle_detect_area{std::nullopt};
     std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_stuck_detect_area{std::nullopt};
+    std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_area{std::nullopt};
 
     std::optional<geometry_msgs::msg::Polygon> candidate_collision_ego_lane_polygon{std::nullopt};
     autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets;
@@ -558,10 +559,7 @@ class IntersectionModule : public SceneModuleInterface
   /**
    * @brief generate IntersectionLanelets
    */
-  intersection::IntersectionLanelets generateObjectiveLanelets(
-    lanelet::LaneletMapConstPtr lanelet_map_ptr,
-    lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-    const lanelet::ConstLanelet assigned_lanelet) const;
+  intersection::IntersectionLanelets generateObjectiveLanelets() const;
 
   /**
    * @brief generate PathLanelets

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

@@ -74,6 +74,8 @@ void IntersectionModule::updateObjectInfoManagerArea()
   const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
   const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_);
   const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
+  const auto & violating_lanelets = intersection_lanelets.yield();
+  const auto & violating_stoplines = intersection_lanelets.yield_stoplines();
 
   // ==========================================================================================
   // entries that are not observed in this iteration need to be cleared
@@ -104,37 +106,52 @@ void IntersectionModule::updateObjectInfoManagerArea()
     if (belong_adjacent_lanelet_id) {
       continue;
     }
-    const auto belong_attention_lanelet_id =
-      checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle);
-    const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position;
-    const bool in_intersection_area = [&]() {
-      if (!intersection_area) {
-        return false;
-      }
-      return bg::within(
-        tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value());
-    }();
+
     std::optional<lanelet::ConstLanelet> attention_lanelet{std::nullopt};
     std::optional<lanelet::ConstLineString3d> stopline{std::nullopt};
-    if (!belong_attention_lanelet_id && !in_intersection_area) {
-      continue;
-    } else if (belong_attention_lanelet_id) {
-      const auto idx = belong_attention_lanelet_id.value();
-      attention_lanelet = attention_lanelets.at(idx);
-      stopline = attention_lanelet_stoplines.at(idx);
+
+    const auto belong_violating_lanelet_id = checkAngleForTargetLanelets(
+      object_direction, violating_lanelets, false /* if is_parked, it is negligible */);
+    bool is_violating_vehicle{false};
+    if (belong_violating_lanelet_id) {
+      const auto idx = belong_violating_lanelet_id.value();
+      attention_lanelet = violating_lanelets.at(idx);
+      stopline = violating_stoplines.at(idx);
+      is_violating_vehicle = true;
+    }
+
+    std::optional<size_t> belong_attention_lanelet_id{std::nullopt};
+    bool in_intersection_area{false};
+    if (!is_violating_vehicle) {
+      belong_attention_lanelet_id =
+        checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle);
+      in_intersection_area = [&]() {
+        const auto & obj_pos =
+          predicted_object.kinematics.initial_pose_with_covariance.pose.position;
+        if (!intersection_area) {
+          return false;
+        }
+        return bg::within(
+          tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value());
+      }();
+      if (belong_attention_lanelet_id) {
+        const auto idx = belong_attention_lanelet_id.value();
+        attention_lanelet = attention_lanelets.at(idx);
+        stopline = attention_lanelet_stoplines.at(idx);
+      }
     }
 
     const auto object_it = old_map.find(predicted_object.object_id);
     if (object_it != old_map.end()) {
       auto object_info = object_it->second;
       object_info_manager_.registerExistingObject(
-        predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area,
-        is_parked_vehicle, object_info);
+        predicted_object.object_id, is_violating_vehicle, belong_attention_lanelet_id.has_value(),
+        in_intersection_area, is_parked_vehicle, object_info);
       object_info->initialize(predicted_object, attention_lanelet, stopline);
     } else {
       auto object_info = object_info_manager_.registerObject(
-        predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area,
-        is_parked_vehicle);
+        predicted_object.object_id, is_violating_vehicle, belong_attention_lanelet_id.has_value(),
+        in_intersection_area, is_parked_vehicle);
       object_info->initialize(predicted_object, attention_lanelet, stopline);
     }
   }

planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp

@@ -219,7 +219,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
   // re-use attention_mask
   attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0));
   // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded
-  const auto & blocking_attention_objects = object_info_manager_.parkedObjects();
+  const auto & blocking_attention_objects = object_info_manager_.parkedAttentionObjects();
   for (const auto & blocking_attention_object_info : blocking_attention_objects) {
     debug_data_.parked_targets.objects.push_back(
       blocking_attention_object_info->predicted_object());
@@ -403,7 +403,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
   LineString2d ego_occlusion_line;
   ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y);
   ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y);
-  for (const auto & attention_object_info : object_info_manager_.allObjects()) {
+  for (const auto & attention_object_info : object_info_manager_.allAttentionObjects()) {
     const auto obj_poly =
       tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object());
     if (bg::intersects(obj_poly, ego_occlusion_line)) {