yield at intersection by making the check conservative

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -98,6 +98,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isStoppedAtRedTrafficLight() const override;
 
+  bool is_safe_to_try_change_lanes_at_intersection() const final;
+
 protected:
   lanelet::ConstLanelets getCurrentLanes() const override;
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

@@ -84,6 +84,8 @@ class LaneChangeBase
 
   virtual bool isAbortState() const = 0;
 
+  virtual bool is_safe_to_try_change_lanes_at_intersection() const = 0;
+
   virtual bool isAbleToReturnCurrentLane() const = 0;
 
   virtual LaneChangePath getLaneChangePath() const = 0;

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -93,6 +93,7 @@ struct LaneChangeParameters
   // lane change parameters
   double backward_length_buffer_for_end_of_lane;
   double backward_length_buffer_for_blocking_object;
+  double backward_length_from_intersection{5.0};
   double lane_changing_lateral_jerk{0.5};
   double minimum_lane_changing_velocity{5.6};
   double lane_change_prepare_duration{4.0};

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp

@@ -21,6 +21,7 @@
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <limits>
 #include <vector>
 
 namespace behavior_path_planner
@@ -39,14 +40,19 @@ using LaneChangePaths = std::vector<LaneChangePath>;
 struct LaneChangeStatus
 {
   PathWithLaneId lane_follow_path{};
+  PathWithLaneId path_after_intersection{};
   LaneChangePath lane_change_path{};
   lanelet::ConstLanelets current_lanes{};
   lanelet::ConstLanelets target_lanes{};
+  lanelet::ConstLanelet current_lane_{};
   std::vector<lanelet::Id> lane_follow_lane_ids{};
   std::vector<lanelet::Id> lane_change_lane_ids{};
   bool is_safe{false};
   bool is_valid_path{true};
+  bool is_ego_in_turn_direction_lane{false};
+  bool is_ego_in_intersection{false};
   double start_distance{0.0};
+  double distance_from_prev_intersection{std::numeric_limits<double>::max()};
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -43,6 +43,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
@@ -338,5 +339,11 @@ bool has_blocking_target_object(
   const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects,
   const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose,
   const PathWithLaneId & path);
+
+std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject & object);
+
+bool has_overtaking_turn_lane_object(
+  const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane,
+  const ExtendedPredictedObjects & trailing_objects);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -120,6 +120,15 @@ ModuleStatus LaneChangeInterface::updateState()
     module_type_->evaluateApprovedPathWithUnsafeHysteresis(module_type_->isApprovedPathSafe());
 
   setObjectDebugVisualization();
+
+  if (
+    !module_type_->is_safe_to_try_change_lanes_at_intersection() &&
+    module_type_->isAbleToReturnCurrentLane()) {
+    RCLCPP_DEBUG(
+      logger_, "unsafe to try lane change at intersection but able to return to current lane");
+    return ModuleStatus::SUCCESS;
+  }
+
   if (is_safe) {
     log_warn_throttled("Lane change path is safe.");
     module_type_->toNormalState();

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -54,6 +54,51 @@ NormalLaneChange::NormalLaneChange(
 void NormalLaneChange::updateLaneChangeStatus()
 {
   updateStopTime();
+  status_.current_lanes = getCurrentLanes();
+  if (status_.current_lanes.empty()) {
+    return;
+  }
+
+  lanelet::ConstLanelet current_lane;
+  if (!getRouteHandler()->getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
+    RCLCPP_DEBUG(logger_, "ego's current lane not in route");
+    return;
+  }
+  status_.current_lane_ = current_lane;
+
+  const auto ego_footprint =
+    utils::lane_change::getEgoCurrentFootprint(getEgoPose(), getCommonParam().vehicle_info);
+  status_.is_ego_in_turn_direction_lane =
+    utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint);
+  status_.is_ego_in_intersection =
+    utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
+
+  if (
+    status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane &&
+    status_.path_after_intersection.points.empty()) {
+    const auto target_neighbor =
+      utils::lane_change::getTargetNeighborLanes(*getRouteHandler(), status_.current_lanes, type_);
+    status_.path_after_intersection = getRouteHandler()->getCenterLinePath(
+      target_neighbor, 0.0, std::numeric_limits<double>::max());
+    status_.distance_from_prev_intersection = 0.0;
+  }
+
+  if (
+    !status_.is_ego_in_intersection && !status_.is_ego_in_turn_direction_lane &&
+    !status_.path_after_intersection.points.empty()) {
+    status_.distance_from_prev_intersection = calcSignedArcLength(
+      status_.path_after_intersection.points,
+      status_.path_after_intersection.points.front().point.pose.position, getEgoPosition());
+  }
+
+  if (
+    !status_.is_ego_in_intersection && !status_.is_ego_in_turn_direction_lane &&
+    status_.distance_from_prev_intersection >
+      lane_change_parameters_->backward_length_from_intersection &&
+    !status_.path_after_intersection.points.empty()) {
+    status_.path_after_intersection = PathWithLaneId();
+    status_.distance_from_prev_intersection = std::numeric_limits<double>::max();
+  }
   const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path);
 
   // Update status
@@ -135,6 +180,24 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const
     status_.lane_change_path.info.length.sum());
 }
 
+bool NormalLaneChange::is_safe_to_try_change_lanes_at_intersection() const
+{
+  if (debug_filtered_objects_.target_lane.empty() || status_.target_lanes.empty()) {
+    return true;
+  }
+
+  const auto has_overtaking_turn_direction_object =
+    utils::lane_change::has_overtaking_turn_lane_object(
+      status_.target_lanes, status_.current_lane_, debug_filtered_objects_.target_lane);
+
+  if (!has_overtaking_turn_direction_object) {
+    return true;
+  }
+
+  return status_.distance_from_prev_intersection >
+         lane_change_parameters_->backward_length_from_intersection;
+}
+
 LaneChangePath NormalLaneChange::getLaneChangePath() const
 {
   return status_.lane_change_path;
@@ -1372,6 +1435,18 @@ bool NormalLaneChange::getLaneChangePaths(
         }
         candidate_paths->push_back(*candidate_path);
 
+        if (status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane) {
+          return false;
+        }
+        if (
+          status_.distance_from_prev_intersection <
+            lane_change_parameters_->backward_length_from_intersection &&
+          utils::lane_change::has_overtaking_turn_lane_object(
+            status_.target_lanes, status_.current_lane_, target_objects.target_lane)) {
+          RCLCPP_DEBUG(logger_, "has overtaking object while ego leaves intersection");
+          return false;
+        }
+
         std::vector<ExtendedPredictedObject> filtered_objects =
           filterObjectsInTargetLane(target_objects, target_lanes);
         if (

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -1305,4 +1305,51 @@ bool has_blocking_target_object(
       return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length;
     });
 }
+
+std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject & object)
+{
+  const auto transform = [](const auto & predicted_path) -> LineString2d {
+    LineString2d line_string;
+    const auto & path = predicted_path.path;
+    line_string.reserve(path.size());
+    for (const auto & path_point : path) {
+      const auto point = tier4_autoware_utils::fromMsg(path_point.pose.position).to_2d();
+      line_string.push_back(point);
+    }
+
+    return line_string;
+  };
+
+  const auto paths = object.predicted_paths;
+  std::vector<LineString2d> line_strings;
+  std::transform(paths.begin(), paths.end(), std::back_inserter(line_strings), transform);
+
+  return line_strings;
+}
+
+bool has_overtaking_turn_lane_object(
+  const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane,
+  const ExtendedPredictedObjects & trailing_objects)
+{
+  const auto target_lane_poly =
+    lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon();
+  const auto is_overlap_with_target = [&](const LineString2d & path) {
+    return !boost::geometry::disjoint(path, target_lane_poly);
+  };
+
+  return std::any_of(
+    trailing_objects.begin(), trailing_objects.end(), [&](const ExtendedPredictedObject & object) {
+      lanelet::ConstLanelet obj_lane;
+      const auto obj_poly =
+        tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape);
+      if (!boost::geometry::disjoint(
+            obj_poly, utils::toPolygon2d(
+                        lanelet::utils::to2D(ego_current_lane.polygon2d().basicPolygon())))) {
+        return true;
+      }
+
+      const auto paths = get_line_string_paths(object);
+      return std::any_of(paths.begin(), paths.end(), is_overlap_with_target);
+    });
+}
 }  // namespace behavior_path_planner::utils::lane_change