From ca643cc7d28c138813358b8023d97fb1521b2949 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 1 Jul 2023 01:10:27 +0900 Subject: [PATCH] fix(dynamic_avoidance): ignore objects on LC target lane Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance/dynamic_avoidance_module.hpp | 1 + .../dynamic_avoidance/dynamic_avoidance_module.cpp | 12 +++++++++++- .../src/scene_module/dynamic_avoidance/manager.cpp | 4 ++++ 3 files changed, 16 insertions(+), 1 deletion(-) 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 32735b72913fa..6d2b66ff025c1 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 @@ -48,6 +48,7 @@ struct DynamicAvoidanceParameters bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; + double min_obj_lat_offset_to_ego_path{0.0}; // drivable area generation double lat_offset_from_obstacle{0.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 262fbb55301ba..11198a49360fb 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 @@ -325,7 +325,8 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes); const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes); - // 4. check if object will cut into the ego lane or cut out to the next lane. + // 4. check if object will not cut into the ego lane or cut out to the next lane, + // or close to the ego's path considering ego's lane change. // NOTE: The oncoming object will be ignored. constexpr double epsilon_path_lat_diff = 0.3; std::vector output_objects_candidate; @@ -380,6 +381,15 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const continue; } + // Ignore object that is close to the ego's path. + const double lat_offset_to_path = + motion_utils::calcLateralOffset(prev_module_path->points, object.pose.position); + if ( + std::abs(lat_offset_to_path) < planner_data_->parameters.vehicle_width / 2.0 + + parameters_->min_obj_lat_offset_to_ego_path) { + continue; + } + // get previous object if it exists const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( prev_target_objects_candidate_, object.uuid); 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 08188343f1d64..53e93a76e7eed 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 @@ -42,6 +42,8 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( 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"); + p.min_obj_lat_offset_to_ego_path = + node->declare_parameter(ns + "min_obj_lat_offset_to_ego_path"); } { // drivable_area_generation @@ -92,6 +94,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", p->successive_num_to_entry_dynamic_avoidance_condition); + updateParam( + parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path); } { // drivable_area_generation