feat: add visualization for no detection area

tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp

@@ -175,6 +175,9 @@ lanelet::ConstLineStrings3d getLinkedParkingSpaces(
 lanelet::ConstLanelets getLinkedLanelets(
   const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets);
 
+// query all no detection area in lanelet2 map
+lanelet::ConstPolygons3d getAllNoDetectionArea(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
 /**
  * [stopLinesLanelets extracts stoplines that are associated to lanelets]
  * @param lanelets [input lanelets]

tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp

@@ -270,6 +270,14 @@ visualization_msgs::msg::MarkerArray generateLaneletIdMarker(
 visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray(
   const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c);
 
+/**
+ * [NoDetectionAreaAsMarkerArray creates marker array to visualize no detection area]
+ * @param  no_detection_area [no detection area polygon]
+ * @param  c                 [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray noDetectionAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_detection_area, const std_msgs::msg::ColorRGBA & c);
+
 }  // namespace lanelet::visualization
 
 // NOLINTEND(readability-identifier-naming)

tmp/lanelet2_extension/lib/query.cpp

@@ -879,6 +879,19 @@ std::vector<lanelet::ConstLanelets> query::getPrecedingLaneletSequences(
   return lanelet_sequences_vec;
 }
 
+lanelet::ConstPolygons3d query::getAllNoDetectionArea(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstPolygons3d no_detection_area;
+  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
+    const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "no_detection_area") {
+      no_detection_area.push_back(poly);
+    }
+  }
+  return no_detection_area;
+}
+
 }  // namespace lanelet::utils
 
 // NOLINTEND(readability-identifier-naming)

tmp/lanelet2_extension/lib/visualization.cpp

@@ -1263,6 +1263,25 @@ void visualization::pushArrowsMarker(
   }
 }
 
+visualization_msgs::msg::MarkerArray visualization::noDetectionAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_detection_area, const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (no_detection_area.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker = createPolygonMarker("no_detection_area", c);
+  for (const auto & polygon : no_detection_area) {
+    pushPolygonMarker(&marker, polygon, c);
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
 }  // namespace lanelet
 
 // NOLINTEND(readability-identifier-naming)