cherry-pick LC PRs 2

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

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/CMakeLists.txt

@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
   src/manager.cpp
   src/scene.cpp
+  src/utils/calculation.cpp
   src/utils/markers.cpp
   src/utils/utils.cpp
 )

planning/behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -27,6 +27,11 @@
       min_longitudinal_acc: -1.0
       max_longitudinal_acc: 1.0
 
+      skip_process:
+        longitudinal_distance_diff_threshold:
+          prepare: 0.5
+          lane_changing: 0.5
+
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -81,7 +81,7 @@ class NormalLaneChange : public LaneChangeBase
   PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis(
     PathSafetyStatus approved_path_safety_status) override;
 
-  bool isRequiredStop(const bool is_object_coming_from_rear) override;
+  bool isRequiredStop(const bool is_trailing_object) override;
 
   bool isNearEndOfCurrentLanes(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
@@ -91,6 +91,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isAbleToReturnCurrentLane() const override;
 
+  bool is_near_terminal() const final;
+
   bool isEgoOnPreparePhase() const override;
 
   bool isAbleToStopSafely() const override;
@@ -119,19 +121,16 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
-  ExtendedPredictedObjects getTargetObjects(
-    const LaneChangeLanesFilteredObjects & predicted_objects,
+  lane_change::TargetObjects getTargetObjects(
+    const FilteredByLanesExtendedObjects & predicted_objects,
     const lanelet::ConstLanelets & current_lanes) const;
 
-  LaneChangeLanesFilteredObjects filterObjects() const;
+  FilteredByLanesExtendedObjects filterObjects() const;
 
   void filterOncomingObjects(PredictedObjects & objects) const;
 
-  void filterObjectsByLanelets(
-    const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path,
-    std::vector<PredictedObject> & current_lane_objects,
-    std::vector<PredictedObject> & target_lane_objects,
-    std::vector<PredictedObject> & other_lane_objects) const;
+  FilteredByLanesObjects filterObjectsByLanelets(
+    const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;
 
   PathWithLaneId getPrepareSegment(
     const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
@@ -157,9 +156,7 @@ class NormalLaneChange : public LaneChangeBase
 
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-    Direction direction, LaneChangePaths * candidate_paths,
-    const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
-    const bool check_safety = true) const override;
+    Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const;
 
   std::optional<LaneChangePath> calcTerminalLaneChangePath(
     const lanelet::ConstLanelets & current_lanes,
@@ -169,7 +166,7 @@ class NormalLaneChange : public LaneChangeBase
 
   PathSafetyStatus isLaneChangePathSafe(
     const LaneChangePath & lane_change_path,
-    const ExtendedPredictedObjects & collision_check_objects,
+    const lane_change::TargetObjects & collision_check_objects,
     const utils::path_safety_checker::RSSparams & rss_params,
     CollisionCheckDebugMap & debug_data) const;
 
@@ -183,6 +180,24 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
 
+  /**
+   * @brief Checks if the given pose is a valid starting point for a lane change.
+   *
+   * This function determines whether the given pose (position) of the vehicle is within
+   * the area of either the target neighbor lane or the target lane itself. It uses geometric
+   * checks to see if the start point of the lane change is covered by the polygons representing
+   * these lanes.
+   *
+   * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
+   *  - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes.
+   * @param pose The current pose of the vehicle
+   *
+   * @return `true` if the pose is within either the target neighbor lane or the target lane;
+   * `false` otherwise.
+   */
+  bool is_valid_start_point(
+    const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const;
+
   bool check_prepare_phase() const;
 
   double calcMaximumLaneChangeLength(

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

@@ -91,13 +91,15 @@ class LaneChangeBase
 
   virtual bool isAbleToReturnCurrentLane() const = 0;
 
+  virtual bool is_near_terminal() const = 0;
+
   virtual LaneChangePath getLaneChangePath() const = 0;
 
   virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0;
 
   virtual bool isEgoOnPreparePhase() const = 0;
 
-  virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0;
+  virtual bool isRequiredStop(const bool is_trailing_object) = 0;
 
   virtual PathSafetyStatus isApprovedPathSafe() const = 0;
 
@@ -227,12 +229,6 @@ class LaneChangeBase
     const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
     const double prepare_length) const = 0;
 
-  virtual bool getLaneChangePaths(
-    const lanelet::ConstLanelets & original_lanelets,
-    const lanelet::ConstLanelets & target_lanelets, Direction direction,
-    LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params,
-    const bool is_stuck, const bool check_safety) const = 0;
-
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 
   virtual bool isAbleToStopSafely() const = 0;

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

@@ -0,0 +1,63 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+
+#include "behavior_path_lane_change_module/utils/data_structs.hpp"
+
+namespace behavior_path_planner::utils::lane_change::calculation
+{
+using behavior_path_planner::lane_change::CommonDataPtr;
+using behavior_path_planner::lane_change::LCParamPtr;
+
+/**
+ * @brief Calculates the distance from the ego vehicle to the terminal point.
+ *
+ * This function computes the distance from the current position of the ego vehicle
+ * to either the goal pose or the end of the current lane, depending on whether
+ * the vehicle's current lane is within the goal section.
+ *
+ * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
+ *  - Non null `lanes_ptr` that points to the current lanes data.
+ *  - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle.
+ *  - Non null `route_handler_ptr` that contains the goal pose of the route.
+ *
+ * @return The distance to the terminal point (either the goal pose or the end of the current lane)
+ * in meters.
+ */
+double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr);
+
+double calc_dist_from_pose_to_terminal_end(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
+  const Pose & src_pose);
+
+/**
+ * @brief Calculates the backward buffer distance required for safe lane changing.
+ *
+ * This function computes the minimum required backward buffer distance based on the
+ * minimum lane changing velocity and the minimum longitudinal acceleration. It then
+ * compares this calculated distance with a pre-defined backward length buffer parameter
+ * and returns the larger of the two values to ensure safe lane changing.
+ *
+ * @param lc_param_ptr Shared pointer to an LCParam structure, which should include:
+ *  - `minimum_lane_changing_velocity`: The minimum velocity required for lane changing.
+ *  - `min_longitudinal_acc`: The minimum longitudinal acceleration used for deceleration.
+ *  - `backward_length_buffer_for_end_of_lane`: A predefined backward buffer length parameter.
+ *
+ * @return The required backward buffer distance in meters.
+ */
+double calc_backward_buffer(const LCParamPtr & lc_param_ptr);
+}  // namespace behavior_path_planner::utils::lane_change::calculation
+
+#endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_

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

@@ -124,6 +124,9 @@ struct Parameters
   double min_longitudinal_acc{-1.0};
   double max_longitudinal_acc{1.0};
 
+  double skip_process_lon_diff_th_prepare{0.5};
+  double skip_process_lon_diff_th_lane_changing{1.0};
+
   // collision check
   bool enable_collision_check_for_prepare_phase_in_general_lanes{false};
   bool enable_collision_check_for_prepare_phase_in_intersection{true};
@@ -186,6 +189,7 @@ struct Lanes
 {
   bool current_lane_in_goal_section{false};
   lanelet::ConstLanelets current;
+  lanelet::ConstLanelets target_neighbor;
   lanelet::ConstLanelets target;
   std::vector<lanelet::ConstLanelets> preceding_target;
 };
@@ -206,11 +210,33 @@ struct Info
   double terminal_lane_changing_velocity{0.0};
 };
 
+template <typename Object>
 struct LanesObjects
 {
-  ExtendedPredictedObjects current_lane{};
-  ExtendedPredictedObjects target_lane{};
-  ExtendedPredictedObjects other_lane{};
+  Object current_lane{};
+  Object target_lane_leading{};
+  Object target_lane_trailing{};
+  Object other_lane{};
+
+  LanesObjects() = default;
+  LanesObjects(
+    Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane)
+  : current_lane(std::move(current_lane)),
+    target_lane_leading(std::move(target_lane_leading)),
+    target_lane_trailing(std::move(target_lane_trailing)),
+    other_lane(std::move(other_lane))
+  {
+  }
+};
+
+struct TargetObjects
+{
+  ExtendedPredictedObjects leading;
+  ExtendedPredictedObjects trailing;
+  TargetObjects(ExtendedPredictedObjects leading, ExtendedPredictedObjects trailing)
+  : leading(std::move(leading)), trailing(std::move(trailing))
+  {
+  }
 };
 
 enum class ModuleType {
@@ -222,7 +248,13 @@ enum class ModuleType {
 struct PathSafetyStatus
 {
   bool is_safe{true};
-  bool is_object_coming_from_rear{false};
+  bool is_trailing_object{false};
+
+  PathSafetyStatus() = default;
+  PathSafetyStatus(const bool is_safe, const bool is_trailing_object)
+  : is_safe(is_safe), is_trailing_object(is_trailing_object)
+  {
+  }
 };
 
 struct LanesPolygon
@@ -271,12 +303,15 @@ using CommonDataPtr = std::shared_ptr<CommonData>;
 
 namespace behavior_path_planner
 {
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using utils::path_safety_checker::ExtendedPredictedObjects;
 using LaneChangeModuleType = lane_change::ModuleType;
 using LaneChangeParameters = lane_change::Parameters;
 using LaneChangeStates = lane_change::States;
 using LaneChangePhaseInfo = lane_change::PhaseInfo;
 using LaneChangeInfo = lane_change::Info;
-using LaneChangeLanesFilteredObjects = lane_change::LanesObjects;
+using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
+using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;
 using LateralAccelerationMap = lane_change::LateralAccelerationMap;
 }  // namespace behavior_path_planner
 

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

@@ -35,7 +35,7 @@ struct Debug
   LaneChangePaths valid_paths;
   CollisionCheckDebugMap collision_check_objects;
   CollisionCheckDebugMap collision_check_objects_after_approval;
-  LaneChangeLanesFilteredObjects filtered_objects;
+  FilteredByLanesExtendedObjects filtered_objects;
   geometry_msgs::msg::Polygon execution_area;
   geometry_msgs::msg::Pose ego_pose;
   lanelet::ConstLanelets current_lanes;
@@ -55,7 +55,8 @@ struct Debug
     collision_check_objects.clear();
     collision_check_objects_after_approval.clear();
     filtered_objects.current_lane.clear();
-    filtered_objects.target_lane.clear();
+    filtered_objects.target_lane_leading.clear();
+    filtered_objects.target_lane_trailing.clear();
     filtered_objects.other_lane.clear();
     execution_area.points.clear();
     current_lanes.clear();

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

@@ -29,6 +29,7 @@
 
 namespace marker_utils::lane_change_markers
 {
+using behavior_path_planner::FilteredByLanesExtendedObjects;
 using behavior_path_planner::LaneChangePath;
 using behavior_path_planner::lane_change::Debug;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
@@ -39,9 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);
 MarkerArray showFilteredObjects(
-  const ExtendedPredictedObjects & current_lane_objects,
-  const ExtendedPredictedObjects & target_lane_objects,
-  const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
+  const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns);
 MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
 MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
 MarkerArray createDebugMarkerArray(

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

@@ -104,10 +104,6 @@ lanelet::ConstLanelets getTargetNeighborLanes(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const LaneChangeModuleType & type);
 
-lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const LaneChangeModuleType & type);
-
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -262,6 +262,21 @@ bool LaneChangeInterface::canTransitFailureState()
     return false;
   }
 
+  if (!module_type_->isValidPath()) {
+    log_debug_throttled("Transit to failure state due not to find valid path");
+    return true;
+  }
+
+  if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) {
+    log_debug_throttled("Abort process has completed.");
+    return true;
+  }
+
+  if (module_type_->is_near_terminal()) {
+    log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change");
+    return false;
+  }
+
   if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) {
     if (module_type_->isStoppedAtRedTrafficLight()) {
       log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change");
@@ -287,6 +302,11 @@ bool LaneChangeInterface::canTransitFailureState()
 
     if (post_process_safety_status_.is_safe) {
       log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe.");
+
+      if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) {
+        log_debug_throttled("Module require stopping");
+      }
+
       return false;
     }
 
@@ -304,7 +324,7 @@ bool LaneChangeInterface::canTransitFailureState()
     return false;
   }
 
-  if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) {
+  if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) {
     log_debug_throttled("Module require stopping");
   }
 

planning/behavior_path_lane_change_module/src/manager.cpp

@@ -330,6 +330,13 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
     updateParam<double>(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc);
   }
 
+  {
+    const std::string ns = "lane_change.skip_process.longitudinal_distance_diff_threshold.";
+    updateParam<double>(parameters, ns + "prepare", p->skip_process_lon_diff_th_prepare);
+    updateParam<double>(
+      parameters, ns + "lane_changing", p->skip_process_lon_diff_th_lane_changing);
+  }
+
   {
     const std::string ns = "lane_change.safety_check.lane_expansion.";
     updateParam<double>(parameters, ns + "left_offset", p->lane_expansion_left_offset);