fix(avoidance): update filtering logic for non-vehicle (autowarefound…

…ation#6206)

Signed-off-by: satoshi-ota <[email protected]>

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

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;