fix(log-messages): proposing solution to warn messages "lineStringToP…

…olygon: linestring x must have more than different 3 points! (size is 2). Failed to convert to polygon." and "pedestrian marking x failed conversion."

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

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

tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp

@@ -263,12 +263,20 @@ 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]

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(

tmp/lanelet2_extension/lib/visualization.cpp

@@ -922,29 +922,57 @@ 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"),
         "pedestrian marking " << linestring.id() << " failed conversion.");
     }
   }
 
-  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;
 }