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 a853661891e75..ee95d814b9306 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -743,6 +743,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 d8860c86ae112..c75ed08b7a5ce 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() @@ -293,8 +305,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; @@ -329,7 +341,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( @@ -358,13 +370,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