From 356a320c5377b4284d6e4fd742b0332caf37b24b Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 24 Jun 2024 17:35:00 +0900 Subject: [PATCH] add param to customize yaw th Signed-off-by: Daniel Sanchez --- .../path_safety_checker/safety_check.hpp | 10 +++++++--- .../path_safety_checker/safety_check.cpp | 20 ++++++++++++++----- .../config/start_planner.param.yaml | 1 + .../data_structs.hpp | 1 + .../src/manager.cpp | 5 ++++- .../src/start_planner_module.cpp | 7 +++++-- 6 files changed, 33 insertions(+), 11 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 3f1094dfd70b6..e710bdbe85986 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 @@ -26,6 +26,7 @@ #include #include +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -98,7 +99,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor); + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th = M_PI); /** * @brief Iterate the points in the ego and target's predicted path and @@ -120,7 +122,8 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug); + const double hysteresis_factor, CollisionCheckDebug & debug, + const double yaw_difference_th = M_PI); /** * @brief Iterate the points in the ego and target's predicted path and @@ -142,7 +145,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug); + const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug, + const double yaw_difference_th = M_PI); bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 7bb8c13aa65fb..3b151d955ca91 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -26,6 +26,9 @@ #include #include +#include + +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -482,7 +485,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor) + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th) { // Check for collisions with each predicted path of the object const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { @@ -495,7 +499,7 @@ bool checkSafetyWithRSS( obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { const bool has_collision = !utils::path_safety_checker::checkCollision( planned_path, ego_predicted_path, object, obj_path, parameters, rss_params, - hysteresis_factor, current_debug_data.second); + hysteresis_factor, current_debug_data.second, yaw_difference_th); utils::path_safety_checker::updateCollisionCheckDebugMap( debug_map, current_debug_data, !has_collision); @@ -559,11 +563,12 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug) + const double hysteresis_factor, CollisionCheckDebug & debug, const double yaw_difference_th) { const auto collided_polygons = getCollidedPolygons( planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, - rss_parameters, hysteresis_factor, std::numeric_limits::max(), debug); + rss_parameters, hysteresis_factor, std::numeric_limits::max(), debug, + yaw_difference_th); return collided_polygons.empty(); } @@ -573,7 +578,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug) + double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug, + const double yaw_difference_th) { { debug.ego_predicted_path = predicted_ego_path; @@ -604,6 +610,10 @@ std::vector getCollidedPolygons( const auto & ego_polygon = interpolated_data->poly; const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit); + const double ego_yaw = tf2::getYaw(ego_pose.orientation); + const double object_yaw = tf2::getYaw(obj_pose.orientation); + if (std::abs(ego_yaw - object_yaw) > yaw_difference_th) continue; + // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index ce7ead87b54c7..d9514440308e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,6 +5,7 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 + collision_check_yaw_diff_threshold: 1.57 collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 extra_width_margin_for_rear_obstacle: 0.5 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 6eea3e3576732..f7ad52eb79263 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -100,6 +100,7 @@ struct StartPlannerParameters double prepare_time_before_start{0.0}; double th_distance_to_middle_of_the_road{0.0}; double extra_width_margin_for_rear_obstacle{0.0}; + double collision_check_yaw_diff_threshold{0.0}; std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 9451bb3870648..1474f03ff3c9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -41,6 +41,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); p.extra_width_margin_for_rear_obstacle = node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_yaw_diff_threshold = + node->declare_parameter(ns + "collision_check_yaw_diff_threshold"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_margin_from_front_object = @@ -367,7 +369,8 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "extra_width_margin_for_rear_obstacle", p->extra_width_margin_for_rear_obstacle); - + updateParam( + parameters, ns + "collision_check_yaw_diff_threshold", p->collision_check_yaw_diff_threshold); updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9400f3ff40286..368d6f8bbb81d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -34,6 +34,7 @@ #include #include +#include #include #include #include @@ -104,7 +105,7 @@ void StartPlannerModule::onFreespacePlannerTimer() std::optional current_status_opt{std::nullopt}; std::optional parameters_opt{std::nullopt}; std::optional pull_out_status_opt{std::nullopt}; - bool is_stopped; + bool is_stopped{false}; // making a local copy of thread sensitive data { @@ -1422,10 +1423,12 @@ bool StartPlannerModule::isSafePath() const merged_target_object.insert( merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), target_objects_on_lane.on_shoulder_lane.end()); + return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, - objects_filtering_params_->use_all_predicted_path, hysteresis_factor); + objects_filtering_params_->use_all_predicted_path, hysteresis_factor, + parameters_->collision_check_yaw_diff_threshold); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const