feat(map_loader): show intersection areas (#211)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Oct 25, 2023 · 6054e6b · 6054e6b
1 parent c3a1a19
commit 6054e6b
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 0 deletions.
diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
@@ -301,6 +301,14 @@ visualization_msgs::msg::MarkerArray generateLaneletIdMarker(
 visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray(
   const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c);
 
+/**
+ * [intersectionAreaAsMarkerArray creates marker array to visualize intersection area]
+ * @param  intersection_areas [intersection area polygon]
+ * @param  c                  [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray intersectionAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & intersection_areas, const std_msgs::msg::ColorRGBA & c);
+
 /**
  * [noObstacleSegmentationAreaAsMarkerArray creates marker array to visualize no obstacle
  * segmentation area]

diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp
@@ -1458,6 +1458,25 @@ void visualization::pushArrowsMarker(
   }
 }
 
+visualization_msgs::msg::MarkerArray visualization::intersectionAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & intersection_areas, const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (intersection_areas.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker = createPolygonMarker("intersection_area", c);
+  for (const auto & polygon : intersection_areas) {
+    pushPolygonMarker(&marker, polygon, c);
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
 visualization_msgs::msg::MarkerArray visualization::noObstacleSegmentationAreaAsMarkerArray(
   const lanelet::ConstPolygons3d & no_obstacle_segmentation_area,
   const std_msgs::msg::ColorRGBA & c)