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/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_prepare_data.cpp

@@ -190,15 +190,15 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL
   }
 
   if (!intersection_lanelets_) {
-    intersection_lanelets_ =
-      generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet);
+    intersection_lanelets_ = generateObjectiveLanelets();
   }
   auto & intersection_lanelets = intersection_lanelets_.value();
   debug_data_.attention_area = intersection_lanelets.attention_area();
   debug_data_.first_attention_area = intersection_lanelets.first_attention_area();
   debug_data_.second_attention_area = intersection_lanelets.second_attention_area();
   debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area();
   debug_data_.adjacent_area = intersection_lanelets.adjacent_area();
+  debug_data_.yield_area = intersection_lanelets.yield_area();
 
   // ==========================================================================================
   // at the very first time of registration of this module, the path may not be conflicting with
@@ -573,25 +573,18 @@ IntersectionModule::generateIntersectionStopLines(
   return intersection_stoplines;
 }
 
-intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
-  lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const lanelet::ConstLanelet assigned_lanelet) const
+intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets() const
 {
+  const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
+  const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
+  const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_);
+
   const double detection_area_length = planner_param_.common.attention_area_length;
   const double occlusion_detection_area_length =
     planner_param_.occlusion.occlusion_attention_area_length;
   const bool consider_wrong_direction_vehicle =
     planner_param_.collision_detection.consider_wrong_direction_vehicle;
 
-  // retrieve a stopline associated with a traffic light
-  bool has_traffic_light = false;
-  if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs<lanelet::TrafficLight>();
-      tl_reg_elems.size() != 0) {
-    const auto tl_reg_elem = tl_reg_elems.front();
-    const auto stopline_opt = tl_reg_elem->stopLine();
-    if (!!stopline_opt) has_traffic_light = true;
-  }
-
   // for low priority lane
   // if ego_lane has right of way (i.e. is high priority),
   // ignore yieldLanelets (i.e. low priority lanes)
@@ -644,7 +637,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets
   }
 
   // exclude yield lanelets and ego lanelets from detection_lanelets
-  if (turn_direction_ == std::string("straight") && has_traffic_light) {
+  if (turn_direction_ == std::string("straight") && has_traffic_light_) {
     // if assigned lanelet is "straight" with traffic light, detection area is not necessary
   } else {
     if (consider_wrong_direction_vehicle) {
@@ -724,6 +717,15 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets
   auto [attention_lanelets, original_attention_lanelet_sequences] =
     util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr);
 
+  const auto adjacents = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_);
+  // yield
+  lanelet::ConstLanelets yield{};
+  for (const auto & conflicting : conflicting_ex_ego_lanelets) {
+    if (!lanelet::utils::contains(adjacents, conflicting)) {
+      yield.push_back(conflicting);
+    }
+  }
+
   intersection::IntersectionLanelets result;
   result.attention_ = std::move(attention_lanelets);
   for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) {
@@ -743,6 +745,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets
     }
     result.attention_stoplines_.push_back(stopline);
   }
+
   result.attention_non_preceding_ = std::move(detection_lanelets);
   for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) {
     std::optional<lanelet::ConstLineString3d> stopline = std::nullopt;
@@ -752,11 +755,42 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets
       const auto stopline_opt = traffic_light->stopLine();
       if (!stopline_opt) continue;
       stopline = stopline_opt.get();
+      break;
     }
     result.attention_non_preceding_stoplines_.push_back(stopline);
   }
+
+  result.yield_ = std::move(yield);
+  for (unsigned i = 0; i < result.yield_.size(); ++i) {
+    std::optional<lanelet::ConstLineString3d> stopline = std::nullopt;
+    const auto & ll = result.yield_.at(i);
+    // ==========================================================================================
+    // NOTE: for yield_lanes we should prioritize road_marking stopline over traffic light stopline
+    // ==========================================================================================
+    const auto traffic_lights = ll.regulatoryElementsAs<lanelet::TrafficLight>();
+    for (const auto & traffic_light : traffic_lights) {
+      const auto stopline_opt = traffic_light->stopLine();
+      if (!stopline_opt) continue;
+      stopline = stopline_opt.get();
+      break;
+    }
+    const auto road_markings = ll.regulatoryElementsAs<lanelet::autoware::RoadMarking>();
+    for (const auto & road_marking : road_markings) {
+      const std::string type =
+        road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none");
+      if (type == lanelet::AttributeValueString::StopLine) {
+        stopline = road_marking->roadMarking();
+        break;
+      }
+    }
+    if (stopline)
+      RCLCPP_INFO(
+        logger_, "for yield lane %ld, stopline id is %ld", ll.id(), stopline.value().id());
+    result.yield_stoplines_.push_back(stopline);
+  }
+
   result.conflicting_ = std::move(conflicting_ex_ego_lanelets);
-  result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_);
+  result.adjacent_ = adjacents;
   // NOTE: occlusion_attention is not inverted here
   // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and
   // then trim part of them based on curvature threshold
@@ -771,6 +805,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets
   result.conflicting_area_ = util::getPolygon3dFromLanelets(result.conflicting_);
   result.adjacent_area_ = util::getPolygon3dFromLanelets(result.adjacent_);
   result.occlusion_attention_area_ = util::getPolygon3dFromLanelets(result.occlusion_attention_);
+  result.yield_area_ = util::getPolygon3dFromLanelets(result.yield_);
   return result;
 }