diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 5dde792453cbe..fa96be366e339 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -34,6 +34,8 @@ bool isTargetObjectType( double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); +bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length); + bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length); size_t findPathIndexFromArclength( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index a8e09dae9e703..9e56ba46093dd 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -3323,6 +3323,7 @@ void AvoidanceModule::setDebugData( add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray( diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index ce601330727af..36160ac23dab9 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -96,6 +96,32 @@ double calcShiftLength( return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0; } +bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length) +{ + /** + * ^ + * | + * --+----x-------------------------------x---> + * | x x + * | ==obj== + */ + if (is_object_on_right && shift_length < 0.0) { + return false; + } + + /** + * ^ ==obj== + * | x x + * --+----x-------------------------------x---> + * | + */ + if (!is_object_on_right && shift_length > 0.0) { + return false; + } + + return true; +} + bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length) { return (is_object_on_right == std::signbit(shift_length)); @@ -682,6 +708,15 @@ void filterTargetObjects( return std::min(max_allowable_lateral_distance, max_avoid_margin); }(); + if (!!avoid_margin) { + const auto shift_length = calcShiftLength(isOnRight(o), o.overhang_dist, avoid_margin.get()); + if (!isShiftNecessary(isOnRight(o), shift_length)) { + o.reason = "NotNeedAvoidance"; + data.other_objects.push_back(o); + continue; + } + } + // force avoidance for stopped vehicle { const auto to_traffic_light =