From 15b10b38a153f571ef39a5b589c77c4730447a50 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 6 Feb 2024 18:14:37 +0900 Subject: [PATCH 1/4] feat(safety_check): add new function to create polygon along path Signed-off-by: satoshi-ota --- .../path_safety_checker_parameters.hpp | 2 + .../path_safety_checker/safety_check.cpp | 104 +++++++++++++++++- 2 files changed, 102 insertions(+), 4 deletions(-) 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..6a0d89a6bca33 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,8 @@ struct TargetObjectsOnLane */ struct RSSparams { + std::string steering{"fixed"}; ///< Policy to create collision check polygon. + /// possible values: ["fixed", "free"] 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..c37173c9c7eb2 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(), forward_lon_offset, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + lon_offset_pose.value(), forward_lon_offset, -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.steering == "fixed") { + return createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, debug); + } + + if (rss_parameters.steering == "free") { + 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 'fixed' or 'free'."); + }(); const auto & extended_obj_polygon = is_object_front ? obj_polygon From 10a68ec897a479888855f69e392d2592a2de1627 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 6 Feb 2024 18:15:25 +0900 Subject: [PATCH 2/4] feat(avoidance): use free steer policy for safety check Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/config/avoidance.param.yaml | 1 + .../include/behavior_path_avoidance_module/parameter_helper.hpp | 1 + 2 files changed, 2 insertions(+) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index d9ae9ed623cb3..6af04ba4bae50 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 + steering: "free" # [-] select "fixed" or "free" 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..13a792600da7f 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,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // safety check rss params { const std::string ns = "avoidance.safety_check."; + p.rss_params.steering = getOrDeclareParameter(*node, ns + "steering"); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = From c42fa54e3c589386de37a9b499849206c19e7484 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 16 Feb 2024 08:52:20 +0900 Subject: [PATCH 3/4] fix(safety_check): fix polygon edge Signed-off-by: satoshi-ota --- .../src/utils/path_safety_checker/safety_check.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 c37173c9c7eb2..61da625032894 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 @@ -232,14 +232,14 @@ Polygon2d createExtendedPolygonAlongPath( } { - const auto p_offset = tier4_autoware_utils::calcOffsetPose( - lon_offset_pose.value(), forward_lon_offset, lat_offset, 0.0); + 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(), forward_lon_offset, -lat_offset, 0.0); + lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } From d0252690419ab6b7359813303faffbcb13097a32 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 16 Feb 2024 09:17:16 +0900 Subject: [PATCH 4/4] fix(safety_check): fix param name Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 2 +- .../behavior_path_avoidance_module/parameter_helper.hpp | 3 ++- .../path_safety_checker/path_safety_checker_parameters.hpp | 5 +++-- .../src/utils/path_safety_checker/safety_check.cpp | 6 +++--- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 6af04ba4bae50..ab4eda81f93c6 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -187,7 +187,7 @@ time_horizon_for_rear_object: 10.0 # [s] delay_until_departure: 0.0 # [s] # rss parameters - steering: "free" # [-] select "fixed" or "free" + 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 13a792600da7f..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,7 +223,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // safety check rss params { const std::string ns = "avoidance.safety_check."; - p.rss_params.steering = getOrDeclareParameter(*node, ns + "steering"); + 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 6a0d89a6bca33..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,8 +126,9 @@ struct TargetObjectsOnLane */ struct RSSparams { - std::string steering{"fixed"}; ///< Policy to create collision check polygon. - /// possible values: ["fixed", "free"] + 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 61da625032894..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 @@ -636,18 +636,18 @@ std::vector getCollidedPolygons( return ego_polygon; } - if (rss_parameters.steering == "fixed") { + 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.steering == "free") { + 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 'fixed' or 'free'."); + throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'."); }(); const auto & extended_obj_polygon = is_object_front