feat(avoidance): suppress unnecessary avoidance path (autowarefoundat…

…ion#5387)

* docs(avoidance): update flowchart

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

* feat(avoidance): suppress unnecessary avoidance path

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

---------

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

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -146,6 +146,10 @@
               front_distance: 30.0                       # [m]
               behind_distance: 30.0                      # [m]
 
+        # params for filtering objects that are in intersection
+        intersection:
+          yaw_deviation: 0.175                           # [rad] (default: 10.0deg)
+
       # For safety check
       safety_check:
         # safety check configuration

planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md

@@ -209,6 +209,149 @@ The closer the object is to the shoulder, the larger the value of $ratio$ (theor
 
 In order to prevent chattering of recognition results, once an obstacle is targeted, it is hold for a while even if it disappears. This is effective when recognition is unstable. However, since it will result in over-detection (increase a number of false-positive), it is necessary to adjust parameters according to the recognition accuracy (if `object_last_seen_threshold = 0.0`, the recognition result is 100% trusted).
 
+### Flowchart
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam noteTextAlignment left
+
+title object filtering flowchart
+start
+
+if(object is satisfied with common target condition ?) then (yes)
+if(vehicle can pass by the object without avoidance ?) then (yes)
+:return false;
+stop
+else (\n no)
+endif
+else (\n no)
+:return false;
+stop
+endif
+
+if(object is vehicle type ?) then (yes)
+if(object is satisfied with vehicle type target condition ?) then (yes)
+else (\n no)
+:return false;
+stop
+endif
+else (\n no)
+if(object is satisfied with non-vehicle type target condition ?) then (yes)
+else (\n no)
+:return false;
+stop
+endif
+endif
+
+#FF006C :return true;
+stop
+@enduml
+```
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam noteTextAlignment left
+
+title filtering flow for all types object
+start
+
+partition isSatisfiedWithCommonCondition() {
+if(object is avoidance target type ?) then (yes)
+if(object is moving more than threshold time ?) then (yes)
+:return false;
+stop
+else (\n no)
+if(object is farther than forward distance threshold ?) then (yes)
+:return false;
+stop
+else (\n no)
+If(object is farther than backward distance threshold ?) then (yes)
+:return false;
+stop
+else (\n no)
+endif
+endif
+endif
+else (\n no)
+:return false;
+stop
+endif
+#FF006C :return true;
+stop
+}
+
+@enduml
+```
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam noteTextAlignment left
+
+title filtering flow for vehicle type objects
+start
+
+partition isSatisfiedWithVehicleCodition() {
+if(object is force avoidance target ?) then (yes)
+#FF006C :return true;
+stop
+else (\n no)
+if(object is nearer lane centerline than threshold ?) then (yes)
+:return false;
+stop
+else (\n no)
+if(object is on same lane for ego ?) then (yes)
+if(object is shifting right or left side road shoulder more than threshold ?) then (yes)
+#FF006C :return true;
+stop
+else (\n no)
+:return false;
+stop
+endif
+else (\n no)
+if(object is in intersection ?) then (no)
+#FF006C :return true;
+stop
+else (\n yes)
+if(object pose is paralell to ego lane ?) then (no)
+#FF006C :return true;
+stop
+else (\n yes)
+:return false;
+stop
+endif
+endif
+endif
+endif
+endif
+}
+
+@enduml
+```
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam noteTextAlignment left
+
+title filtering flow for non-vehicle type objects
+start
+
+partition isSatisfiedWithNonVehicleCodition() {
+if(object is nearer crosswalk than threshold ?) then (yes)
+:return false;
+stop
+else (\n no)
+endif
+#FF006C :return true;
+stop
+}
+
+@enduml
+```
+
 ## Overview of algorithm for avoidance path generation
 
 ### How to prevent shift line chattering that is caused by perception noise

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -159,6 +159,7 @@ struct AvoidanceParameters
   double object_check_min_forward_distance{0.0};
   double object_check_max_forward_distance{0.0};
   double object_check_backward_distance{0.0};
+  double object_check_yaw_deviation{0.0};
 
   // if the distance between object and goal position is less than this parameter, the module ignore
   // the object.
@@ -394,8 +395,7 @@ struct ObjectData  // avoidance target
   std::string reason{""};
 
   // lateral avoid margin
-  // NOTE: If margin is less than the minimum margin threshold, boost::none will be set.
-  boost::optional<double> avoid_margin{boost::none};
+  std::optional<double> avoid_margin{std::nullopt};
 };
 using ObjectDataArray = std::vector<ObjectData>;
 
@@ -482,6 +482,7 @@ struct AvoidancePlanningData
 
   // current driving lanelet
   lanelet::ConstLanelets current_lanelets;
+  lanelet::ConstLanelets extend_lanelets;
 
   // output path
   ShiftedPath candidate_path;

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -33,18 +33,6 @@ using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygo
 
 bool isOnRight(const ObjectData & obj);
 
-bool isVehicleTypeObject(const ObjectData & object);
-
-bool isWithinCrosswalk(
-  const ObjectData & object,
-  const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
-
-bool isWithinIntersection(
-  const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler);
-
-bool isTargetObjectType(
-  const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);
-
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
 
@@ -106,6 +94,10 @@ lanelet::ConstLanelets getTargetLanelets(
 lanelet::ConstLanelets getCurrentLanesFromPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
 
+lanelet::ConstLanelets getExtendLanes(
+  const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
+  const std::shared_ptr<const PlannerData> & planner_data);
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out);
@@ -139,6 +131,11 @@ void filterTargetObjects(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+double getRoadShoulderDistance(
+  ObjectData & object, const AvoidancePlanningData & data,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
+
 double extendToRoadShoulderDistanceWithPolygon(
   const std::shared_ptr<route_handler::RouteHandler> & rh,
   const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance,

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -265,6 +265,9 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
   data.current_lanelets = utils::avoidance::getCurrentLanesFromPath(
     *getPreviousModuleOutput().reference_path, planner_data_);
 
+  data.extend_lanelets =
+    utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
+
   // expand drivable lanes
   std::for_each(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
@@ -1058,7 +1061,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
 
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
-    if (!o.avoid_margin) {
+    if (!o.avoid_margin.has_value()) {
       o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
       if (o.avoid_required && is_forward_object(o)) {
         break;
@@ -1069,7 +1072,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
 
     const auto is_object_on_right = utils::avoidance::isOnRight(o);
     const auto desire_shift_length =
-      helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get());
+      helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.value());
     if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) {
       o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT;
       if (o.avoid_required && is_forward_object(o)) {
@@ -2813,6 +2816,7 @@ void AvoidanceModule::updateDebugMarker(
     addObjects(data.other_objects, std::string("NotNeedAvoidance"));
     addObjects(data.other_objects, std::string("LessThanExecutionThreshold"));
     addObjects(data.other_objects, std::string("TooNearToGoal"));
+    addObjects(data.other_objects, std::string("ParallelToEgoLane"));
   }
 
   // shift line pre-process

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -114,6 +114,8 @@ AvoidanceModuleManager::AvoidanceModuleManager(
       getOrDeclareParameter<double>(*node, ns + "object_check_shiftable_ratio");
     p.object_check_min_road_shoulder_width =
       getOrDeclareParameter<double>(*node, ns + "object_check_min_road_shoulder_width");
+    p.object_check_yaw_deviation =
+      getOrDeclareParameter<double>(*node, ns + "intersection.yaw_deviation");
     p.object_last_seen_threshold =
       getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
   }