From c79878ebd1c5d9c4852fe13e4a5eb2342a90f1ec Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 6 Dec 2023 19:45:39 +0900 Subject: [PATCH 1/2] fix(start_planner): check safety only when waiting approval (#5792) 1. The `updateData()` function now sets `status_.is_safe_dynamic_objects` to true when `requiresDynamicObjectsCollisionDetection()` returns false. 2. The `isExecutionReady()` function now checks for dynamic object collisions only if `requiresDynamicObjectsCollisionDetection()` returns true and `isWaitingApproval()` also returns true. This change ensures that dynamic object collision detection is performed only when necessary and approval is pending. Signed-off-by: kyoichi-sugahara --- .../src/scene_module/start_planner/start_planner_module.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 7bbee7fe0a10c..23a981c4920b4 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -146,6 +146,8 @@ void StartPlannerModule::updateData() if (requiresDynamicObjectsCollisionDetection()) { status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects(); + } else { + status_.is_safe_dynamic_objects = true; } } @@ -279,7 +281,7 @@ bool StartPlannerModule::isExecutionReady() const is_safe = false; } - if (requiresDynamicObjectsCollisionDetection()) { + if (requiresDynamicObjectsCollisionDetection() && isWaitingApproval()) { is_safe = !hasCollisionWithDynamicObjects(); } From ca5cf61a78fbca2423cce0b5190a2ccbe917c887 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 7 Dec 2023 01:04:35 +0900 Subject: [PATCH 2/2] feat(start_planner): add surround moving obstacle check (#5782) * feat(start_planner): add surround moving obstacle check This commit introduces a new feature in the start_planner module for checking surrounding moving obstacles. - It adds parameters to specify the search radius and threshold velocity for moving obstacles, along with flags to indicate which types of objects should be checked. - The `noMovingObjectsAround` function has been added to filter dynamic objects within a certain radius based on their velocity. - If no moving objects are detected, the function returns true; otherwise, it returns false. - This feature enhances the safety of the start_planner by ensuring that the path can't be approved while surrond moving obstacles exist. --------- Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner.param.yaml | 14 +++++++++ .../start_planner/start_planner_module.hpp | 11 +++++++ .../start_planner_parameters.hpp | 6 ++++ .../scene_module/start_planner/manager.cpp | 29 +++++++++++++++++ .../start_planner/start_planner_module.cpp | 31 +++++++++++++++---- .../path_safety_checker/objects_filtering.hpp | 13 ++++++++ .../path_safety_checker/objects_filtering.cpp | 21 +++++++++++++ 7 files changed, 119 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 4bcb847d1280c..514d61e225ecd 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -138,6 +138,20 @@ backward_path_length: 30.0 forward_path_length: 100.0 + surround_moving_obstacle_check: + search_radius: 10.0 + th_moving_obstacle_velocity: 1.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index e3249d7ffd311..f70effb7cf186 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -137,6 +137,17 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); bool requiresDynamicObjectsCollisionDetection() const; + + /** + * @brief Check if there are no moving objects around within a certain radius. + * + * This function filters the dynamic objects within a certain radius and then filters them by + * their velocity. If there are no moving objects around, it returns true. Otherwise, it returns + * false. + * + * @return True if there are no moving objects around. False otherwise. + */ + bool noMovingObjectsAround() const; bool receivedNewRoute() const; bool isModuleRunning() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 46b72cacac651..cbde2b514366d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -93,6 +93,12 @@ struct StartPlannerParameters utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; utils::path_safety_checker::SafetyCheckParams safety_check_params{}; + // surround moving obstacle check + double search_radius{0.0}; + double th_moving_obstacle_velocity{0.0}; + behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck + surround_moving_obstacles_type_to_check{}; + bool print_debug_info{false}; }; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index e379fcfa03a80..14660396062aa 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -273,6 +273,35 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); } + // surround moving obstacle check + std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; + { + p.search_radius = + node->declare_parameter(surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = node->declare_parameter( + surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); + // ObjectTypesToCheck + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + { + p.surround_moving_obstacles_type_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.surround_moving_obstacles_type_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.surround_moving_obstacles_type_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.surround_moving_obstacles_type_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.surround_moving_obstacles_type_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.surround_moving_obstacles_type_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.surround_moving_obstacles_type_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.surround_moving_obstacles_type_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + } + // debug std::string debug_ns = ns + "debug."; { diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 23a981c4920b4..7bede9192d35f 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -181,6 +181,21 @@ bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; } +bool StartPlannerModule::noMovingObjectsAround() const +{ + auto dynamic_objects = *(planner_data_->dynamic_object); + utils::path_safety_checker::filterObjectsWithinRadius( + dynamic_objects, planner_data_->self_odometry->pose.pose.position, parameters_->search_radius); + utils::path_safety_checker::filterObjectsByClass( + dynamic_objects, parameters_->surround_moving_obstacles_type_to_check); + const auto filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_objects, parameters_->th_moving_obstacle_velocity, false); + if (!filtered_objects.objects.empty()) { + DEBUG_PRINT("Moving objects exist in the safety check area"); + } + return filtered_objects.objects.empty(); +} + bool StartPlannerModule::hasCollisionWithDynamicObjects() const { // TODO(Sugahara): update path, params for predicted path and so on in this function to avoid @@ -273,20 +288,24 @@ bool StartPlannerModule::isStopped() bool StartPlannerModule::isExecutionReady() const { bool is_safe = true; - // Evaluate safety. The situation is not safe if any of the following conditions are met: // 1. pull out path has not been found - // 2. waiting for approval and there is a collision with dynamic objects + // 2. there is a moving objects around ego + // 3. waiting for approval and there is a collision with dynamic objects if (!status_.found_pull_out_path) { is_safe = false; + } else if (isWaitingApproval()) { + if (!noMovingObjectsAround()) { + is_safe = false; + } else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) { + is_safe = false; + } } - if (requiresDynamicObjectsCollisionDetection() && isWaitingApproval()) { - is_safe = !hasCollisionWithDynamicObjects(); + if (!is_safe) { + stop_pose_ = planner_data_->self_odometry->pose.pose; } - if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose; - return is_safe; } diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 018e6bd2a3d6e..f66f425f1b7fa 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -134,6 +134,19 @@ void filterObjectsByPosition( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); +/** + * @brief Filter objects based on their distance from a reference point. + * + * This function filters out objects that are outside a specified radius from a reference point. + * + * @param objects The predicted objects to filter. + * @param reference_point The reference point (e.g., current pose of the ego vehicle). + * @param search_radius The radius within which to retain objects. + */ +void filterObjectsWithinRadius( + PredictedObjects & objects, const geometry_msgs::msg::Point & reference_point, + const double search_radius); + /** * @brief Filters the provided objects based on their classification. * diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 84c02b3b325de..188c50993e4ae 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -44,6 +44,15 @@ bool position_filter( return (backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance); } + +bool is_within_circle( + const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point, + const double search_radius) +{ + const double dist = + std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y); + return dist < search_radius; +} } // namespace behavior_path_planner::utils::path_safety_checker::filter namespace behavior_path_planner::utils::path_safety_checker @@ -128,6 +137,18 @@ void filterObjectsByPosition( filterObjects(objects, filter); } +void filterObjectsWithinRadius( + PredictedObjects & objects, const geometry_msgs::msg::Point & reference_point, + const double search_radius) +{ + const auto filter = [&](const auto & object) { + return filter::is_within_circle( + object.kinematics.initial_pose_with_covariance.pose.position, reference_point, search_radius); + }; + + filterObjects(objects, filter); +} + void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) {