From 5d4b6ac2e502841198c5f21c0352d62531f107c9 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 7 Oct 2024 09:25:53 +0900 Subject: [PATCH] fix(behavior_path_planner_common): swap boolean for filterObjectsByVelocity (#9036) fix filter object by velocity Signed-off-by: Go Sakayori Signed-off-by: prakash-kannaiah --- .../src/goal_planner_module.cpp | 2 +- .../utils/path_safety_checker/objects_filtering.hpp | 2 +- .../src/utils/path_safety_checker/objects_filtering.cpp | 4 ++-- .../src/start_planner_module.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 710700c1f6dc1..d10de53d5fa2f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2162,7 +2162,7 @@ static std::vector filterOb const auto & target_object_types = params->object_types_to_check; PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( - *objects, ignore_object_velocity_threshold, false); + *objects, ignore_object_velocity_threshold, true); utils::path_safety_checker::filterObjectsByClass(filtered_objects, target_object_types); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index f73f989174b54..3d7532ba5b244 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -113,7 +113,7 @@ PredictedObjects filterObjects( */ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, const double velocity_threshold, - const bool remove_above_threshold = true); + const bool remove_above_threshold = false); /** * @brief Helper function to filter objects based on their velocity. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 25b307ab2cc4d..20221fff82175 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -118,7 +118,7 @@ PredictedObjects filterObjects( const ObjectTypesToCheck & target_object_types = params->object_types_to_check; PredictedObjects filtered_objects = - filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); + filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, true); filterObjectsByClass(filtered_objects, target_object_types); @@ -136,7 +136,7 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, const double velocity_threshold, const bool remove_above_threshold) { - if (remove_above_threshold) { + if (!remove_above_threshold) { return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); } return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); 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 9df0791ed9375..f44bf204bd2d7 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 @@ -329,7 +329,7 @@ bool StartPlannerModule::noMovingObjectsAround() const 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); + dynamic_objects, parameters_->th_moving_obstacle_velocity, true); if (!filtered_objects.objects.empty()) { DEBUG_PRINT("Moving objects exist in the safety check area"); }