From 07a753066da84690648117b3060c8edeabd2f3a8 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Sep 2024 18:32:36 +0900 Subject: [PATCH] feat(lanelet2_extension): add util/visualization of BusStopArea Signed-off-by: Mamoru Sobue --- .../utility/query.hpp | 7 +++ .../visualization/visualization.hpp | 11 ++++ autoware_lanelet2_extension/lib/query.cpp | 20 ++++++ .../lib/visualization.cpp | 62 +++++++++++++++++++ 4 files changed, 100 insertions(+) diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp index c6cefd5..ff57ae5 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp @@ -95,6 +95,13 @@ std::vector noStoppingAreas( */ std::vector noParkingAreas(const lanelet::ConstLanelets & lanelets); +/** + * [busStopArea extracts BusStop Area regulatory elements from lanelets] + * @param lanelets [input lanelets] + * @return [bus stop areas that are associated with input lanelets] + */ +std::vector busStopAreas(const lanelet::ConstLanelets & lanelets); + /** * [speedBumps extracts Speed Bump regulatory elements from lanelets] * @param lanelets [input lanelets] diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp index 3886c56..0dab839 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp @@ -20,6 +20,7 @@ // NOLINTBEGIN(readability-identifier-naming) #include "autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" @@ -111,6 +112,16 @@ visualization_msgs::msg::MarkerArray noParkingAreasAsMarkerArray( const std::vector & no_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); +/** + * [busStopAreasAsMarkerArray creates marker array to visualize bus stop areas] + * @param no_reg_elems [bus stop area regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + */ +visualization_msgs::msg::MarkerArray busStopAreasAsMarkerArray( + const std::vector & no_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); + /** * [noStoppingAreasAsMarkerArray creates marker array to visualize detection areas] * @param no_reg_elems [mp stopping area regulatory elements] diff --git a/autoware_lanelet2_extension/lib/query.cpp b/autoware_lanelet2_extension/lib/query.cpp index 1d11914..4a8a6b3 100644 --- a/autoware_lanelet2_extension/lib/query.cpp +++ b/autoware_lanelet2_extension/lib/query.cpp @@ -19,6 +19,7 @@ #include "autoware_lanelet2_extension/utility/query.hpp" #include "autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp" #include "autoware_lanelet2_extension/regulatory_elements/detection_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" @@ -212,6 +213,25 @@ std::vector noParkingAreas(const lanelet::ConstL return no_pa_reg_elems; } +std::vector busStopAreas(const lanelet::ConstLanelets & lanelets) +{ + std::vector bus_stop_area_reg_elems; + std::set found_ids; + + for (const auto & ll : lanelets) { + std::vector reg_elems = + ll.regulatoryElementsAs(); + for (const auto & reg_elem : reg_elems) { + const auto id = reg_elem->id(); + if (found_ids.find(id) == found_ids.end()) { + found_ids.insert(id); + bus_stop_area_reg_elems.push_back(reg_elem); + } + } + } + return bus_stop_area_reg_elems; +} + std::vector speedBumps(const lanelet::ConstLanelets & lanelets) { std::vector sb_reg_elems; diff --git a/autoware_lanelet2_extension/lib/visualization.cpp b/autoware_lanelet2_extension/lib/visualization.cpp index 2b24907..7ddf475 100644 --- a/autoware_lanelet2_extension/lib/visualization.cpp +++ b/autoware_lanelet2_extension/lib/visualization.cpp @@ -651,6 +651,68 @@ visualization_msgs::msg::MarkerArray noParkingAreasAsMarkerArray( return marker_array; } +visualization_msgs::msg::MarkerArray busStopAreasAsMarkerArray( + const std::vector & bus_stop_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + if (bus_stop_reg_elems.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = false; + marker.ns = "bus_stop_area"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = duration; + marker.pose.position.x = 0.0; // p.x(); + marker.pose.position.y = 0.0; // p.y(); + marker.pose.position.z = 0.0; // p.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.r = 1.0f; + marker.color.g = 1.0f; + marker.color.b = 1.0f; + marker.color.a = 0.999; + + for (const auto & bus_stop_reg_elem : bus_stop_reg_elems) { + marker.points.clear(); + marker.colors.clear(); + marker.id = static_cast(bus_stop_reg_elem->id()); + + // area visualization + const auto bus_stop_areas = bus_stop_reg_elem->busStopAreas(); + for (const auto & bus_stop_area : bus_stop_areas) { + geometry_msgs::msg::Polygon geom_poly; + utils::conversion::toGeomMsgPoly(bus_stop_area, &geom_poly); + + std::vector triangles; + polygon2Triangle(geom_poly, &triangles); + + for (auto tri : triangles) { + geometry_msgs::msg::Point tri0[3]; + + for (int i = 0; i < 3; i++) { + utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]); + marker.points.push_back(tri0[i]); + marker.colors.push_back(c); + } + } // for triangles0 + } // for bus_stop_area + marker_array.markers.push_back(marker); + } // for regulatory elements + return marker_array; +} + visualization_msgs::msg::MarkerArray noStoppingAreasAsMarkerArray( const std::vector & no_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration)