diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index f088b9228d964..7c55c62bd8cc1 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -557,6 +557,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("TooNearToGoal")); addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("UnstableObject")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 5fe47715bff08..b5d351cbc48a8 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -368,13 +368,15 @@ bool isSafetyCheckTargetObjectType( return parameters->object_parameters.at(object_type).is_safety_check_target; } -bool isVehicleTypeObject(const ObjectData & object) +bool isUnknownTypeObject(const ObjectData & object) { const auto object_type = utils::getHighestProbLabel(object.object.classification); + return object_type == ObjectClassification::UNKNOWN; +} - if (object_type == ObjectClassification::UNKNOWN) { - return false; - } +bool isVehicleTypeObject(const ObjectData & object) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); if (object_type == ObjectClassification::PEDESTRIAN) { return false; @@ -723,6 +725,15 @@ bool isSatisfiedWithNonVehicleCondition( return false; } + // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + return true; } @@ -1626,6 +1637,16 @@ void filterTargetObjects( o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] + if (filtering_utils::isUnknownTypeObject(o)) { + if (o.stop_time < STOP_TIME_THRESHOLD) { + o.reason = "UnstableObject"; + data.other_objects.push_back(o); + continue; + } + } + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue;