From 6054e6bf566430141ebaf1f1966819ea0c5fc62b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 25 Oct 2023 12:07:11 +0900 Subject: [PATCH] feat(map_loader): show intersection areas (#211) Signed-off-by: satoshi-ota --- .../visualization/visualization.hpp | 8 ++++++++ tmp/lanelet2_extension/lib/visualization.cpp | 19 +++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index d5ff5b0e..a4f568a0 100644 --- 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 index 5d58f5b7..1f434a23 100644 --- 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)