feat(goal_planner): add no_parking_area for goal search (autowarefoun…

…dation#3467) (#700)

* feat(behavior_path_planner): use no_parking_area for pull_over



* support no_stopping_area



---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

@@ -123,13 +123,15 @@ void Lanelet2MapVisualizationNode::onMapBin(
       viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out");
   lanelet::ConstPolygons3d hatched_road_markings_area =
     lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings");
+  std::vector<lanelet::NoParkingAreaConstPtr> no_parking_reg_elems =
+    lanelet::utils::query::noParkingAreas(all_lanelets);
 
   std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
     cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
     cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons,
     cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
     cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area,
-    cl_hatched_road_markings_line;
+    cl_hatched_road_markings_line, cl_no_parking_areas;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -150,6 +152,7 @@ void Lanelet2MapVisualizationNode::onMapBin(
   setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5);
   setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5);
   setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999);
+  setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -229,6 +232,10 @@ void Lanelet2MapVisualizationNode::onMapBin(
     lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray(
       hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line));
 
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas));
+
   pub_marker_->publish(map_marker_array);
 }
 

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp

@@ -44,6 +44,7 @@ class GoalSearcher : public GoalSearcherBase
   bool checkCollision(const Pose & pose) const;
   bool checkCollisionWithLongitudinalDistance(
     const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
+  BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;
 

planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

@@ -26,6 +26,7 @@
 namespace behavior_path_planner
 {
 using lane_departure_checker::LaneDepartureChecker;
+using lanelet::autoware::NoParkingArea;
 using lanelet::autoware::NoStoppingArea;
 using tier4_autoware_utils::calcOffsetPose;
 using tier4_autoware_utils::inverseTransformPose;
@@ -102,6 +103,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
       const auto transformed_vehicle_footprint =
         transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose));
 
+      if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) {
+        continue;
+      }
+
       if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) {
         continue;
       }
@@ -274,6 +279,20 @@ void GoalSearcher::createAreaPolygons(std::vector<Pose> original_search_poses)
   }
 }
 
+BasicPolygons2d GoalSearcher::getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const
+{
+  BasicPolygons2d area_polygons{};
+  for (const auto & ll : lanes) {
+    for (const auto & reg_elem : ll.regulatoryElementsAs<NoParkingArea>()) {
+      for (const auto & area : reg_elem->noParkingAreas()) {
+        const auto & area_poly = lanelet::utils::to2D(area).basicPolygon();
+        area_polygons.push_back(area_poly);
+      }
+    }
+  }
+  return area_polygons;
+}
+
 BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const
 {
   BasicPolygons2d area_polygons{};