feat(intersection): use intersection_area if available (autowarefound…

…ation#1733)

Signed-off-by: Mamoru Sobue <[email protected]>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

@@ -14,6 +14,7 @@
       detection_area_right_margin: 0.5 # [m]
       detection_area_left_margin: 0.5 # [m]
       detection_area_length: 200.0 # [m]
+      detection_area_angle_threshold: 0.785 # [rad]
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -14,6 +14,7 @@
       detection_area_right_margin: 0.5 # [m]
       detection_area_left_margin: 0.5 # [m]
       detection_area_length: 200.0 # [m]
+      detection_area_angle_threshold: 0.785 # [rad]
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -53,12 +53,13 @@ class IntersectionModule : public SceneModuleInterface
     geometry_msgs::msg::Pose stop_point_pose;
     geometry_msgs::msg::Pose judge_point_pose;
     geometry_msgs::msg::Polygon ego_lane_polygon;
-    std::vector<lanelet::CompoundPolygon3d> detection_area_with_margin;
     geometry_msgs::msg::Polygon stuck_vehicle_detect_area;
     geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon;
     std::vector<geometry_msgs::msg::Polygon> candidate_collision_object_polygons;
     std::vector<lanelet::ConstLanelet> intersection_detection_lanelets;
     std::vector<lanelet::CompoundPolygon3d> detection_area;
+    geometry_msgs::msg::Polygon intersection_area;
+    std::vector<lanelet::CompoundPolygon3d> adjacent_area;
     autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;
     autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets;
     geometry_msgs::msg::Pose predicted_obj_pose;
@@ -129,6 +130,7 @@ class IntersectionModule : public SceneModuleInterface
    * @param path             ego-car lane
    * @param detection_area_lanelet_ids  angle check is performed for obstacles using this lanelet
    * ids
+   * @param intersection_area associated intersection_area if exists
    * @param objects_ptr      target objects
    * @param closest_idx      ego-car position index on the lane
    * @return true if collision is detected
@@ -137,6 +139,8 @@ class IntersectionModule : public SceneModuleInterface
     lanelet::LaneletMapConstPtr lanelet_map_ptr,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const std::vector<int> & detection_area_lanelet_ids,
+    const std::vector<int> & adjacent_lanelet_ids,
+    const std::optional<Polygon2d> & intersection_area,
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
     const int closest_idx, const Polygon2d & stuck_vehicle_detect_area);
 
@@ -218,7 +222,8 @@ class IntersectionModule : public SceneModuleInterface
    * @return true if the given pose belongs to any target lanelet
    */
   bool checkAngleForTargetLanelets(
-    const geometry_msgs::msg::Pose & pose, const std::vector<int> & target_lanelet_ids);
+    const geometry_msgs::msg::Pose & pose, const std::vector<int> & target_lanelet_ids,
+    const double margin = 0);
 
   /**
    * @brief Get lanes including ego lanelet and next lanelet

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -27,6 +27,7 @@
 #endif
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -48,11 +49,9 @@ bool getDuplicatedPointIdx(
  */
 bool getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const double detection_area_length, const double right_margin,
-  const double left_margin, std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
-  lanelet::ConstLanelets * objective_lanelets_result,
-  std::vector<lanelet::ConstLanelets> * objective_lanelets_with_margin_result,
-  const rclcpp::Logger logger);
+  const int lane_id, const double detection_area_length,
+  std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
+  lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger);
 
 struct StopLineIdx
 {
@@ -143,6 +142,13 @@ bool isBeforeTargetIndex(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
   const geometry_msgs::msg::Pose & current_pose, const int target_idx);
 
+std::vector<int> extendedAdjacentDirectionLanes(
+  const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph,
+  lanelet::ConstLanelet lane);
+
+std::optional<Polygon2d> getIntersectionArea(
+  lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr);
+
 }  // namespace util
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/intersection-design.md

@@ -25,11 +25,12 @@ Objects that satisfy all of the following conditions are considered as target ob
 
 - The type of object type is **car**, **truck**, **bus** or **motorbike**. (Bicycle, pedestrian, animal, unknown are not.)
 - The center of gravity of object is **located within a certain distance** from the attention lane (threshold = `detection_area_margin`) .
-  - In the past, the decision was made based on whether any points of the object's polygon exists in the attention lane, but since there were many false positives, the logic has changed to the current one.
+  - (Optional condition) The center of gravity is in the **intersection area**.
+    - To deal with objects that is in the area not covered by the lanelets in the intersection.
 - The posture of object is **the same direction as the attention lane** (threshold = `detection_area_angle_threshold`).
   - The orientation of the target is recalculated in this module according to the `orientation_reliable` and the sign of the velocity of the target.
-- Not being **in the same lane as the ego vehicle**.
-  - To avoid judging the vehicle ahead as a collision target. This logic needs to be improved.
+- Not being **in the neighbouring lanes of the ego vehicle**.
+  - neighbouring lanes include the ego lane of the vehicle and the adjacent lanes of it with turn_direction as the ego lane.
 
 #### How to Define Attention Lanes
 

planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp

@@ -30,14 +30,14 @@ using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerOrientation;
 using tier4_autoware_utils::createMarkerScale;
 
-visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray(
+static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray(
   const std::vector<lanelet::CompoundPolygon3d> & polygons, const std::string & ns,
-  const int64_t module_id)
+  const int64_t lane_id, const double r, const double g, const double b)
 {
   visualization_msgs::msg::MarkerArray msg;
 
   int32_t i = 0;
-  int32_t uid = planning_utils::bitShift(module_id);
+  int32_t uid = planning_utils::bitShift(lane_id);
   for (const auto & polygon : polygons) {
     visualization_msgs::msg::Marker marker{};
     marker.header.frame_id = "map";
@@ -49,7 +49,7 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray(
     marker.action = visualization_msgs::msg::Marker::ADD;
     marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0);
     marker.scale = createMarkerScale(0.1, 0.0, 0.0);
-    marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999);
+    marker.color = createMarkerColor(r, g, b, 0.999);
     for (const auto & p : polygon) {
       geometry_msgs::msg::Point point;
       point.x = p.x();
@@ -118,22 +118,29 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
     &debug_marker_array, now);
 
   appendMarkerArray(
-    createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", module_id_),
-    &debug_marker_array, now);
+    createLaneletPolygonsMarkerArray(
+      debug_data_.detection_area, "detection_area", lane_id_, 0.0, 1.0, 0.0),
+    &debug_marker_array);
 
   appendMarkerArray(
     createLaneletPolygonsMarkerArray(
-      debug_data_.detection_area_with_margin, "detection_area_with_margin", module_id_),
-    &debug_marker_array, now);
+      debug_data_.adjacent_area, "adjacent_area", lane_id_, 0.913, 0.639, 0.149),
+    &debug_marker_array);
 
   appendMarkerArray(
     debug::createPolygonMarkerArray(
-      debug_data_.ego_lane_polygon, "ego_lane", module_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7),
-    &debug_marker_array, now);
+      debug_data_.intersection_area, "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 1.0,
+      0.0),
+    &debug_marker_array);
+
+  appendMarkerArray(
+    debug::createPolygonMarkerArray(
+      debug_data_.ego_lane_polygon, "ego_lane", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7),
+    &debug_marker_array);
 
   appendMarkerArray(
     debug::createPolygonMarkerArray(
-      debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.3, 0.0,
+      debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, now, 0.3, 0.0,
       0.0, 0.0, 0.5, 0.5),
     &debug_marker_array, now);
 
@@ -147,7 +154,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
   for (const auto & p : debug_data_.candidate_collision_object_polygons) {
     appendMarkerArray(
       debug::createPolygonMarkerArray(
-        p, "candidate_collision_object_polygons", module_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5,
+        p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5,
         0.5),
       &debug_marker_array, now);
   }

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -94,20 +94,14 @@ bool IntersectionModule::modifyPathVelocity(
   /* get detection area and conflicting area */
   lanelet::ConstLanelets detection_area_lanelets;
   std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
-  std::vector<lanelet::ConstLanelets> detection_area_lanelets_with_margin;
 
   util::getObjectiveLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin,
-    &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin,
-    logger_);
+    &conflicting_area_lanelets, &detection_area_lanelets, logger_);
   std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
     conflicting_area_lanelets, planner_param_.detection_area_length);
   std::vector<lanelet::CompoundPolygon3d> detection_areas =
     util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length);
-  std::vector<lanelet::CompoundPolygon3d> detection_areas_with_margin =
-    util::getPolygon3dFromLaneletsVec(
-      detection_area_lanelets_with_margin, planner_param_.detection_area_length);
   std::vector<int> conflicting_area_lanelet_ids =
     util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets);
   std::vector<int> detection_area_lanelet_ids =
@@ -120,7 +114,24 @@ bool IntersectionModule::modifyPathVelocity(
     return true;
   }
   debug_data_.detection_area = detection_areas;
-  debug_data_.detection_area_with_margin = detection_areas_with_margin;
+
+  /* get intersection area */
+  const auto & assigned_lanelet =
+    planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_);
+  const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
+  if (intersection_area) {
+    const auto intersection_area_2d = intersection_area.value();
+    debug_data_.intersection_area = toGeomPoly(intersection_area_2d);
+  }
+
+  /* get adjacent lanelets */
+  const auto adjacent_lanelet_ids =
+    util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet);
+  std::vector<lanelet::CompoundPolygon3d> adjacent_lanes;
+  for (auto && adjacent_lanelet_id : adjacent_lanelet_ids) {
+    adjacent_lanes.push_back(lanelet_map_ptr->laneletLayer.get(adjacent_lanelet_id).polygon3d());
+  }
+  debug_data_.adjacent_area = adjacent_lanes;
 
   /* set stop-line and stop-judgement-line for base_link */
   util::StopLineIdx stop_line_idxs;
@@ -198,8 +209,8 @@ bool IntersectionModule::modifyPathVelocity(
     planner_param_.stuck_vehicle_detect_dist);
   bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
   bool has_collision = checkCollision(
-    lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx,
-    stuck_vehicle_detect_area);
+    lanelet_map_ptr, *path, detection_area_lanelet_ids, adjacent_lanelet_ids, intersection_area,
+    objects_ptr, closest_idx, stuck_vehicle_detect_area);
   bool is_entry_prohibited = (has_collision || is_stuck);
   if (external_go) {
     is_entry_prohibited = false;
@@ -284,6 +295,7 @@ bool IntersectionModule::checkCollision(
   lanelet::LaneletMapConstPtr lanelet_map_ptr,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::vector<int> & detection_area_lanelet_ids,
+  const std::vector<int> & adjacent_lanelet_ids, const std::optional<Polygon2d> & intersection_area,
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
   const int closest_idx, const Polygon2d & stuck_vehicle_detect_area)
 {
@@ -326,7 +338,24 @@ bool IntersectionModule::checkCollision(
 
     // check direction of objects
     const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics);
-    if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) {
+    if (intersection_area) {
+      const auto obj_poly = tier4_autoware_utils::toPolygon2d(object);
+      const auto intersection_area_2d = intersection_area.value();
+      const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d);
+      const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets(
+        object_direction, adjacent_lanelet_ids, planner_param_.detection_area_margin);
+      if (is_in_adjacent_lanelets) continue;
+      if (is_in_intersection_area) {
+        target_objects.objects.push_back(object);
+      } else if (checkAngleForTargetLanelets(
+                   object_direction, detection_area_lanelet_ids,
+                   planner_param_.detection_area_margin)) {
+        target_objects.objects.push_back(object);
+      }
+    } else if (checkAngleForTargetLanelets(
+                 object_direction, detection_area_lanelet_ids,
+                 planner_param_.detection_area_margin)) {
+      // intersection_area is not available, use detection_area_with_margin as before
       target_objects.objects.push_back(object);
     }
   }
@@ -610,11 +639,12 @@ bool IntersectionModule::isTargetExternalInputStatus(const int target_status)
 }
 
 bool IntersectionModule::checkAngleForTargetLanelets(
-  const geometry_msgs::msg::Pose & pose, const std::vector<int> & target_lanelet_ids)
+  const geometry_msgs::msg::Pose & pose, const std::vector<int> & target_lanelet_ids,
+  const double margin)
 {
   for (const int lanelet_id : target_lanelet_ids) {
     const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id);
-    if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) {
+    if (!lanelet::utils::isInLanelet(pose, ll, margin)) {
       continue;
     }
     const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position);

planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

@@ -68,13 +68,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   /* get detection area */
   lanelet::ConstLanelets detection_area_lanelets;
   std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
-  std::vector<lanelet::ConstLanelets> detection_area_lanelets_with_margin;
 
   util::getObjectiveLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin,
-    &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin,
-    logger_);
+    &conflicting_area_lanelets, &detection_area_lanelets, logger_);
   std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
     conflicting_area_lanelets, planner_param_.detection_area_length);
   if (conflicting_areas.empty()) {