feat(lanelet2_extension): add util/visualization of BusStopArea (#28)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp

@@ -103,6 +103,13 @@ std::vector<lanelet::NoStoppingAreaConstPtr> noStoppingAreas(
  */
 std::vector<lanelet::NoParkingAreaConstPtr> 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<lanelet::BusStopAreaConstPtr> busStopAreas(const lanelet::ConstLanelets & lanelets);
+
 /**
  * [speedBumps extracts Speed Bump regulatory elements from lanelets]
  * @param lanelets [input lanelets]

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<lanelet::NoParkingAreaConstPtr> & 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<lanelet::BusStopAreaConstPtr> & 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]

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"
@@ -217,6 +218,25 @@ std::vector<lanelet::NoParkingAreaConstPtr> noParkingAreas(const lanelet::ConstL
   return no_pa_reg_elems;
 }
 
+std::vector<lanelet::BusStopAreaConstPtr> busStopAreas(const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::BusStopAreaConstPtr> bus_stop_area_reg_elems;
+  std::set<lanelet::Id> found_ids;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::BusStopAreaConstPtr> reg_elems =
+      ll.regulatoryElementsAs<lanelet::autoware::BusStopArea>();
+    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<lanelet::SpeedBumpConstPtr> speedBumps(const lanelet::ConstLanelets & lanelets)
 {
   std::vector<lanelet::SpeedBumpConstPtr> sb_reg_elems;

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<lanelet::BusStopAreaConstPtr> & 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<int32_t>(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<geometry_msgs::msg::Polygon> triangles;
+      polygon2Triangle(geom_poly, &triangles);
+
+      for (const 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<lanelet::NoStoppingAreaConstPtr> & no_reg_elems,
   const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration)