diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index 57b642df..853d4a5d 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -146,6 +146,9 @@ lanelet::ConstPolygons3d getAllParkingLots(const lanelet::LaneletMapConstPtr & l // query all partitions in lanelet2 map lanelet::ConstLineStrings3d getAllPartitions(const lanelet::LaneletMapConstPtr & lanelet_map_ptr); +// query all fences in lanelet2 map +lanelet::ConstLineStrings3d getAllFences(const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + // query all pedestrian markings in lanelet2 map lanelet::ConstLineStrings3d getAllPedestrianMarkings( const lanelet::LaneletMapConstPtr & lanelet_map_ptr); diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index 47a35a64..f444bdbd 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -369,6 +369,18 @@ lanelet::ConstLineStrings3d query::getAllPartitions( return partitions; } +lanelet::ConstLineStrings3d query::getAllFences(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLineStrings3d fences; + for (const auto & ls : lanelet_map_ptr->lineStringLayer) { + const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "fence") { + fences.push_back(ls); + } + } + return fences; +} + lanelet::ConstLineStrings3d query::getAllPedestrianMarkings( const lanelet::LaneletMapConstPtr & lanelet_map_ptr) {