fix(lane_change): enable cancel when ego in turn direction lane (RT0-…

…33893) (autowarefoundation#1594)

* yield at intersection by making the check conservative

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

* make code of similar to main branch

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

* fix build error

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

* fix logic

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

* update README

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

* change variable name

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

---------

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

planning/behavior_path_lane_change_module/README.md

@@ -341,6 +341,7 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `prepare_duration`                           | [m]    | double  | The preparation time for the ego vehicle to be ready to perform lane change.                                           | 4.0                |
 | `backward_length_buffer_for_end_of_lane`     | [m]    | double  | The end of lane buffer to ensure ego vehicle has enough distance to start lane change                                  | 3.0                |
 | `backward_length_buffer_for_blocking_object` | [m]    | double  | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0                |
+| `backward_length_from_intersection`          | [m]    | double  | Distance threshold from the last intersection to invalidate or cancel the lane change path                             | 5.0                |
 | `lane_change_finish_judge_buffer`            | [m]    | double  | The additional buffer used to confirm lane change process completion                                                   | 3.0                |
 | `finish_judge_lateral_threshold`             | [m]    | double  | Lateral distance threshold to confirm lane change process completion                                                   | 0.2                |
 | `lane_changing_lateral_jerk`                 | [m/s3] | double  | Lateral jerk value for lane change path generation                                                                     | 0.5                |

planning/behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -6,6 +6,8 @@
 
       backward_length_buffer_for_end_of_lane: 3.0 # [m]
       backward_length_buffer_for_blocking_object: 3.0 # [m]
+      backward_length_from_intersection: 5.0 # [m]
+
       lane_change_finish_judge_buffer: 2.0      # [m]
 
       lane_changing_lateral_jerk: 0.5              # [m/s3]

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -186,6 +186,9 @@ class NormalLaneChange : public LaneChangeBase
 
   double getStopTime() const { return stop_time_; }
 
+  void update_dist_from_intersection();
+
+  std::vector<PathPointWithLaneId> path_after_intersection_;
   double stop_time_{0.0};
 };
 }  // namespace behavior_path_planner

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
@@ -42,11 +43,15 @@ struct LaneChangeStatus
   LaneChangePath lane_change_path{};
   lanelet::ConstLanelets current_lanes{};
   lanelet::ConstLanelets target_lanes{};
+  lanelet::ConstLanelet ego_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 dist_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,14 @@ 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);
+
+bool has_passed_intersection_turn_direction(
+  const LaneChangeParameters & lc_param, const LaneChangeStatus & status);
+
+std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject & object);
+
+bool has_overtaking_turn_lane_object(
+  const LaneChangeStatus & status, const LaneChangeParameters & lc_param,
+  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,7 @@ ModuleStatus LaneChangeInterface::updateState()
     module_type_->evaluateApprovedPathWithUnsafeHysteresis(module_type_->isApprovedPathSafe());
 
   setObjectDebugVisualization();
+
   if (is_safe) {
     log_warn_throttled("Lane change path is safe.");
     module_type_->toNormalState();

planning/behavior_path_lane_change_module/src/manager.cpp

@@ -169,6 +169,8 @@ void LaneChangeModuleManager::init(rclcpp::Node * node)
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
   p.backward_length_buffer_for_blocking_object =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_blocking_object"));
+  p.backward_length_from_intersection =
+    getOrDeclareParameter<double>(*node, parameter("backward_length_from_intersection"));
   p.lane_changing_lateral_jerk =
     getOrDeclareParameter<double>(*node, parameter("lane_changing_lateral_jerk"));
   p.lane_change_prepare_duration =

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -54,6 +54,27 @@ 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_.ego_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);
+
+  update_dist_from_intersection();
+
   const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path);
 
   // Update status
@@ -1372,6 +1393,15 @@ 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 (utils::lane_change::has_overtaking_turn_lane_object(
+              status_, *lane_change_parameters_, 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 (
@@ -1415,6 +1445,12 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
   debug_filtered_objects_ = target_objects;
 
+  const auto has_overtaking_object = utils::lane_change::has_overtaking_turn_lane_object(
+    status_, *lane_change_parameters_, target_objects.target_lane);
+
+  if (has_overtaking_object) {
+    return {false, true};
+  }
   CollisionCheckDebugMap debug_data;
   const bool is_stuck = isVehicleStuck(current_lanes);
   const auto safety_status = isLaneChangePathSafe(
@@ -1890,4 +1926,40 @@ bool NormalLaneChange::check_prepare_phase() const
   return check_in_intersection || check_in_turns || check_in_general_lanes;
 }
 
+void NormalLaneChange::update_dist_from_intersection()
+{
+  if (
+    status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane &&
+    path_after_intersection_.empty()) {
+    const auto target_neighbor =
+      utils::lane_change::getTargetNeighborLanes(*getRouteHandler(), status_.current_lanes, type_);
+    auto path_after_intersection = getRouteHandler()->getCenterLinePath(
+      target_neighbor, 0.0, std::numeric_limits<double>::max());
+    path_after_intersection_ = std::move(path_after_intersection.points);
+    status_.dist_from_prev_intersection = 0.0;
+    return;
+  }
+
+  if (
+    status_.is_ego_in_intersection || status_.is_ego_in_turn_direction_lane ||
+    path_after_intersection_.empty()) {
+    return;
+  }
+
+  const auto & path_points = path_after_intersection_;
+  const auto & front_point = path_points.front().point.pose.position;
+  const auto & ego_position = getEgoPosition();
+  status_.dist_from_prev_intersection = calcSignedArcLength(path_points, front_point, ego_position);
+
+  if (
+    status_.dist_from_prev_intersection >= 0.0 &&
+    status_.dist_from_prev_intersection <=
+      lane_change_parameters_->backward_length_from_intersection) {
+    return;
+  }
+
+  path_after_intersection_.clear();
+  status_.dist_from_prev_intersection = std::numeric_limits<double>::max();
+}
+
 }  // namespace behavior_path_planner

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -1305,4 +1305,71 @@ bool has_blocking_target_object(
       return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length;
     });
 }
+
+bool has_passed_intersection_turn_direction(
+  const LaneChangeParameters & lc_param, const LaneChangeStatus & status)
+{
+  if (status.is_ego_in_intersection && status.is_ego_in_turn_direction_lane) {
+    return false;
+  }
+
+  return status.dist_from_prev_intersection > lc_param.backward_length_from_intersection;
+}
+
+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 LaneChangeStatus & status, const LaneChangeParameters & lc_param,
+  const ExtendedPredictedObjects & trailing_objects)
+{
+  // Note: This situation is only applicable if the ego is in a turn lane.
+  if (has_passed_intersection_turn_direction(lc_param, status)) {
+    return false;
+  }
+
+  const auto & target_lanes = status.target_lanes;
+  const auto & ego_current_lane = status.ego_lane;
+
+  const auto target_lane_poly =
+    lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon();
+  // to compensate for perception issue, or if object is from behind ego, and tries to overtake,
+  // but stop all of sudden
+  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