fix

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Sep 9, 2024 · 80f4524 · 80f4524
1 parent 4f61667
commit 80f4524
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 15 deletions.
diff --git a/..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
@@ -66,7 +66,6 @@ class GoalSearcher : public GoalSearcherBase
   BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes) const;
-  bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;
 
   LinearRing2d vehicle_footprint_{};
   bool left_side_parking_{true};

diff --git a/...vior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/...vior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
@@ -123,6 +123,16 @@ std::vector<Polygon2d> createPathFootPrints(
   const PathWithLaneId & path, const double base_to_front, const double base_to_rear,
   const double width);
 
+/**
+ * @brief check if footprint intersects with given areas
+ */
+bool isIntersectingAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas);
+
+/**
+ * @brief check if footprint is within one of the areas
+ */
+bool isWithinAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas);
+
 // debug
 MarkerArray createPullOverAreaMarkerArray(
   const autoware::universe_utils::MultiPolygon2d area_polygons,

diff --git a/...ng/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/...ng/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
@@ -179,16 +179,18 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
 
       if (
         parameters_.bus_stop_area.use_bus_stop_area &&
-        !isInAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) {
-        break;
+        !goal_planner_utils::isWithinAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) {
+        continue;
       }
 
-      if (isInAreas(transformed_vehicle_footprint, no_parking_area_polygons)) {
+      if (goal_planner_utils::isIntersectingAreas(
+            transformed_vehicle_footprint, no_parking_area_polygons)) {
         // break here to exclude goals located laterally in no_parking_areas
         break;
       }
 
-      if (isInAreas(transformed_vehicle_footprint, no_stopping_area_polygons)) {
+      if (goal_planner_utils::isIntersectingAreas(
+            transformed_vehicle_footprint, no_stopping_area_polygons)) {
         // break here to exclude goals located laterally in no_stopping_areas
         break;
       }
@@ -520,16 +522,6 @@ BasicPolygons2d GoalSearcher::getBusStopAreaPolygons(const lanelet::ConstLanelet
   return area_polygons;
 }
 
-bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const
-{
-  for (const auto & area : areas) {
-    if (boost::geometry::intersects(area, footprint)) {
-      return true;
-    }
-  }
-  return false;
-}
-
 GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes(
   const GoalCandidates & goal_candidates,
   const std::shared_ptr<const PlannerData> & planner_data) const

diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
@@ -290,6 +290,26 @@ PredictedObjects filterObjectsByLateralDistance(
   return filtered_objects;
 }
 
+bool isIntersectingAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas)
+{
+  for (const auto & area : areas) {
+    if (boost::geometry::intersects(area, footprint)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+bool isWithinAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas)
+{
+  for (const auto & area : areas) {
+    if (boost::geometry::within(footprint, area)) {
+      return true;
+    }
+  }
+  return false;
+}
+
 MarkerArray createPullOverAreaMarkerArray(
   const autoware::universe_utils::MultiPolygon2d area_polygons,
   const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z)