feat(avoidance): suppress unnecessary avoidance path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
satoshi-ota · Oct 24, 2023 · 7401250 · 7401250
1 parent 0f9eb8a
commit 7401250
Show file tree

Hide file tree

Showing 5 changed files with 244 additions and 168 deletions.
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
@@ -42,6 +42,10 @@ bool isWithinCrosswalk(
 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/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -330,6 +330,8 @@ std::optional<double> getSignedDistanceFromBoundary(
 
 Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet);
 
+Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon);
+
 std::vector<Polygon2d> getTargetLaneletPolygons(
   const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length,
   const std::string & target_type);

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