feat(avoidance): suppress unnecessary avoidance path

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

@@ -213,7 +213,6 @@ In order to prevent chattering of recognition results, once an obstacle is targe
 
 ```plantuml
 @startuml
-skinparam monochrome true
 skinparam defaultTextAlignment center
 skinparam noteTextAlignment left
 
@@ -245,14 +244,13 @@ stop
 endif
 endif
 
-:return true;
+#FF006C :return true;
 stop
 @enduml
 ```
 
 ```plantuml
 @startuml
-skinparam monochrome true
 skinparam defaultTextAlignment center
 skinparam noteTextAlignment left
 
@@ -280,7 +278,7 @@ else (\n no)
 :return false;
 stop
 endif
-:return true;
+#FF006C :return true;
 stop
 }
 
@@ -289,7 +287,6 @@ stop
 
 ```plantuml
 @startuml
-skinparam monochrome true
 skinparam defaultTextAlignment center
 skinparam noteTextAlignment left
 
@@ -298,36 +295,44 @@ start
 
 partition isSatisfiedWithVehicleCodition() {
 if(object is force avoidance target ?) then (yes)
-else (\n 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 nearer lane centerline than threshold ?) then (no)
+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
-else (\n yes)
 endif
 else (\n no)
-if(object is shifting right or left side road shoulder more than threshold ?) then (no)
-:return false;
+if(object is in intersection ?) then (no)
+#FF006C :return true;
 stop
 else (\n yes)
-if(object is in intersection ?) then (no)
-:return false;
+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
-:return true;
-stop
 }
 
 @enduml
 ```
 
 ```plantuml
 @startuml
-skinparam monochrome true
 skinparam defaultTextAlignment center
 skinparam noteTextAlignment left
 
@@ -336,11 +341,11 @@ start
 
 partition isSatisfiedWithNonVehicleCodition() {
 if(object is nearer crosswalk than threshold ?) then (yes)
-:ignore the object;
+:return false;
 stop
 else (\n no)
 endif
-:return true;
+#FF006C :return true;
 stop
 }
 

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");
   }