make code of similar to main branch

Signed-off-by: Zulfaqar Azmi <[email protected]>

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

@@ -98,8 +98,6 @@ 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;
 
@@ -188,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/base_class.hpp

@@ -84,8 +84,6 @@ 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/path.hpp

@@ -40,19 +40,18 @@ 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_{};
+  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()};
+  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

@@ -340,10 +340,13 @@ bool has_blocking_target_object(
   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 lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane,
+  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

@@ -121,14 +121,6 @@ ModuleStatus LaneChangeInterface::updateState()
 
   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/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

@@ -73,32 +73,8 @@ void NormalLaneChange::updateLaneChangeStatus()
   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;
-  }
+  update_dist_from_intersection();
 
-  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
@@ -180,24 +156,6 @@ 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;
@@ -1438,11 +1396,8 @@ bool NormalLaneChange::getLaneChangePaths(
         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)) {
+        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;
         }
@@ -1490,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(
@@ -1965,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

@@ -1306,6 +1306,16 @@ bool has_blocking_target_object(
     });
 }
 
+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 {
@@ -1328,11 +1338,21 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
 }
 
 bool has_overtaking_turn_lane_object(
-  const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane,
+  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 true;
+  }
+
+  const auto & target_lanes = status.target_lanes;
+  const auto & ego_current_lane = status.current_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);
   };