diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index e7f3b51bd2d26..d3c0a22e867f9 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/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: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index a3469dee2909b..26f68e69b3728 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/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; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 4f1eb7340fdff..e9e361e1c39b8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/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( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 24a507d1f8695..8b9ef52394cdd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/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{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index e33a6c4b05ee3..3d7cf07308a79 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/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( *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); + p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold")); + p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time")); + p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_front_deceleration")); + p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_rear_deceleration")); + p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_reaction_time")); + p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin")); + p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c332438dd31a5..d155b204f62d6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -83,8 +83,17 @@ std::pair 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(