feat: add visualization for no detection area (#92)

* feat: add no detection area filter

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* chore: use the string equality operator

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* chore: move the position of the function

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* feat: add function that allows to get polygons by type

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* chore: add visualization for no obstacle segmentation area

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* chore: fix typo

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* docs: add ducuments for no_obstacle_segmentation_area

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* ci(pre-commit): autofix

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

tmp/lanelet2_extension/docs/lanelet2_format_extension.md

@@ -208,3 +208,25 @@ _An example:_
   <tag k='type' v='lanelet' />
 </relation>
 ```
+
+### No Obstacle Segmentation Area
+
+If there is a polygon area that has `no_obstacle_segmentation_area` tag, the obstacle points in this area are removed.
+If you want to ignore points for a certain module, you have to define another tag and specify it in the parameter of vector_map_inside_area_filter.
+Currently, following tags are defined other than `no_obstacle_segmentation_area`.
+
+- `no_obstacle_segmentation_area_for_run_out`
+  - remove points for run out module
+
+_An example:_
+
+```xml
+  <way id="1658">
+    <nd ref="1653"/>
+    <nd ref="1654"/>
+    <nd ref="1656"/>
+    <nd ref="1657"/>
+    <tag k="type" v="no_obstacle_segmentation_area"/>
+    <tag k="area" v="yes"/>
+  </way>
+```

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

@@ -114,6 +114,10 @@ std::vector<lanelet::DetectionAreaConstPtr> detectionAreas(const lanelet::ConstL
 std::vector<lanelet::NoStoppingAreaConstPtr> noStoppingAreas(
   const lanelet::ConstLanelets & lanelets);
 
+// query all polygons that has given type in lanelet2 map
+lanelet::ConstPolygons3d getAllPolygonsByType(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type);
+
 // query all obstacle polygons in lanelet2 map
 lanelet::ConstPolygons3d getAllObstaclePolygons(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr);

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

@@ -270,6 +270,27 @@ visualization_msgs::msg::MarkerArray generateLaneletIdMarker(
 visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray(
   const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c);
 
+/**
+ * [noObstacleSegmentationAreaAsMarkerArray creates marker array to visualize no obstacle
+ * segmentation area]
+ * @param  no_obstacle_segmentation_area [no obstacle segmentation area polygon]
+ * @param  c                             [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_obstacle_segmentation_area,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [noObstacleSegmentationAreaForRunOutAsMarkerArray creates marker array to visualize no obstacle
+ * segmentation area for run out]
+ * @param  no_obstacle_segmentation_area_for_run_out [no obstacle segmentation area for run out
+ * polygon]
+ * @param  c                                         [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaForRunOutAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_obstacle_segmentation_area_for_run_out,
+  const std_msgs::msg::ColorRGBA & c);
+
 }  // namespace lanelet::visualization
 
 // NOLINTEND(readability-identifier-naming)

tmp/lanelet2_extension/lib/query.cpp

@@ -212,6 +212,19 @@ std::vector<lanelet::NoStoppingAreaConstPtr> query::noStoppingAreas(
   return no_reg_elems;
 }
 
+lanelet::ConstPolygons3d query::getAllPolygonsByType(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type)
+{
+  lanelet::ConstPolygons3d polygons;
+  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
+    const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == polygon_type) {
+      polygons.push_back(poly);
+    }
+  }
+  return polygons;
+}
+
 lanelet::ConstPolygons3d query::getAllObstaclePolygons(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
 {
@@ -878,7 +891,6 @@ std::vector<lanelet::ConstLanelets> query::getPrecedingLaneletSequences(
   }
   return lanelet_sequences_vec;
 }
-
 }  // namespace lanelet::utils
 
 // NOLINTEND(readability-identifier-naming)

tmp/lanelet2_extension/lib/visualization.cpp

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