feat(avoidance): enhance target filtering logic for non-vehicle object

Signed-off-by: satoshi-ota <[email protected]>
satoshi-ota · Nov 21, 2023 · dfe8ac3 · dfe8ac3
1 parent d12f463
commit dfe8ac3
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 3 deletions.
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
@@ -2775,6 +2775,7 @@ void AvoidanceModule::updateDebugMarker(
     addObjects(data.other_objects, std::string("TooNearToGoal"));
     addObjects(data.other_objects, std::string("ParallelToEgoLane"));
     addObjects(data.other_objects, std::string("BehindReturnPoint"));
+    addObjects(data.other_objects, std::string("UnstableObject"));
   }
 
   // shift line pre-process

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -248,9 +248,9 @@ bool isVehicleTypeObject(const ObjectData & object)
 {
   const auto object_type = utils::getHighestProbLabel(object.object.classification);
 
-  if (object_type == ObjectClassification::UNKNOWN) {
-    return false;
-  }
+  // if (object_type == ObjectClassification::UNKNOWN) {
+  //   return false;
+  // }
 
   if (object_type == ObjectClassification::PEDESTRIAN) {
     return false;
@@ -263,6 +263,12 @@ bool isVehicleTypeObject(const ObjectData & object)
   return true;
 }
 
+bool isUnknownTypeObject(const ObjectData & object)
+{
+  const auto object_type = utils::getHighestProbLabel(object.object.classification);
+  return object_type == ObjectClassification::UNKNOWN;
+}
+
 bool isWithinCrosswalk(
   const ObjectData & object,
   const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
@@ -561,6 +567,12 @@ bool isSatisfiedWithNonVehicleCondition(
     return false;
   }
 
+  // Object is on center line -> ignore.
+  if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) {
+    object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE;
+    return false;
+  }
+
   return true;
 }
 
@@ -1557,6 +1569,14 @@ void filterTargetObjects(
     o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters);
     o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);
 
+    if (filtering_utils::isUnknownTypeObject(o)) {
+      if (o.stop_time < 3.0) {
+        o.reason = "UnstableObject";
+        data.other_objects.push_back(o);
+        continue;
+      }
+    }
+
     if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) {
       data.other_objects.push_back(o);
       continue;