feat(lane_change): add rss paramas for stuck (#5300)

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -45,6 +45,14 @@
           lateral_distance_max_threshold: 1.0
           longitudinal_distance_min_threshold: 2.5
           longitudinal_velocity_delta_time: 0.6
+        stuck:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -1.0
+          rear_vehicle_reaction_time: 2.0
+          rear_vehicle_safety_time_margin: 1.0
+          lateral_distance_max_threshold: 2.0
+          longitudinal_distance_min_threshold: 3.0
+          longitudinal_velocity_delta_time: 0.8
 
         # lane expansion for object filtering
         lane_expansion:

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -230,7 +230,8 @@ class LaneChangeBase
   virtual bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,
     const lanelet::ConstLanelets & target_lanelets, Direction direction,
-    LaneChangePaths * candidate_paths, const bool check_safety) const = 0;
+    LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params,
+    const bool is_stuck, const bool check_safety) const = 0;
 
   virtual TurnSignalInfo calcTurnSignalInfo() = 0;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -138,6 +138,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;
 
   TurnSignalInfo calcTurnSignalInfo() override;
@@ -146,7 +147,7 @@ class NormalLaneChange : public LaneChangeBase
 
   PathSafetyStatus isLaneChangePathSafe(
     const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-    const utils::path_safety_checker::RSSparams & rss_params,
+    const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
     CollisionCheckDebugMap & debug_data) const;
 
   LaneChangeTargetObjectIndices filterObject(

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -85,6 +85,7 @@ struct LaneChangeParameters
   bool allow_loose_check_for_cancel{true};
   utils::path_safety_checker::RSSparams rss_params{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
+  utils::path_safety_checker::RSSparams rss_params_for_stuck{};
 
   // abort
   LaneChangeCancelParameters cancel{};

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -128,6 +128,21 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.lateral_distance_max_threshold"));
 
+  p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold"));
+  p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time"));
+  p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.expected_front_deceleration"));
+  p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.expected_rear_deceleration"));
+  p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.rear_vehicle_reaction_time"));
+  p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin"));
+  p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.lateral_distance_max_threshold"));
+
   // target object
   {
     std::string ns = "lane_change.target_object.";

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -83,8 +83,17 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
 
   // find candidate paths
   LaneChangePaths valid_paths{};
-  const auto found_safe_path =
-    getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths);
+  const bool is_stuck = isVehicleStuckByObstacle(current_lanes);
+  bool found_safe_path = getLaneChangePaths(
+    current_lanes, target_lanes, direction_, &valid_paths, lane_change_parameters_->rss_params,
+    is_stuck);
+  // if no safe path is found and ego is stuck, try to find a path with a small margin
+  if (!found_safe_path && is_stuck) {
+    found_safe_path = getLaneChangePaths(
+      current_lanes, target_lanes, direction_, &valid_paths,
+      lane_change_parameters_->rss_params_for_stuck, is_stuck);
+  }
+
   debug_valid_path_ = valid_paths;
 
   if (valid_paths.empty()) {
@@ -1006,7 +1015,9 @@ bool NormalLaneChange::hasEnoughLengthToIntersection(
 
 bool NormalLaneChange::getLaneChangePaths(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-  Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const
+  Direction direction, LaneChangePaths * candidate_paths,
+  const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
+  const bool check_safety) const
 {
   object_debug_.clear();
   if (current_lanes.empty() || target_lanes.empty()) {
@@ -1237,9 +1248,11 @@ bool NormalLaneChange::getLaneChangePaths(
         }
 
         candidate_paths->push_back(*candidate_path);
-        if (utils::lane_change::passParkedObject(
-              route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
-              is_goal_in_route, *lane_change_parameters_, object_debug_)) {
+        if (
+          !is_stuck &&
+          utils::lane_change::passParkedObject(
+            route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
+            is_goal_in_route, *lane_change_parameters_, object_debug_)) {
           return false;
         }
 
@@ -1248,7 +1261,7 @@ bool NormalLaneChange::getLaneChangePaths(
         }
 
         const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe(
-          *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_);
+          *candidate_path, target_objects, rss_params, is_stuck, object_debug_);
 
         if (is_safe) {
           return true;
@@ -1270,8 +1283,9 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   debug_filtered_objects_ = target_objects;
 
   CollisionCheckDebugMap debug_data;
+  const bool is_stuck = isVehicleStuckByObstacle(current_lanes);
   const auto safety_status = isLaneChangePathSafe(
-    path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data);
+    path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data);
   {
     // only for debug purpose
     object_debug_.clear();
@@ -1528,7 +1542,7 @@ bool NormalLaneChange::getAbortPath()
 
 PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
   const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-  const utils::path_safety_checker::RSSparams & rss_params,
+  const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
   CollisionCheckDebugMap & debug_data) const
 {
   PathSafetyStatus path_safety_status;
@@ -1552,7 +1566,6 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 
   auto collision_check_objects = target_objects.target_lane;
   const auto current_lanes = getCurrentLanes();
-  const auto is_stuck = isVehicleStuckByObstacle(current_lanes);
 
   if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) {
     collision_check_objects.insert(