diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index 853d4a5d..b094a2b5 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -149,8 +149,12 @@ lanelet::ConstLineStrings3d getAllPartitions(const lanelet::LaneletMapConstPtr & // 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( +// query all pedestrian polygon markings in lanelet2 map +lanelet::ConstLineStrings3d getAllPedestrianPolygonMarkings( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all pedestrian line markings in lanelet2 map +lanelet::ConstLineStrings3d getAllPedestrianLineMarkings( const lanelet::LaneletMapConstPtr & lanelet_map_ptr); // query all parking spaces in lanelet2 map diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index 5cf3bf4b..c904c43a 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -275,12 +275,22 @@ visualization_msgs::msg::MarkerArray crosswalkAreasAsMarkerArray( const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); /** - * [pedestrianMarkingsAsMarkerArray creates marker array to visualize pedestrian markings] - * @param pedestrian_markings [pedestrian marking polygon] + * [pedestrianPolygonMarkingsAsMarkerArray creates marker array to visualize pedestrian polygon + * markings] + * @param pedestrian_polygon_markings [pedestrian marking polygon] * @param c [color of the marker] */ -visualization_msgs::msg::MarkerArray pedestrianMarkingsAsMarkerArray( - const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c); +visualization_msgs::msg::MarkerArray pedestrianPolygonMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_polygon_markings, + const std_msgs::msg::ColorRGBA & c); + +/** + * [pedestrianLineMarkingsAsMarkerArray creates marker array to visualize pedestrian line markings] + * @param pedestrian_line_markings [pedestrian marking line] + * @param c [color of the marker] + */ +visualization_msgs::msg::MarkerArray pedestrianLineMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_line_markings, const std_msgs::msg::ColorRGBA & c); /** * [parkingLotsAsMarkerArray creates marker array to visualize parking lots] diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index f444bdbd..a505c12e 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -381,17 +381,30 @@ lanelet::ConstLineStrings3d query::getAllFences(const lanelet::LaneletMapConstPt return fences; } -lanelet::ConstLineStrings3d query::getAllPedestrianMarkings( +lanelet::ConstLineStrings3d query::getAllPedestrianPolygonMarkings( const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { - lanelet::ConstLineStrings3d pedestrian_markings; + lanelet::ConstLineStrings3d pedestrian_polygon_markings; for (const auto & ls : lanelet_map_ptr->lineStringLayer) { const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); - if (type == "pedestrian_marking") { - pedestrian_markings.push_back(ls); + if ((type == "pedestrian_marking") && (ls.size() >= 3)) { + pedestrian_polygon_markings.push_back(ls); } } - return pedestrian_markings; + return pedestrian_polygon_markings; +} + +lanelet::ConstLineStrings3d query::getAllPedestrianLineMarkings( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLineStrings3d pedestrian_line_markings; + for (const auto & ls : lanelet_map_ptr->lineStringLayer) { + const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); + if ((type == "pedestrian_marking") && (ls.size() < 3)) { + pedestrian_line_markings.push_back(ls); + } + } + return pedestrian_line_markings; } lanelet::ConstLineStrings3d query::getAllParkingSpaces( diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 26ba5d8b..0c48008c 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -974,19 +974,21 @@ visualization_msgs::msg::MarkerArray visualization::crosswalkAreasAsMarkerArray( return marker_array; } -visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerArray( - const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c) +visualization_msgs::msg::MarkerArray visualization::pedestrianPolygonMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_polygon_markings, + const std_msgs::msg::ColorRGBA & c) { visualization_msgs::msg::MarkerArray marker_array; - if (pedestrian_markings.empty()) { + if (pedestrian_polygon_markings.empty()) { return marker_array; } - visualization_msgs::msg::Marker marker = createPolygonMarker("pedestrian_marking", c); - for (const auto & linestring : pedestrian_markings) { + visualization_msgs::msg::Marker polygon_marker = + createPolygonMarker("pedestrian_polygon_marking", c); + for (const auto & linestring : pedestrian_polygon_markings) { lanelet::ConstPolygon3d polygon; if (utils::lineStringToPolygon(linestring, &polygon)) { - pushPolygonMarker(&marker, polygon, c); + pushPolygonMarker(&polygon_marker, polygon, c); } else { RCLCPP_WARN_STREAM( rclcpp::get_logger("lanelet2_extension.visualization"), @@ -994,9 +996,35 @@ visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerAr } } - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); + if (!polygon_marker.points.empty()) { + marker_array.markers.push_back(polygon_marker); + } + + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::pedestrianLineMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_line_markings, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (pedestrian_line_markings.empty()) { + return marker_array; } + + const float lss = 0.1; // line string size + visualization_msgs::msg::Marker line_marker; + visualization::initLineStringMarker(&line_marker, "map", "pedestrian_line_marking", c); + + for (const auto & linestring : pedestrian_line_markings) { + if ((linestring.size() < 3) && (linestring.front().id() != linestring.back().id())) { + pushLineStringMarker(&line_marker, linestring, c, lss); + } + } + + if (!line_marker.points.empty()) { + marker_array.markers.push_back(line_marker); + } + return marker_array; } diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py index 2ba57f47..c9311bcb 100644 --- a/tmp/lanelet2_extension_python/example/example.py +++ b/tmp/lanelet2_extension_python/example/example.py @@ -134,7 +134,8 @@ def test_utility_query(lanelet_map, routing_graph): print(f"""{len(query.getAllPartitions(lanelet_map))=}""") print(f"""{len(query.getAllParkingSpaces(lanelet_map))=}""") print(f"""{len(query.getAllFences(lanelet_map))=}""") - print(f"""{len(query.getAllPedestrianMarkings(lanelet_map))=}""") + print(f"""{len(query.getAllPedestrianPolygonMarkings(lanelet_map))=}""") + print(f"""{len(query.getAllPedestrianLineMarkings(lanelet_map))=}""") print(f"""{len(query.stopLinesLanelets(lanelets))=}""") print(f"""{len(query.stopLinesLanelet(lanelet108))=}""") print(f"""{len(query.stopSignStopLines(lanelets))=}""") diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py index 7a245b46..7ac7206e 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py @@ -25,7 +25,8 @@ getAllParkingLots = _utility_cpp.getAllParkingLots getAllPartitions = _utility_cpp.getAllPartitions getAllFences = _utility_cpp.getAllFences -getAllPedestrianMarkings = _utility_cpp.getAllPedestrianMarkings +getAllPedestrianPolygonMarkings = _utility_cpp.getAllPedestrianPolygonMarkings +getAllPedestrianLineMarkings = _utility_cpp.getAllPedestrianLineMarkings getAllParkingSpaces = _utility_cpp.getAllParkingSpaces getLinkedParkingSpaces = _utility_cpp.getLinkedParkingSpaces getLinkedLanelet = _utility_cpp.getLinkedLanelet diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index fa69bc34..fdd612f1 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -470,7 +470,9 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def("getAllParkingLots", lanelet::utils::query::getAllParkingLots); bp::def("getAllPartitions", lanelet::utils::query::getAllPartitions); bp::def("getAllFences", lanelet::utils::query::getAllFences); - bp::def("getAllPedestrianMarkings", lanelet::utils::query::getAllPedestrianMarkings); + bp::def( + "getAllPedestrianPolygonMarkings", lanelet::utils::query::getAllPedestrianPolygonMarkings); + bp::def("getAllPedestrianLineMarkings", lanelet::utils::query::getAllPedestrianLineMarkings); bp::def("getAllParkingSpaces", lanelet::utils::query::getAllParkingSpaces); bp::def