feat(avoidance): suppress unnecessary avoidance path

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Oct 30, 2023 · ec94924 · ec94924
1 parent 9f3ac65
commit ec94924
Show file tree

Hide file tree

Showing 6 changed files with 173 additions and 136 deletions.
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/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

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -151,6 +151,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.

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
@@ -45,6 +45,10 @@ bool isWithinIntersection(
 bool isTargetObjectType(
   const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);
 
+bool isObjectOnRoadShoulder(
+  ObjectData & object, std::shared_ptr<RouteHandler> & route_handler,
+  std::shared_ptr<AvoidanceParameters> & parameters);
+
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
 

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
@@ -2736,6 +2736,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

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/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");
   }