From 793892060cc9dd415e34d796f23fedb66c4c28cd Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 16 Feb 2024 18:01:40 +0900 Subject: [PATCH] feat(safety_check): add option to use polygon along path in safety check (#6336) * feat(safety_check): add new function to create polygon along path Signed-off-by: satoshi-ota * feat(avoidance): use free steer policy for safety check Signed-off-by: satoshi-ota * fix(safety_check): fix polygon edge Signed-off-by: satoshi-ota * fix(safety_check): fix param name Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 1 + .../parameter_helper.hpp | 2 + .../path_safety_checker_parameters.hpp | 3 + .../path_safety_checker/safety_check.cpp | 104 +++++++++++++++++- 4 files changed, 106 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index d9ae9ed623cb3..ab4eda81f93c6 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -187,6 +187,7 @@ time_horizon_for_rear_object: 10.0 # [s] delay_until_departure: 0.0 # [s] # rss parameters + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] rear_vehicle_reaction_time: 2.0 # [s] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 03902e4328eb2..fcaad191b1a33 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -223,6 +223,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // safety check rss params { const std::string ns = "avoidance.safety_check."; + p.rss_params.extended_polygon_policy = + getOrDeclareParameter(*node, ns + "extended_polygon_policy"); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index d10a2f38fb975..b3c8034a3545c 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -126,6 +126,9 @@ struct TargetObjectsOnLane */ struct RSSparams { + std::string extended_polygon_policy{ + "rectangle"}; ///< Policy to create collision check polygon. + ///< possible values: ["rectangle", "along_path"] double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle. double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle. double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance. diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index e209e8dba36be..7b94066b74db3 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -184,6 +184,88 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +Polygon2d createExtendedPolygonAlongPath( + const PathWithLaneId & planned_path, const Pose & base_link_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length, + const double lat_margin, const double is_stopped_obj, CollisionCheckDebug & debug) +{ + const double & base_to_front = vehicle_info.max_longitudinal_offset_m; + const double & width = vehicle_info.vehicle_width_m; + const double & base_to_rear = vehicle_info.rear_overhang_m; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = + -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value + const double lat_offset = width / 2.0 + lat_margin; + + { + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = lat_offset; + } + + const auto lon_offset_pose = motion_utils::calcLongitudinalOffsetPose( + planned_path.points, base_link_pose.position, lon_length); + if (!lon_offset_pose.has_value()) { + return createExtendedPolygon( + base_link_pose, vehicle_info, lon_length, lat_margin, is_stopped_obj, debug); + } + + const size_t start_idx = + motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); + const size_t end_idx = + motion_utils::findNearestSegmentIndex(planned_path.points, lon_offset_pose.value().position); + + Polygon2d polygon; + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + for (size_t i = start_idx + 1; i < end_idx + 1; ++i) { + const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); + const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + for (size_t i = end_idx; i > start_idx; --i) { + const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); + const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector createExtendedPolygonsFromPoseWithVelocityStamped( const std::vector & predicted_path, const VehicleInfo & vehicle_info, const double forward_margin, const double backward_margin, const double lat_margin) @@ -549,10 +631,24 @@ std::vector getCollidedPolygons( const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; // TODO(watanabe) fix hard coding value const bool is_stopped_object = object_velocity < 0.3; - const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon( - ego_pose, ego_vehicle_info, lon_offset, - lat_margin, is_stopped_object, debug) - : ego_polygon; + const auto extended_ego_polygon = [&]() { + if (!is_object_front) { + return ego_polygon; + } + + if (rss_parameters.extended_polygon_policy == "rectangle") { + return createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, debug); + } + + if (rss_parameters.extended_polygon_policy == "along_path") { + return createExtendedPolygonAlongPath( + planned_path, ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, + debug); + } + + throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'."); + }(); const auto & extended_obj_polygon = is_object_front ? obj_polygon