feat(blind_spot): consider opposite adjacent lane for wrong vehicles (a…

…utowarefoundation#5635)

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

planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml

@@ -8,4 +8,5 @@
       max_future_movement_time: 10.0 # [second]
       threshold_yaw_diff: 0.523 # [rad]
       adjacent_extend_width: 1.5 # [m]
+      opposite_adjacent_extend_width: 1.5 # [m]
       enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval

planning/behavior_velocity_blind_spot_module/src/debug.cpp

@@ -145,14 +145,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray()
 
   appendMarkerArray(
     createLaneletPolygonsMarkerArray(
-      debug_data_.conflict_areas_for_blind_spot, "conflict_area_for_blind_spot", module_id_, 0.0,
-      0.5, 0.5),
+      debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5),
     &debug_marker_array, now);
 
   appendMarkerArray(
     createLaneletPolygonsMarkerArray(
-      debug_data_.detection_areas_for_blind_spot, "detection_area_for_blind_spot", module_id_, 0.5,
-      0.0, 0.0),
+      debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0),
     &debug_marker_array, now);
 
   appendMarkerArray(

planning/behavior_velocity_blind_spot_module/src/manager.cpp

@@ -48,6 +48,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".threshold_yaw_diff");
   planner_param_.adjacent_extend_width =
     getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
+  planner_param_.opposite_adjacent_extend_width =
+    getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
 }
 
 void BlindSpotModuleManager::launchNewModules(

planning/behavior_velocity_blind_spot_module/src/scene.cpp

@@ -105,16 +105,18 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
 
   /* set stop-line and stop-judgement-line for base_link */
-  int stop_line_idx = -1;
   const auto straight_lanelets = getStraightLanelets(lanelet_map_ptr, routing_graph_ptr, lane_id_);
-  if (!generateStopLine(straight_lanelets, path, &stop_line_idx)) {
+  const auto stoplines_idx_opt = generateStopLine(straight_lanelets, path);
+  if (!stoplines_idx_opt) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(
       logger_, *clock_, 1000 /* ms */, "[BlindSpotModule::run] setStopLineIdx fail");
     *path = input_path;  // reset path
     return false;
   }
 
-  if (stop_line_idx <= 0) {
+  const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value();
+
+  if (default_stopline_idx <= 0) {
     RCLCPP_DEBUG(
       logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning.");
     *path = input_path;  // reset path
@@ -133,6 +135,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     return false;
   }
   const size_t closest_idx = closest_idx_opt.value();
+  const auto stop_line_idx =
+    closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx;
 
   /* set judge line dist */
   const double current_vel = planner_data_->current_velocity->twist.linear.x;
@@ -156,9 +160,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   debug_data_.virtual_wall_pose = stop_line_pose;
   const auto stop_pose = path->points.at(stop_line_idx).point.pose;
   debug_data_.stop_point_pose = stop_pose;
-  auto offset_pose = motion_utils::calcLongitudinalOffsetPose(
-    path->points, stop_pose.position, -pass_judge_line_dist);
-  if (offset_pose) debug_data_.judge_point_pose = *offset_pose;
 
   /* if current_state = GO, and current_pose is over judge_line, ignore planning. */
   if (planner_param_.use_pass_judge_line) {
@@ -235,69 +236,41 @@ std::optional<int> BlindSpotModule::getFirstPointConflictingLanelets(
   }
 }
 
-bool BlindSpotModule::generateStopLine(
+std::optional<std::pair<size_t, size_t>> BlindSpotModule::generateStopLine(
   const lanelet::ConstLanelets straight_lanelets,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const
+  autoware_auto_planning_msgs::msg::PathWithLaneId * path) const
 {
   /* set parameters */
   constexpr double interval = 0.2;
   const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interval);
-  const int base2front_idx_dist =
-    std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval);
+  // const int base2front_idx_dist =
+  //  std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval);
 
   /* spline interpolation */
   autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
   if (!splineInterpolate(*path, interval, path_ip, logger_)) {
-    return false;
+    return std::nullopt;
   }
 
   /* generate stop point */
-  int stop_idx_ip = 0;  // stop point index for interpolated path.
+  size_t stop_idx_default_ip = 0;  // stop point index for interpolated path.
+  size_t stop_idx_critical_ip = 0;
   if (straight_lanelets.size() > 0) {
     std::optional<int> first_idx_conflicting_lane_opt =
       getFirstPointConflictingLanelets(path_ip, straight_lanelets);
     if (!first_idx_conflicting_lane_opt) {
       RCLCPP_DEBUG(logger_, "No conflicting line found.");
-      return false;
-    }
-    stop_idx_ip = std::max(
-      first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0);
-  } else {
-    std::optional<geometry_msgs::msg::Pose> intersection_enter_point_opt =
-      getStartPointFromLaneLet(lane_id_);
-    if (!intersection_enter_point_opt) {
-      RCLCPP_DEBUG(logger_, "No intersection enter point found.");
-      return false;
-    }
-
-    geometry_msgs::msg::Pose intersection_enter_pose;
-    intersection_enter_pose = intersection_enter_point_opt.value();
-    const auto stop_idx_ip_opt =
-      motion_utils::findNearestIndex(path_ip.points, intersection_enter_pose, 10.0, M_PI_4);
-    if (stop_idx_ip_opt) {
-      stop_idx_ip = stop_idx_ip_opt.value();
+      return std::nullopt;
     }
-
-    stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0);
+    stop_idx_default_ip = std::max(first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist, 0);
+    stop_idx_critical_ip = std::max(first_idx_conflicting_lane_opt.value() - 1, 0);
   }
 
   /* insert stop_point to use interpolated path*/
-  *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path);
+  const size_t stopline_idx_default = insertPoint(stop_idx_default_ip, path_ip, path);
+  const size_t stopline_idx_critical = insertPoint(stop_idx_critical_ip, path_ip, path);
 
-  /* if another stop point exist before intersection stop_line, disable judge_line. */
-  bool has_prior_stopline = false;
-  for (int i = 0; i < *stop_line_idx; ++i) {
-    if (std::fabs(path->points.at(i).point.longitudinal_velocity_mps) < 0.1) {
-      has_prior_stopline = true;
-      break;
-    }
-  }
-
-  RCLCPP_DEBUG(
-    logger_, "generateStopLine() : stop_idx = %d, stop_idx_ip = %d, has_prior_stopline = %d",
-    *stop_line_idx, stop_idx_ip, has_prior_stopline);
-
-  return true;
+  return std::make_pair(stopline_idx_default, stopline_idx_critical);
 }
 
 void BlindSpotModule::cutPredictPathWithDuration(
@@ -385,36 +358,61 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
 
   const auto areas_opt = generateBlindSpotPolygons(
     lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose);
-  if (!!areas_opt) {
-    debug_data_.detection_areas_for_blind_spot = areas_opt.value().detection_areas;
-    debug_data_.conflict_areas_for_blind_spot = areas_opt.value().conflict_areas;
+  if (areas_opt) {
+    const auto & detection_areas = areas_opt.value().detection_areas;
+    const auto & conflict_areas = areas_opt.value().conflict_areas;
+    const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas;
+    const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas;
+    debug_data_.detection_areas = detection_areas;
+    debug_data_.conflict_areas = conflict_areas;
+    debug_data_.detection_areas.insert(
+      debug_data_.detection_areas.end(), opposite_detection_areas.begin(),
+      opposite_detection_areas.end());
+    debug_data_.conflict_areas.insert(
+      debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(),
+      opposite_conflict_areas.end());
 
     autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr;
     cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time);
 
     // check objects in blind spot areas
-    bool obstacle_detected = false;
     for (const auto & object : objects.objects) {
       if (!isTargetObjectType(object)) {
         continue;
       }
 
-      const auto & detection_areas = areas_opt.value().detection_areas;
-      const auto & conflict_areas = areas_opt.value().conflict_areas;
-      const bool exist_in_detection_area =
+      // right direction
+      const bool exist_in_right_detection_area =
         std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) {
           return bg::within(
             to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
             lanelet::utils::to2D(area));
         });
-      const bool exist_in_conflict_area =
+      // opposite direction
+      const bool exist_in_opposite_detection_area = std::any_of(
+        opposite_detection_areas.begin(), opposite_detection_areas.end(),
+        [&object](const auto & area) {
+          return bg::within(
+            to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
+            lanelet::utils::to2D(area));
+        });
+      const bool exist_in_detection_area =
+        exist_in_right_detection_area || exist_in_opposite_detection_area;
+      if (!exist_in_detection_area) {
+        continue;
+      }
+      const bool exist_in_right_conflict_area =
         isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose);
+      const bool exist_in_opposite_conflict_area = isPredictedPathInArea(
+        object, opposite_conflict_areas, planner_data_->current_odometry->pose);
+      const bool exist_in_conflict_area =
+        exist_in_right_conflict_area || exist_in_opposite_conflict_area;
       if (exist_in_detection_area && exist_in_conflict_area) {
-        obstacle_detected = true;
         debug_data_.conflicting_targets.objects.push_back(object);
+        return true;
       }
     }
-    return obstacle_detected;
+    return false;
   } else {
     return false;
   }
@@ -504,6 +502,37 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet(
   return new_lanelet;
 }
 
+lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet(
+  const lanelet::ConstLanelet lanelet, const TurnDirection direction) const
+{
+  const auto centerline = lanelet.centerline2d();
+  const auto width =
+    boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline);
+  const double extend_width =
+    std::min<double>(planner_param_.opposite_adjacent_extend_width, width);
+  const auto left_bound_ =
+    direction == TurnDirection::RIGHT
+      ? lanelet.rightBound().invert()
+      : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width);
+  const auto right_bound_ =
+    direction == TurnDirection::RIGHT
+      ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width)
+      : lanelet.leftBound().invert();
+  lanelet::Points3d lefts, rights;
+  for (const auto & pt : left_bound_) {
+    lefts.push_back(lanelet::Point3d(pt));
+  }
+  for (const auto & pt : right_bound_) {
+    rights.push_back(lanelet::Point3d(pt));
+  }
+  const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts));
+  const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights));
+  auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
+  const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0);
+  new_lanelet.setCenterline(new_centerline);
+  return new_lanelet;
+}
+
 std::optional<BlindSpotPolygons> BlindSpotModule::generateBlindSpotPolygons(
   lanelet::LaneletMapConstPtr lanelet_map_ptr,
   [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr,
@@ -512,19 +541,20 @@ std::optional<BlindSpotPolygons> BlindSpotModule::generateBlindSpotPolygons(
 {
   std::vector<int64_t> lane_ids;
   lanelet::ConstLanelets blind_spot_lanelets;
+  lanelet::ConstLanelets blind_spot_opposite_lanelets;
   /* get lane ids until intersection */
   for (const auto & point : path.points) {
     bool found_intersection_lane = false;
     for (const auto lane_id : point.lane_ids) {
-      // make lane_ids unique
-      if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) {
-        lane_ids.push_back(lane_id);
-      }
-
       if (lane_id == lane_id_) {
         found_intersection_lane = true;
+        lane_ids.push_back(lane_id);
         break;
       }
+      // make lane_ids unique
+      if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) {
+        lane_ids.push_back(lane_id);
+      }
     }
     if (found_intersection_lane) break;
   }
@@ -537,27 +567,49 @@ std::optional<BlindSpotPolygons> BlindSpotModule::generateBlindSpotPolygons(
 
   // additional detection area on left/right side
   lanelet::ConstLanelets adjacent_lanelets;
+  lanelet::ConstLanelets opposite_adjacent_lanelets;
   for (const auto i : lane_ids) {
     const auto lane = lanelet_map_ptr->laneletLayer.get(i);
-    const auto adj = std::invoke([&]() -> std::optional<lanelet::ConstLanelet> {
-      if (turn_direction_ == TurnDirection::INVALID) {
+    const auto adj =
+      turn_direction_ == TurnDirection::LEFT
+        ? (routing_graph_ptr->adjacentLeft(lane))
+        : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane))
+                                                   : boost::none);
+    const std::optional<lanelet::ConstLanelet> opposite_adj =
+      [&]() -> std::optional<lanelet::ConstLanelet> {
+      if (!!adj) {
         return std::nullopt;
       }
-      const auto adj_lane = (turn_direction_ == TurnDirection::LEFT)
-                              ? routing_graph_ptr->adjacentLeft(lane)
-                              : routing_graph_ptr->adjacentRight(lane);
-
-      if (adj_lane) {
-        return *adj_lane;
+      if (turn_direction_ == TurnDirection::LEFT) {
+        // this should exist in right-hand traffic
+        const auto adjacent_lanes =
+          lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert());
+        if (adjacent_lanes.empty()) {
+          return std::nullopt;
+        }
+        return adjacent_lanes.front();
       }
-
-      return std::nullopt;
-    });
-
+      if (turn_direction_ == TurnDirection::RIGHT) {
+        // this should exist in left-hand traffic
+        const auto adjacent_lanes =
+          lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert());
+        if (adjacent_lanes.empty()) {
+          return std::nullopt;
+        }
+        return adjacent_lanes.front();
+      } else {
+        return std::nullopt;
+      }
+    }();
     if (adj) {
       const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_);
       adjacent_lanelets.push_back(half_lanelet);
     }
+    if (opposite_adj) {
+      const auto half_lanelet =
+        generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_);
+      opposite_adjacent_lanelets.push_back(half_lanelet);
+    }
   }
 
   const auto current_arc_ego =
@@ -587,6 +639,24 @@ std::optional<BlindSpotPolygons> BlindSpotModule::generateBlindSpotPolygons(
       blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj));
       blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj));
     }
+    // additional detection area on left/right opposite lane side
+    if (!opposite_adjacent_lanelets.empty()) {
+      const auto stop_line_arc_opposite_adj =
+        lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets);
+      const auto current_arc_opposite_adj =
+        stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego);
+      const auto detection_area_start_length_opposite_adj =
+        stop_line_arc_opposite_adj - planner_param_.backward_length;
+      auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength(
+        opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj);
+      auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength(
+        opposite_adjacent_lanelets, detection_area_start_length_opposite_adj,
+        stop_line_arc_opposite_adj);
+      blind_spot_polygons.opposite_conflict_areas.emplace_back(
+        std::move(conflicting_area_opposite_adj));
+      blind_spot_polygons.opposite_detection_areas.emplace_back(
+        std::move(detection_area_opposite_adj));
+    }
     return blind_spot_polygons;
   } else {
     return std::nullopt;
@@ -677,7 +747,7 @@ lanelet::ConstLanelets BlindSpotModule::getStraightLanelets(
   const auto next_lanelets = routing_graph_ptr->following(prev_intersection_lanelets.front());
   for (const auto & ll : next_lanelets) {
     const std::string turn_direction = ll.attributeOr("turn_direction", "else");
-    if (!turn_direction.compare("straight")) {
+    if (turn_direction.compare("straight") == 0) {
       straight_lanelets.push_back(ll);
     }
   }