From 8227c227b45dea7f60d2727022b7c1433b414bcb Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 23 Jun 2023 16:48:43 +0900 Subject: [PATCH] feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching (#4048) * feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance.param.yaml | 10 +++--- .../dynamic_avoidance_module.hpp | 23 +++++++++++- .../src/behavior_path_planner_node.cpp | 2 ++ .../dynamic_avoidance_module.cpp | 35 +++++++++++++++---- .../dynamic_avoidance/manager.cpp | 4 +++ 5 files changed, 63 insertions(+), 11 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 index a4ff684839321..0b8f35bac3131 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 @@ -12,21 +12,23 @@ motorcycle: true pedestrian: false - min_obstacle_vel: 1.0 # [m/s] + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 drivable_area_generation: - lat_offset_from_obstacle: 1.3 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] # for same directional object overtaking_object: max_time_to_collision: 3.0 # [s] start_duration_to_avoid: 4.0 # [s] - end_duration_to_avoid: 5.0 # [s] + end_duration_to_avoid: 8.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 9.0 # [s] + start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] 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 217104eb7c938..799c0aad6e307 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 @@ -47,6 +47,7 @@ struct DynamicAvoidanceParameters bool avoid_motorcycle{false}; bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; + int successive_num_to_entry_dynamic_avoidance_condition{0}; // drivable area generation double lat_offset_from_obstacle{0.0}; @@ -87,6 +88,24 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool is_left; }; + struct DynamicAvoidanceObjectCandidate + { + DynamicAvoidanceObject object; + int alive_counter; + + static std::optional getObjectFromUuid( + const std::vector & objects, const std::string & target_uuid) + { + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; + } + }; #ifdef USE_OLD_ARCHITECTURE DynamicAvoidanceModule( @@ -119,12 +138,14 @@ class DynamicAvoidanceModule : public SceneModuleInterface private: bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjects() const; + std::vector calcTargetObjectsCandidate() const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; std::optional calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; + std::vector + prev_target_objects_candidate_; std::vector target_objects_; // std::vector prev_target_objects_; std::shared_ptr parameters_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index e26e9874ede39..03ce2fb6e4992 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -721,6 +721,8 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() p.avoid_motorcycle = declare_parameter(ns + "motorcycle"); p.avoid_pedestrian = declare_parameter(ns + "pedestrian"); p.min_obstacle_vel = declare_parameter(ns + "min_obstacle_vel"); + p.successive_num_to_entry_dynamic_avoidance_condition = + declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); } { // drivable_area_generation 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 8a444d309dedd..33431632b03f8 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 @@ -192,7 +192,19 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { - target_objects_ = calcTargetObjects(); + // calculate target objects candidate + const auto target_objects_candidate = calcTargetObjectsCandidate(); + prev_target_objects_candidate_ = target_objects_candidate; + + // calculate target objects considering flickering suppress + target_objects_.clear(); + for (const auto & target_object_candidate : target_objects_candidate) { + if ( + parameters_->successive_num_to_entry_dynamic_avoidance_condition <= + target_object_candidate.alive_counter) { + target_objects_.push_back(target_object_candidate.object); + } + } } ModuleStatus DynamicAvoidanceModule::updateState() @@ -295,8 +307,8 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const return false; } -std::vector -DynamicAvoidanceModule::calcTargetObjects() const +std::vector +DynamicAvoidanceModule::calcTargetObjectsCandidate() const { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; @@ -331,7 +343,7 @@ DynamicAvoidanceModule::calcTargetObjects() const // 4. check if object will cut into the ego lane. // NOTE: The oncoming object will be ignored. constexpr double epsilon_path_lat_diff = 0.3; - std::vector output_objects; + std::vector output_objects_candidate; for (const bool is_left : {true, false}) { for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) { const auto reliable_predicted_path = std::max_element( @@ -360,13 +372,24 @@ DynamicAvoidanceModule::calcTargetObjects() const continue; } + // get previous object if it exists + const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( + prev_target_objects_candidate_, object.uuid); + const int alive_counter = + prev_target_object_candidate + ? std::min( + parameters_->successive_num_to_entry_dynamic_avoidance_condition, + prev_target_object_candidate->alive_counter + 1) + : 0; + auto target_object = object; target_object.is_left = is_left; - output_objects.push_back(target_object); + output_objects_candidate.push_back( + DynamicAvoidanceObjectCandidate{target_object, alive_counter}); } } - return output_objects; + return output_objects_candidate; } std::pair DynamicAvoidanceModule::getAdjacentLanes( 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 d3212a7c0b1d4..b9ca4ffa964f7 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 @@ -49,6 +49,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); + + updateParam( + parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", + p->successive_num_to_entry_dynamic_avoidance_condition); } { // drivable_area_generation