diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index fe30397683494..127b70fbf7bcb 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -16,6 +16,7 @@ motorcycle: true pedestrian: false + max_obstacle_vel: 100.0 # [m/s] min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index b00818f2f79d1..ab8e3cfd70dff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -56,6 +56,7 @@ struct DynamicAvoidanceParameters bool avoid_bicycle{false}; bool avoid_motorcycle{false}; bool avoid_pedestrian{false}; + double max_obstacle_vel{0.0}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; int successive_num_to_exit_dynamic_avoidance_condition{0}; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 181b9fbfd5260..24c074bcb629b 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -411,10 +411,12 @@ void DynamicAvoidanceModule::updateTargetObjects() continue; } - // 1.b. check if velocity is large enough + // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); - if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { + if ( + std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || + parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { continue; } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 616194ea66ac7..6f624a9f6f017 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -47,6 +47,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.avoid_bicycle = node->declare_parameter(ns + "bicycle"); p.avoid_motorcycle = node->declare_parameter(ns + "motorcycle"); p.avoid_pedestrian = node->declare_parameter(ns + "pedestrian"); + p.max_obstacle_vel = node->declare_parameter(ns + "max_obstacle_vel"); p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); @@ -136,6 +137,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "motorcycle", p->avoid_motorcycle); updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); + updateParam(parameters, ns + "max_obstacle_vel", p->max_obstacle_vel); updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); updateParam(