feat(dynamic_avoidance): add max obstacle vel (#5142)

* feat(dynamic_avoidance): add max obstacle vel

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

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

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};

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;
     }
 

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp

@@ -47,6 +47,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
     p.avoid_bicycle = node->declare_parameter<bool>(ns + "bicycle");
     p.avoid_motorcycle = node->declare_parameter<bool>(ns + "motorcycle");
     p.avoid_pedestrian = node->declare_parameter<bool>(ns + "pedestrian");
+    p.max_obstacle_vel = node->declare_parameter<double>(ns + "max_obstacle_vel");
     p.min_obstacle_vel = node->declare_parameter<double>(ns + "min_obstacle_vel");
     p.successive_num_to_entry_dynamic_avoidance_condition =
       node->declare_parameter<int>(ns + "successive_num_to_entry_dynamic_avoidance_condition");
@@ -136,6 +137,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
     updateParam<bool>(parameters, ns + "motorcycle", p->avoid_motorcycle);
     updateParam<bool>(parameters, ns + "pedestrian", p->avoid_pedestrian);
 
+    updateParam<double>(parameters, ns + "max_obstacle_vel", p->max_obstacle_vel);
     updateParam<double>(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel);
 
     updateParam<int>(