fix(static_obstacle_avoidance): fix issues in target filtiering logic (…

…autowarefoundation#7954) * fix: unknown filtering flow Signed-off-by: satoshi-ota <[email protected]> * fix: relax target filtering logic for object which is in freespace Signed-off-by: satoshi-ota <[email protected]> * docs: update flowchart Signed-off-by: satoshi-ota <[email protected]> * fix: check stopped time in freespace Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
Ariiees · Jul 22, 2024 · 3b984e7 · 3b984e7
1 parent 356ece2
commit 3b984e7
Show file tree

Hide file tree

Showing 7 changed files with 103 additions and 5 deletions.
diff --git a/..._path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/..._path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md
@@ -401,6 +401,15 @@ else (\n no)
 #00FFB1 :IGNORE;
 stop
 endif
+if(Is UNKNOWN objects?) then (yes)
+if(isSatisfiedWithUnknownTypeObjectCodition) then (yes)
+#FF006C :AVOID;
+stop
+else (\n no)
+#00FFB1 :IGNORE;
+stop
+endif
+else (\n no)
 if(Is vehicle type?) then (yes)
 if(isSatisfiedWithVehicleCodition()) then (yes)
 else (\n no)
@@ -618,6 +627,18 @@ title Filter vehicle which is obviously an avoidance target
 start
 
 partition isObviousAvoidanceTarget() {
+if(Is object within freespace?) then (yes)
+if(Is object on ego lane?) then (no)
+if(Is object stopping longer than threshold?) then (yes)
+#FF006C :return true;
+stop
+else (\n no)
+endif
+else (\n yes)
+endif
+else (\n no)
+endif
+
 if(Is object within intersection?) then (yes)
 else (\n no)
 if(Is object parallel to ego lane? AND Is object a parked vehicle?) then (yes)

diff --git a/...ehavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/...ehavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
@@ -160,6 +160,10 @@
         intersection:
           yaw_deviation: 0.349                          # [rad] (default 20.0deg)
 
+        freespace:
+          condition:
+            th_stopped_time: 5.0                        # [-]
+
       # For safety check
       safety_check:
         # safety check target type

diff --git a/...e_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/...e_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
@@ -191,6 +191,9 @@ struct AvoidanceParameters
   // minimum road shoulder width. maybe 0.5 [m]
   double object_check_min_road_shoulder_width{0.0};
 
+  // time threshold for vehicle in freespace.
+  double freespace_condition_th_stopped_time{0.0};
+
   // force avoidance
   std::vector<std::string> wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"};
   double wait_and_see_th_closest_distance{0.0};

diff --git a/...dule/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/...dule/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
@@ -154,6 +154,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.behind_distance");
   }
 
+  {
+    const std::string ns = "avoidance.target_filtering.freespace.";
+    p.freespace_condition_th_stopped_time =
+      getOrDeclareParameter<double>(*node, ns + "condition.th_stopped_time");
+  }
+
   {
     const std::string ns = "avoidance.target_filtering.detection_area.";
     p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");

diff --git a/...havior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/...havior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
@@ -812,6 +812,25 @@
               },
               "required": ["yaw_deviation"],
               "additionalProperties": false
+            },
+            "freespace": {
+              "type": "object",
+              "properties": {
+                "condition": {
+                  "type": "object",
+                  "properties": {
+                    "th_stopped_time": {
+                      "type": "number",
+                      "description": "This module delays avoidance maneuver to see vehicle behavior in freespace.",
+                      "default": 5.0
+                    }
+                  },
+                  "required": ["th_stopped_time"],
+                  "additionalProperties": false
+                }
+              },
+              "required": ["condition"],
+              "additionalProperties": false
             }
           },
           "required": [
@@ -823,7 +842,8 @@
             "merging_vehicle",
             "parked_vehicle",
             "avoidance_for_ambiguous_vehicle",
-            "intersection"
+            "intersection",
+            "freespace"
           ],
           "additionalProperties": false
         },

diff --git a/...vior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/...vior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
@@ -150,6 +150,12 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
       p->object_ignore_section_crosswalk_behind_distance);
   }
 
+  {
+    const std::string ns = "avoidance.target_filtering.freespace.";
+    updateParam<double>(
+      parameters, ns + "condition.th_stopped_time", p->freespace_condition_th_stopped_time);
+  }
+
   {
     const std::string ns = "avoidance.target_filtering.intersection.";
     updateParam<double>(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation);

diff --git a/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
@@ -339,6 +339,32 @@ bool isWithinIntersection(
     lanelet::utils::to2D(polygon.basicPolygon()));
 }
 
+bool isWithinFreespace(
+  const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
+{
+  auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr());
+  if (polygons.empty()) {
+    return false;
+  }
+
+  std::sort(polygons.begin(), polygons.end(), [&object](const auto & a, const auto & b) {
+    const double a_distance = boost::geometry::distance(
+      lanelet::utils::to2D(a).basicPolygon(),
+      lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+        .basicPoint());
+    const double b_distance = boost::geometry::distance(
+      lanelet::utils::to2D(b).basicPolygon(),
+      lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+        .basicPoint());
+    return a_distance < b_distance;
+  });
+
+  return boost::geometry::within(
+    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+      .basicPoint(),
+    lanelet::utils::to2D(polygons.front().basicPolygon()));
+}
+
 bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
 {
   if (boost::geometry::within(
@@ -708,6 +734,14 @@ bool isObviousAvoidanceTarget(
   [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data,
   [[maybe_unused]] const std::shared_ptr<AvoidanceParameters> & parameters)
 {
+  if (isWithinFreespace(object, planner_data->route_handler)) {
+    if (!object.is_on_ego_lane) {
+      if (object.stop_time > parameters->freespace_condition_th_stopped_time) {
+        return true;
+      }
+    }
+  }
+
   if (!object.is_within_intersection) {
     if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) {
       RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle.");
@@ -1842,17 +1876,19 @@ void filterTargetObjects(
       utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance(o, data.reference_path);
     o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
 
-    // TODO(Satoshi Ota) parametrize stop time threshold if need.
-    constexpr double STOP_TIME_THRESHOLD = 3.0;  // [s]
     if (filtering_utils::isUnknownTypeObject(o)) {
+      // TARGET: UNKNOWN
+
+      // TODO(Satoshi Ota) parametrize stop time threshold if need.
+      constexpr double STOP_TIME_THRESHOLD = 3.0;  // [s]
       if (o.stop_time < STOP_TIME_THRESHOLD) {
         o.info = ObjectInfo::UNSTABLE_OBJECT;
         data.other_objects.push_back(o);
         continue;
       }
-    }
+    } else if (filtering_utils::isVehicleTypeObject(o)) {
+      // TARGET: CAR, TRUCK, BUS, TRAILER, MOTORCYCLE
 
-    if (filtering_utils::isVehicleTypeObject(o)) {
       o.is_within_intersection =
         filtering_utils::isWithinIntersection(o, planner_data->route_handler);
       o.is_parked =
@@ -1869,6 +1905,8 @@ void filterTargetObjects(
         continue;
       }
     } else {
+      // TARGET: PEDESTRIAN, BICYCLE
+
       o.is_within_intersection =
         filtering_utils::isWithinIntersection(o, planner_data->route_handler);
       o.is_parked = false;