From 2e0d5de5195ef515d292505c64cdf7893d971c2d Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Tue, 19 Nov 2024 21:12:19 +0900 Subject: [PATCH] test(bpp_common): add unit test for safety check (#9386) * fix docstring Signed-off-by: Go Sakayori * add basic collision test Signed-off-by: Go Sakayori * add some more tests Signed-off-by: Go Sakayori * add unit test for all functions Signed-off-by: Go Sakayori * remove unecessary header and space Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../path_safety_checker/safety_check.hpp | 69 ++++- .../test/test_safety_check.cpp | 263 ++++++++++++++++-- 2 files changed, 297 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 56ffd99905579..ab62c369c41e4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -76,9 +76,22 @@ Polygon2d createExtendedPolygon( const PoseWithVelocityAndPolygonStamped & obj_pose_with_poly, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); +/** + * @brief Converts path (path with velocity stamped) to predicted path. + * @param path Path. + * @param time_resolution Time resolution. + * @return Predicted path. + */ PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution); +/** + * @brief Calculates RSS related distance. + * @param front_object_velocity Velocity of front object. + * @param rear_object_velocity Velocity of rear object. + * @param rss_params RSS parameters. + * @return Longitudinal distance. + */ double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, const RSSparams & rss_params); @@ -128,9 +141,29 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( const ExtendedPredictedObjects & objects, const double time_horizon, const bool check_all_predicted_path); +/** + * @brief Filters the path by obtaining points after target pose. + * @param path Path to filter. + * @param target_pose Target pose. + * @return Filtered path. + */ std::vector filterPredictedPathAfterTargetPose( const std::vector & path, const Pose & target_pose); +/** + * @brief Iterate the points in the ego and target's predicted path and + * perform safety check for each of the iterated points using RSS parameters + * @param planned_path The planned path of the ego vehicle. + * @param ego_predicted_path Ego vehicle's predicted path + * @param objects Detected objects. + * @param debug_map Map for collision check debug. + * @param parameters The common parameters used in behavior path planner. + * @param rss_params The parameters used in RSSs + * @param check_all_predicted_path If true, uses all predicted path + * @param hysteresis_factor Hysteresis factor + * @param yaw_difference_th Threshold for yaw difference + * @return True if planned path is safe. + */ bool checkSafetyWithRSS( const PathWithLaneId & planned_path, const std::vector & ego_predicted_path, @@ -144,16 +177,14 @@ bool checkSafetyWithRSS( * perform safety check for each of the iterated points. * @param planned_path The predicted path of the ego vehicle. * @param predicted_ego_path Ego vehicle's predicted path - * @param ego_current_velocity Current velocity of the ego vehicle. * @param target_object The predicted object to check collision with. * @param target_object_path The predicted path of the target object. - * @param common_parameters The common parameters used in behavior path planner. - * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) - * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) - * @param yaw_difference_th maximum yaw difference between any given ego path pose and object - * predicted path pose. + * @param common_parameters Common parameters used for behavior path planning. + * @param rss_parameters The parameters used in RSS. + * @param hysteresis_factor Hysteresis factor. + * @param yaw_difference_th Threshold of yaw difference. * @param debug The debug information for collision checking. - * @return true if distance is safe. + * @return True if there is no collision. */ bool checkCollision( const PathWithLaneId & planned_path, @@ -166,16 +197,17 @@ bool checkCollision( /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. - * @param planned_path The predicted path of the ego vehicle. + * @param planned_path The planned path of the ego vehicle. * @param predicted_ego_path Ego vehicle's predicted path - * @param ego_current_velocity Current velocity of the ego vehicle. * @param target_object The predicted object to check collision with. * @param target_object_path The predicted path of the target object. - * @param common_parameters The common parameters used in behavior path planner. - * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) - * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param vehicle_info Ego vehicle information. + * @param rss_parameters The parameters used in RSS. + * @param hysteresis_factor Hysteresis factor. + * @param max_velocity_limit Maximum velocity of ego vehicle. + * @param yaw_difference_th Threshold of yaw difference. * @param debug The debug information for collision checking. - * @return a list of polygon when collision is expected. + * @return List of polygon which collision is expected. */ std::vector get_collided_polygons( const PathWithLaneId & planned_path, @@ -187,6 +219,17 @@ std::vector get_collided_polygons( bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); + +/** + * @brief Checks for safety using integral predicted polygons. + * @param ego_predicted_path The predicted path of ego vehicle. + * @param vehicle_info Information (parameters) about ego vehicle. + * @param objects Surrounding objects. + * @param check_all_predicted_path Whether to check all predicted paths of objects. + * @param params Parameters for integral predicted polygon. + * @param debug_map Map to store debug information. + * @return True if the ego vehicle's path is safe, and false otherwise. + */ bool checkSafetyWithIntegralPredictedPolygon( const std::vector & ego_predicted_path, const VehicleInfo & vehicle_info, const ExtendedPredictedObjects & objects, const bool check_all_predicted_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 0d36b2c3fa377..c5694db10f65e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -22,6 +21,7 @@ #include #include +#include #include #include @@ -29,15 +29,21 @@ #include +#include +#include #include #include constexpr double epsilon = 1e-6; using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using autoware::behavior_path_planner::utils::path_safety_checker:: PoseWithVelocityAndPolygonStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; +using autoware::behavior_path_planner::utils::path_safety_checker::RSSparams; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; using autoware::universe_utils::Polygon2d; @@ -45,7 +51,7 @@ using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; -std::vector createTestPath() +std::vector create_test_path() { std::vector path; path.emplace_back(0.0, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), 1.0); @@ -54,6 +60,69 @@ std::vector createTestPath() return path; } +RSSparams create_rss_parameters( + double longitudinal_velocity_delta_time = 2.0, double rear_vehicle_reaction_time = 1.0, + double rear_vehicle_safety_time_margin = 1.0, double longitudinal_distance_min_threshold = 3.0, + double rear_vehicle_deceleration = -1.0, double front_vehicle_deceleration = -2.0) +{ + RSSparams params; + params.longitudinal_velocity_delta_time = longitudinal_velocity_delta_time; + params.rear_vehicle_reaction_time = rear_vehicle_reaction_time; + params.rear_vehicle_safety_time_margin = rear_vehicle_safety_time_margin; + params.longitudinal_distance_min_threshold = longitudinal_distance_min_threshold; + params.rear_vehicle_deceleration = rear_vehicle_deceleration; + params.front_vehicle_deceleration = front_vehicle_deceleration; + params.extended_polygon_policy = "rectangle"; + + return params; +} + +std::vector create_path_with_velocity_and_polygon( + const Pose initial_pose, const Shape & shape, size_t point_num, double interval, double velocity) +{ + std::vector predicted_path; + predicted_path.reserve(point_num); + Pose pose = initial_pose; + + for (size_t i = 0; i < point_num; i++) { + double time = static_cast(i) * interval; + pose.position.x = initial_pose.position.x + time * velocity; + PoseWithVelocityAndPolygonStamped obj_pose_with_poly( + time, pose, velocity, autoware::universe_utils::toPolygon2d(pose, shape)); + predicted_path.push_back(obj_pose_with_poly); + } + + return predicted_path; +} + +PredictedPathWithPolygon create_predicted_path_with_polygon(Pose pose, float confidence) +{ + PredictedPathWithPolygon path; + Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = 1.0; + shape.dimensions.y = 1.0; + path.path = create_path_with_velocity_and_polygon(pose, shape, 10, 1.0, 1.0); + path.confidence = confidence; + + return path; +} + +ExtendedPredictedObject create_extended_predicted_object(Pose pose, float confidence) +{ + ExtendedPredictedObject object; + Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = 1.0; + shape.dimensions.y = 1.0; + object.initial_pose = pose; + object.shape = shape; + object.initial_polygon = autoware::universe_utils::toPolygon2d(pose, shape); + object.predicted_paths.push_back(create_predicted_path_with_polygon(pose, confidence)); + + return object; +} + TEST(BehaviorPathPlanningSafetyUtilsTest, isTargetObjectOncoming) { using autoware::behavior_path_planner::utils::path_safety_checker::isTargetObjectOncoming; @@ -228,9 +297,6 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) CollisionCheckDebug debug; - using autoware::behavior_path_planner::utils::path_safety_checker:: - PoseWithVelocityAndPolygonStamped; - PoseWithVelocityAndPolygonStamped obj_pose_with_poly( 0.0, obj_pose, 0.0, autoware::universe_utils::toPolygon2d(obj_pose, shape)); const auto polygon = @@ -253,22 +319,26 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) } } +TEST(BehaviorPathPlanningSafetyUtilsTest, convertToPredictedPath) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::convertToPredictedPath; + + auto path = create_test_path(); + double time_resolution = 1.0; + auto predicted_path = convertToPredictedPath(path, time_resolution); + EXPECT_EQ(predicted_path.path.size(), 3); + EXPECT_DOUBLE_EQ(predicted_path.time_step.sec, 1.0); + EXPECT_DOUBLE_EQ(predicted_path.time_step.nanosec, 0.0); +} + TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { using autoware::behavior_path_planner::utils::path_safety_checker::calcRssDistance; - using autoware::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; - 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; + auto params = create_rss_parameters(); EXPECT_NEAR(calcRssDistance(front_vel, rear_vel, params), 63.75, epsilon); } @@ -281,9 +351,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, calc_minimum_longitudinal_length) double front_object_velocity = 10.0; double rear_object_velocity = 5.0; - autoware::behavior_path_planner::utils::path_safety_checker::RSSparams param; + auto param = create_rss_parameters(); param.longitudinal_distance_min_threshold = 4.0; - param.longitudinal_velocity_delta_time = 2.0; // Condition: front is faster than rear object EXPECT_DOUBLE_EQ( @@ -295,7 +364,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, calc_minimum_longitudinal_length) // Basic interpolation test TEST(CalcInterpolatedPoseWithVelocityTest, BasicInterpolation) { - auto path = createTestPath(); + auto path = create_test_path(); auto result = calc_interpolated_pose_with_velocity(path, 0.5); ASSERT_TRUE(result.has_value()); @@ -307,7 +376,7 @@ TEST(CalcInterpolatedPoseWithVelocityTest, BasicInterpolation) // Boundary conditions test TEST(CalcInterpolatedPoseWithVelocityTest, BoundaryConditions) { - auto path = createTestPath(); + auto path = create_test_path(); // First point of the path auto start_result = calc_interpolated_pose_with_velocity(path, 0.0); @@ -330,7 +399,7 @@ TEST(CalcInterpolatedPoseWithVelocityTest, InvalidInput) using autoware::behavior_path_planner::utils::path_safety_checker:: calc_interpolated_pose_with_velocity; - auto path = createTestPath(); + auto path = create_test_path(); // Empty path EXPECT_FALSE(calc_interpolated_pose_with_velocity({}, 1.0).has_value()); @@ -372,13 +441,11 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, get_interpolated_pose_with_velocity_an get_interpolated_pose_with_velocity_and_polygon_stamped; std::vector pred_path; - std::vector pred_path_with_polygon; double current_time = 0.5; autoware::vehicle_info_utils::VehicleInfo vehicle_info{}; vehicle_info.max_longitudinal_offset_m = 1.0; vehicle_info.rear_overhang_m = 1.0; vehicle_info.vehicle_width_m = 2.0; - Shape shape; // Condition: empty path EXPECT_FALSE( @@ -386,12 +453,164 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, get_interpolated_pose_with_velocity_an .has_value()); // Condition: with path - pred_path = createTestPath(); + pred_path = create_test_path(); auto interpolation_result = get_interpolated_pose_with_velocity_and_polygon_stamped(pred_path, current_time, vehicle_info); EXPECT_TRUE(interpolation_result.has_value()); } +TEST(BehaviorPathPlanningSafetyUtilsTest, filterPredictedPathAfterTargetPose) +{ + using autoware::behavior_path_planner::utils::path_safety_checker:: + filterPredictedPathAfterTargetPose; + + auto path = create_test_path(); + Pose pose = createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0); + auto filtered_path = filterPredictedPathAfterTargetPose(path, pose); + EXPECT_EQ(filtered_path.size(), 2); + double x_target = 1.0; + for (const auto & pose_with_velocity : filtered_path) { + EXPECT_DOUBLE_EQ(pose_with_velocity.pose.position.x, x_target); + x_target += 1.0; + } +} + +TEST(BehaviorPathPlanningSafetyUtilsTest, checkSafetyWithRSS) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS; + + auto planned_path = generateTrajectory(3, 1.0); + auto ego_predicted_path = create_test_path(); + autoware::vehicle_info_utils::VehicleInfo vehicle_info{}; + vehicle_info.max_longitudinal_offset_m = 1.0; + vehicle_info.rear_overhang_m = 1.0; + vehicle_info.vehicle_width_m = 2.0; + std::vector objects; + objects.push_back( + create_extended_predicted_object(createPose(10.0, 8.0, 0.0, 0.0, 0.0, 0.0), 0.5)); + objects.push_back( + create_extended_predicted_object(createPose(0.0, 1.0, 0.0, 0.0, 0.0, 0.0), 0.6)); + CollisionCheckDebugMap debug_map; + BehaviorPathPlannerParameters parameters; + parameters.vehicle_info = vehicle_info; + auto rss_params = create_rss_parameters(); + double hysteresis_factor = 1.0; + const double yaw_difference_th = M_PI_2; + + EXPECT_FALSE(checkSafetyWithRSS( + planned_path, ego_predicted_path, objects, debug_map, parameters, rss_params, true, + hysteresis_factor, yaw_difference_th)); + objects.pop_back(); + EXPECT_TRUE(checkSafetyWithRSS( + planned_path, ego_predicted_path, objects, debug_map, parameters, rss_params, true, + hysteresis_factor, yaw_difference_th)); +} + +TEST(BehaviorPathPlanningSafetyUtilsTest, checkSafetyWithIntegralPredictedPolygon) +{ + using autoware::behavior_path_planner::utils::path_safety_checker:: + checkSafetyWithIntegralPredictedPolygon; + + auto ego_predicted_path = create_test_path(); + autoware::vehicle_info_utils::VehicleInfo vehicle_info{}; + vehicle_info.max_longitudinal_offset_m = 1.0; + vehicle_info.rear_overhang_m = 1.0; + vehicle_info.vehicle_width_m = 2.0; + autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects objects; + objects.push_back( + create_extended_predicted_object(createPose(10.0, 8.0, 0.0, 0.0, 0.0, 0.0), 0.5)); + objects.push_back( + create_extended_predicted_object(createPose(0.0, 1.0, 0.0, 0.0, 0.0, 0.0), 0.6)); + + autoware::behavior_path_planner::utils::path_safety_checker::IntegralPredictedPolygonParams + params{1.0, 1.0, 1.0, 2.0}; + CollisionCheckDebugMap debug_map; + + EXPECT_FALSE(checkSafetyWithIntegralPredictedPolygon( + ego_predicted_path, vehicle_info, objects, true, params, debug_map)); + + objects.pop_back(); + EXPECT_TRUE(checkSafetyWithIntegralPredictedPolygon( + ego_predicted_path, vehicle_info, objects, true, params, debug_map)); +} + +TEST(BehaviorPathPlanningSafetyUtilsTest, checkCollision) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::checkCollision; + using autoware::behavior_path_planner::utils::path_safety_checker::get_collided_polygons; + + auto planned_path = autoware::test_utils::generateTrajectory(3, 1.0); + auto predicted_ego_path = create_test_path(); + + ExtendedPredictedObject target_object; + target_object.initial_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = 2.0; + shape.dimensions.y = 2.0; + auto object_path = + create_predicted_path_with_polygon(createPose(0.0, 1.0, 0.0, 0.0, 0.0, 0.0), 1.0); + + autoware::vehicle_info_utils::VehicleInfo vehicle_info; + vehicle_info.max_longitudinal_offset_m = 4.0; + vehicle_info.vehicle_width_m = 2.0; + vehicle_info.rear_overhang_m = 1.0; + BehaviorPathPlannerParameters common_parameters; + common_parameters.vehicle_info = vehicle_info; + + auto rss_parameters = create_rss_parameters(); + double hysteresis_factor = 1.0; + double max_velocity_limit = 10.0; + const double yaw_difference_th = M_PI_2; + CollisionCheckDebug debug; + + { + EXPECT_FALSE(checkCollision( + planned_path, predicted_ego_path, target_object, object_path, common_parameters, + rss_parameters, hysteresis_factor, yaw_difference_th, debug)); + auto collide_polygon = get_collided_polygons( + planned_path, predicted_ego_path, target_object, object_path, vehicle_info, rss_parameters, + hysteresis_factor, max_velocity_limit, yaw_difference_th, debug); + EXPECT_EQ(collide_polygon.size(), 3); + } + { + target_object.initial_pose = createPose(0.0, 4.0, 0.0, 0.0, 0.0, 0.0); + object_path = create_predicted_path_with_polygon(createPose(0.0, 4.0, 0.0, 0.0, 0.0, 0.0), 1.0); + auto collide_polygon = get_collided_polygons( + planned_path, predicted_ego_path, target_object, object_path, vehicle_info, rss_parameters, + hysteresis_factor, max_velocity_limit, yaw_difference_th, debug); + EXPECT_TRUE(checkCollision( + planned_path, predicted_ego_path, target_object, object_path, common_parameters, + rss_parameters, hysteresis_factor, yaw_difference_th, debug)); + EXPECT_TRUE(collide_polygon.empty()); + } + { + target_object.initial_pose = createPose(10.0, 4.0, 0.0, 0.0, 0.0, 0.0); + object_path = + create_predicted_path_with_polygon(createPose(10.0, 4.0, 0.0, 0.0, 0.0, 0.0), 1.0); + EXPECT_TRUE(checkCollision( + planned_path, predicted_ego_path, target_object, object_path, common_parameters, + rss_parameters, hysteresis_factor, yaw_difference_th, debug)); + auto collide_polygon = get_collided_polygons( + planned_path, predicted_ego_path, target_object, object_path, vehicle_info, rss_parameters, + hysteresis_factor, max_velocity_limit, yaw_difference_th, debug); + EXPECT_TRUE(collide_polygon.empty()); + } + { + rss_parameters.extended_polygon_policy = "along_path"; + target_object.initial_pose = createPose(10.0, 4.0, 0.0, 0.0, 0.0, 0.0); + object_path = + create_predicted_path_with_polygon(createPose(10.0, 4.0, 0.0, 0.0, 0.0, 0.0), 1.0); + EXPECT_TRUE(checkCollision( + planned_path, predicted_ego_path, target_object, object_path, common_parameters, + rss_parameters, hysteresis_factor, yaw_difference_th, debug)); + auto collide_polygon = get_collided_polygons( + planned_path, predicted_ego_path, target_object, object_path, vehicle_info, rss_parameters, + hysteresis_factor, max_velocity_limit, yaw_difference_th, debug); + EXPECT_TRUE(collide_polygon.empty()); + } +} + TEST(BehaviorPathPlanningSafetyUtilsTest, checkPolygonsIntersects) { using autoware::behavior_path_planner::utils::path_safety_checker::checkPolygonsIntersects;