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

* feat(dynamic_avoidance): add max obstacle vel Signed-off-by: Takayuki Murooka <[email protected]> * update config Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Sep 26, 2023 · 2f4d7c3 · 2f4d7c3
1 parent bacf767
commit 2f4d7c3
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 2 deletions.
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
@@ -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/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/...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/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -383,10 +383,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
@@ -45,6 +45,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>(