From 09a92f5f4463fcc4db18897931471f5aaa53bd9f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 23 Aug 2023 23:00:00 +0900 Subject: [PATCH 1/5] refactor(safety_check): use safety check common param struct (#4719) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 8 +++++ .../config/behavior_path_planner.param.yaml | 13 -------- .../config/lane_change/lane_change.param.yaml | 12 ++++++++ .../behavior_path_planner/parameters.hpp | 14 --------- .../scene_module/lane_change/normal.hpp | 2 +- .../utils/avoidance/avoidance_module_data.hpp | 4 +++ .../lane_change/lane_change_module_data.hpp | 4 +++ .../path_safety_checker_parameters.hpp | 2 ++ .../path_safety_checker/safety_check.hpp | 7 ++--- .../src/behavior_path_planner_node.cpp | 17 ----------- .../avoidance/avoidance_module.cpp | 4 +-- .../src/scene_module/avoidance/manager.cpp | 21 ++++++++++++- .../src/scene_module/lane_change/manager.cpp | 30 +++++++++++++++++++ .../src/scene_module/lane_change/normal.cpp | 11 +++---- .../path_safety_checker/safety_check.cpp | 28 ++++++++--------- .../test/test_safety_check.cpp | 8 +++-- 16 files changed, 108 insertions(+), 77 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 8d413f54817fc..0c8797cb39c59 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -146,6 +146,14 @@ safety_check_backward_distance: 100.0 # [m] safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] + # rss parameters + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.8 # [s] # For avoidance maneuver avoidance: diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 02dc13ffd84da..95c60612bfdb7 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,19 +25,6 @@ visualize_maximum_drivable_area: true - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 # [s] - - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 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 8fabe8daa85b2..544471d9f8a91 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 @@ -27,6 +27,18 @@ min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + # safety check + safety_check: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.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 + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 1fdf4e020d71f..0cad130ffffdb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -142,20 +142,6 @@ struct BehaviorPathPlannerParameters // maximum drivable area visualization bool visualize_maximum_drivable_area; - - // collision check - double lateral_distance_max_threshold; - double longitudinal_distance_min_threshold; - double longitudinal_velocity_delta_time; - - double expected_front_deceleration; // brake parameter under normal lane change - double expected_rear_deceleration; // brake parameter under normal lane change - - double expected_front_deceleration_for_abort; // hard brake parameter for abort - double expected_rear_deceleration_for_abort; // hard brake parameter for abort - - double rear_vehicle_reaction_time; - double rear_vehicle_safety_time_margin; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ 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 0b63d8e40ae81..00c60cbfa8a6d 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 @@ -140,7 +140,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const; rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index ac6afde3ff47c..b51fbba92b148 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -302,6 +303,9 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // rss parameters + utils::path_safety_checker::RSSparams rss_params; + // clip left and right bounds for objects bool enable_bound_clipping{false}; 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 721c5b00f6e33..0556a780467c0 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 @@ -76,6 +76,10 @@ struct LaneChangeParameters bool check_motorcycle{true}; // check object motorbike bool check_pedestrian{true}; // check object pedestrian + // safety check + utils::path_safety_checker::RSSparams rss_params; + utils::path_safety_checker::RSSparams rss_params_for_abort; + // abort LaneChangeCancelParameters cancel; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 519f589f561ee..c4a6a86861e6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -123,6 +123,8 @@ struct RSSparams double longitudinal_distance_min_threshold{ 0.0}; ///< Minimum threshold for longitudinal distance. double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. + double front_vehicle_deceleration; ///< brake parameter + double rear_vehicle_deceleration; ///< brake parameter }; /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8606f2cd2397d..6d9df7677ead5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -65,8 +65,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, @@ -98,8 +97,8 @@ bool checkCollision( const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug); + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug); /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 29f1ee762abdf..1302b579d2875 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -391,23 +391,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold"); - p.longitudinal_distance_min_threshold = - declare_parameter("longitudinal_distance_min_threshold"); - p.longitudinal_velocity_delta_time = - declare_parameter("longitudinal_velocity_delta_time"); - - p.expected_front_deceleration = declare_parameter("expected_front_deceleration"); - p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration"); - - p.expected_front_deceleration_for_abort = - declare_parameter("expected_front_deceleration_for_abort"); - p.expected_rear_deceleration_for_abort = - declare_parameter("expected_rear_deceleration_for_abort"); - - p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time"); - p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin"); - if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 66808be897ae0..24a286cc1809d 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1924,8 +1924,8 @@ bool AvoidanceModule::isSafePath( for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( - shifted_path.path, ego_predicted_path, object, obj_path, p, - p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { + shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, + collision)) { return false; } } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index a039c0388b6a4..638b47e46fc66 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -136,7 +136,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); } - // safety check + // safety check general params { std::string ns = "avoidance.safety_check."; p.enable_safety_check = get_parameter(node, ns + "enable"); @@ -155,6 +155,25 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); } + // safety check rss params + { + std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, ns + "lateral_distance_max_threshold"); + } + // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; 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 18c669a207095..fff078ce7ef3d 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 @@ -85,6 +85,36 @@ LaneChangeModuleManager::LaneChangeModuleManager( get_parameter(node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + + p.rss_params_for_abort.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params_for_abort.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params_for_abort.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.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 f4ef15ad50cd6..2ab306cebd7d0 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 @@ -937,8 +937,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, object_debug_); + *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); if (is_safe) { return true; @@ -951,7 +950,6 @@ bool NormalLaneChange::getLaneChangePaths( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { - const auto & common_parameters = getCommonParam(); const auto & path = status_.lane_change_path; const auto & current_lanes = status_.current_lanes; const auto & target_lanes = status_.target_lanes; @@ -960,8 +958,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; const auto safety_status = isLaneChangePathSafe( - path, target_objects, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1218,7 +1215,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const { PathSafetyStatus path_safety_status; @@ -1280,7 +1277,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 34ba607938385..c6a68e108e7e1 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -135,8 +135,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { const auto stoppingDistance = [](const auto vehicle_velocity, const auto vehicle_accel) { // compensate if user accidentally set the deceleration to some positive value @@ -145,23 +144,23 @@ double calcRssDistance( }; const double & reaction_time = - params.rear_vehicle_reaction_time + params.rear_vehicle_safety_time_margin; + rss_params.rear_vehicle_reaction_time + rss_params.rear_vehicle_safety_time_margin; const double front_object_stop_length = - stoppingDistance(front_object_velocity, front_object_deceleration); + stoppingDistance(front_object_velocity, rss_params.front_vehicle_deceleration); const double rear_object_stop_length = rear_object_velocity * reaction_time + - stoppingDistance(rear_object_velocity, rear_object_deceleration); + stoppingDistance(rear_object_velocity, rss_params.rear_vehicle_deceleration); return rear_object_stop_length - front_object_stop_length; } double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { - const double & lon_threshold = params.longitudinal_distance_min_threshold; + const double & lon_threshold = rss_params.longitudinal_distance_min_threshold; const auto max_vel = std::max(front_object_velocity, rear_object_velocity); - return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; + return rss_params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } boost::optional calcInterpolatedPoseWithVelocity( @@ -219,8 +218,8 @@ bool checkCollision( const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug) + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug) { debug.lerped_path.reserve(target_object_path.path.size()); @@ -267,16 +266,15 @@ bool checkCollision( : std::make_pair(ego_velocity, object_velocity); // compute rss dist - const auto rss_dist = calcRssDistance( - front_object_velocity, rear_object_velocity, front_object_deceleration, - rear_object_deceleration, common_parameters); + const auto rss_dist = + calcRssDistance(front_object_velocity, rear_object_velocity, rss_parameters); // minimum longitudinal length const auto min_lon_length = - calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = common_parameters.lateral_distance_max_threshold; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index e24f3274179fe..e1b74636f5ed1 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -178,18 +178,20 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { using behavior_path_planner::utils::path_safety_checker::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::RSSparams; { const double front_vel = 5.0; const double front_decel = -2.0; const double rear_vel = 10.0; const double rear_decel = -1.0; - BehaviorPathPlannerParameters params; + RSSparams params; params.rear_vehicle_reaction_time = 1.0; params.rear_vehicle_safety_time_margin = 1.0; params.longitudinal_distance_min_threshold = 3.0; + params.rear_vehicle_deceleration = rear_decel; + params.front_vehicle_deceleration = front_decel; - EXPECT_NEAR( - calcRssDistance(front_vel, rear_vel, front_decel, rear_decel, params), 63.75, epsilon); + EXPECT_NEAR(calcRssDistance(front_vel, rear_vel, params), 63.75, epsilon); } } From 37577fad153c92d9137a45398d2de3840fbf0315 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 29 Aug 2023 08:14:29 +0900 Subject: [PATCH 2/5] feat(avoidance): flexible avoidance safety check param (#4754) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 3 ++- .../utils/avoidance/avoidance_module_data.hpp | 3 ++- .../utils/avoidance/utils.hpp | 2 +- .../scene_module/avoidance/avoidance_module.cpp | 16 ++++++++++++++-- .../src/scene_module/avoidance/manager.cpp | 11 ++++++++--- .../src/utils/avoidance/utils.cpp | 8 +++++--- 6 files changed, 32 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 0c8797cb39c59..968cd1d0110fe 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -142,7 +142,8 @@ # collision check parameters check_all_predicted_path: false # [-] time_resolution: 0.5 # [s] - time_horizon: 10.0 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index b51fbba92b148..c57f94f27e430 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -193,7 +193,8 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double safety_check_time_horizon{0.0}; + double time_horizon_for_front_object{0.0}; + double time_horizon_for_rear_object{0.0}; double safety_check_time_resolution{0.0}; // find adjacent lane vehicles diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 420ace9458220..ede0e5bfb728c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -88,7 +88,7 @@ std::vector generateObstaclePolygonsForDrivableArea( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); + const bool is_object_front, const std::shared_ptr & parameters); double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 24a286cc1809d..bb62516ddbfab 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1891,8 +1891,10 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const auto ego_predicted_path = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); + const auto ego_predicted_path_for_front_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, true, parameters_); + const auto ego_predicted_path_for_rear_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, false, parameters_); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { @@ -1919,8 +1921,18 @@ bool AvoidanceModule::isSafePath( avoidance_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { + const auto obj_polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + + const auto is_object_front = utils::path_safety_checker::isTargetObjectFront( + shifted_path.path, getEgoPose(), p.vehicle_info, obj_polygon); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); + + const auto & ego_predicted_path = + is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 638b47e46fc66..257ccf52f2a4e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -143,10 +143,15 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_current_lane = get_parameter(node, ns + "check_current_lane"); p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); - p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); + p.check_unavoidable_object = + get_parameter(node, ns + "check_unavoidable_object"); p.check_other_object = get_parameter(node, ns + "check_other_object"); - p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); - p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); + p.check_all_predicted_path = + get_parameter(node, ns + "check_all_predicted_path"); + p.time_horizon_for_front_object = + get_parameter(node, ns + "time_horizon_for_front_object"); + p.time_horizon_for_rear_object = + get_parameter(node, ns + "time_horizon_for_rear_object"); p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 2e6d8f982b8a2..4b109a63ac1ce 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1310,7 +1310,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const bool is_object_front, const std::shared_ptr & parameters) { if (path.points.empty()) { return {}; @@ -1319,7 +1319,8 @@ std::vector convertToPredictedPath( const auto & acceleration = parameters->max_acceleration; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = is_object_front ? parameters->time_horizon_for_front_object + : parameters->time_horizon_for_rear_object; const auto & time_resolution = parameters->safety_check_time_resolution; const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); @@ -1350,7 +1351,8 @@ ExtendedPredictedObject transform( extended_object.shape = object.shape; const auto & obj_velocity = extended_object.initial_twist.twist.linear.x; - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = + std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object); const auto & time_resolution = parameters->safety_check_time_resolution; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); From 3beb77cc94eedf50bb36660dcbef90b558fe792e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 29 Aug 2023 10:05:42 +0900 Subject: [PATCH 3/5] perf(safety_check): use light weight object position check logic (#4762) perf(safety_check): use light weight object position check Signed-off-by: satoshi-ota --- .../path_safety_checker/safety_check.hpp | 3 +++ .../avoidance/avoidance_module.cpp | 4 ++-- .../path_safety_checker/safety_check.cpp | 24 ++++++++++++++++--- 3 files changed, 26 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 6d9df7677ead5..4a50065014f59 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -52,6 +52,9 @@ using vehicle_info_util::VehicleInfo; namespace bg = boost::geometry; +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index bb62516ddbfab..9122c244411dc 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1924,8 +1924,8 @@ bool AvoidanceModule::isSafePath( const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); - const auto is_object_front = utils::path_safety_checker::isTargetObjectFront( - shifted_path.path, getEgoPose(), p.vehicle_info, obj_polygon); + const auto is_object_front = + utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index c6a68e108e7e1..582c4922c4b0a 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -30,6 +30,25 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_offset_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + + // check all edges in the polygon + for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + return true; + } + } + + return false; +} + bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) @@ -214,7 +233,7 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( - const PathWithLaneId & planned_path, + [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, @@ -259,8 +278,7 @@ bool checkCollision( } // compute which one is at the front of the other - const bool is_object_front = - isTargetObjectFront(planned_path, ego_pose, ego_vehicle_info, obj_polygon); + const bool is_object_front = isTargetObjectFront(ego_pose, obj_polygon, ego_vehicle_info); const auto & [front_object_velocity, rear_object_velocity] = is_object_front ? std::make_pair(object_velocity, ego_velocity) : std::make_pair(ego_velocity, object_velocity); From 71277da29b07ca22e7f1e4fc566aba3105aeef69 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sun, 27 Aug 2023 21:50:20 +0900 Subject: [PATCH 4/5] feat(safety_check): add hysteresis factor in safety check logic (#4735) * feat(safety_check): add hysteresis factor Signed-off-by: satoshi-ota * feat(lane_change): add hysteresis factor Signed-off-by: satoshi-ota * feat(avoidance): add hysteresis factor Signed-off-by: satoshi-ota * feat(avoidance): add time series hysteresis Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 3 ++- .../scene_module/avoidance/avoidance_module.hpp | 4 ++++ .../utils/avoidance/avoidance_module_data.hpp | 3 ++- .../utils/path_safety_checker/safety_check.hpp | 2 +- .../src/scene_module/avoidance/avoidance_module.cpp | 11 +++++++++-- .../src/scene_module/avoidance/manager.cpp | 5 +++-- .../src/scene_module/lane_change/manager.cpp | 4 ++-- .../src/scene_module/lane_change/normal.cpp | 2 +- .../src/utils/avoidance/utils.cpp | 2 +- .../src/utils/path_safety_checker/safety_check.cpp | 6 +++--- 10 files changed, 28 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 968cd1d0110fe..53db7ee81c40e 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -145,8 +145,9 @@ time_horizon_for_front_object: 10.0 # [s] time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] + hysteresis_factor_expand_rate: 2.0 # [-] + hysteresis_factor_safe_count: 10 # [-] # rss parameters expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 01c82e62c1cdc..cda680b9dec7c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -555,6 +555,8 @@ class AvoidanceModule : public SceneModuleInterface bool arrived_path_end_{false}; + bool safe_{true}; + std::shared_ptr parameters_; helper::avoidance::AvoidanceHelper helper_; @@ -575,6 +577,8 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray registered_objects_; + mutable size_t safe_count_{0}; + mutable ObjectDataArray ego_stopped_objects_; mutable ObjectDataArray stopped_objects_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index c57f94f27e430..27cd89e7756cc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -201,7 +201,8 @@ struct AvoidanceParameters double safety_check_backward_distance; // transit hysteresis (unsafe to safe) - double safety_check_hysteresis_factor; + size_t hysteresis_factor_safe_count; + double hysteresis_factor_expand_rate; // don't output new candidate path if the offset between ego and path is larger than this. double safety_check_ego_offset; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 4a50065014f59..a0d4282c5f232 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -101,7 +101,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug); + const double hysteresis_factor, CollisionCheckDebug & debug); /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 9122c244411dc..1630ce7465c6e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1917,6 +1917,8 @@ bool AvoidanceModule::isSafePath( return true; } + const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoidance_data_, planner_data_, parameters_, is_right_shift.value()); @@ -1937,13 +1939,16 @@ bool AvoidanceModule::isSafePath( CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - collision)) { + hysteresis_factor, collision)) { + safe_count_ = 0; return false; } } } - return true; + safe_count_++; + + return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const @@ -2589,6 +2594,8 @@ void AvoidanceModule::updateData() fillShiftLine(avoidance_data_, debug_data_); fillEgoStatus(avoidance_data_, debug_data_); fillDebugData(avoidance_data_, debug_data_); + + safe_ = avoidance_data_.safe; } void AvoidanceModule::processOnEntry() diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 257ccf52f2a4e..bec3ba0afd025 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -155,9 +155,10 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = get_parameter(node, ns + "hysteresis_factor_safe_count"); } // safety check rss params 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 fff078ce7ef3d..7692cd624bffd 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 @@ -294,8 +294,8 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // safety check { std::string ns = "avoidance.safety_check."; - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); } avoidance_parameters_ = std::make_shared(p); 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 2ab306cebd7d0..eee17d79af648 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 @@ -1277,7 +1277,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 4b109a63ac1ce..10766b7aec416 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -645,7 +645,7 @@ void fillAvoidanceNecessity( } // TRUE -> ? (check with hysteresis factor) - object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor); + object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } void fillObjectStoppableJudge( diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 582c4922c4b0a..6dd3b627e67f4 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -238,7 +238,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug) + double hysteresis_factor, CollisionCheckDebug & debug) { debug.lerped_path.reserve(target_object_path.path.size()); @@ -291,8 +291,8 @@ bool checkCollision( const auto min_lon_length = calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); - const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; + const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) From 03c987c3868097bfae5eae0b24c36534327b1e8d Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 11 Sep 2023 15:24:21 +0900 Subject: [PATCH 5/5] feat(avoidance): improve canceling judgement Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 8 ++++++-- .../src/scene_module/avoidance/manager.cpp | 6 ++---- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 1630ce7465c6e..40fc6ef7b53a6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1929,11 +1929,15 @@ bool AvoidanceModule::isSafePath( const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + const auto is_object_incoming = + std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); - const auto & ego_predicted_path = - is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + const auto & ego_predicted_path = is_object_front && !is_object_incoming + ? ego_predicted_path_for_front_object + : ego_predicted_path_for_rear_object; for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index bec3ba0afd025..a9e8982a9a48a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -143,11 +143,9 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_current_lane = get_parameter(node, ns + "check_current_lane"); p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); - p.check_unavoidable_object = - get_parameter(node, ns + "check_unavoidable_object"); + p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); p.check_other_object = get_parameter(node, ns + "check_other_object"); - p.check_all_predicted_path = - get_parameter(node, ns + "check_all_predicted_path"); + p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); p.time_horizon_for_front_object = get_parameter(node, ns + "time_horizon_for_front_object"); p.time_horizon_for_rear_object =