feat: reverting changes of lanelet regulatory element

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

tmp/lanelet2_extension/CMakeLists.txt

@@ -44,7 +44,6 @@ ament_auto_add_library(lanelet2_extension_lib SHARED
   lib/virtual_traffic_light.cpp
   lib/visualization.cpp
   lib/route_checker.cpp
-  lib/invalid_lanelet.cpp
 )
 target_link_libraries(lanelet2_extension_lib
   ${GeographicLib_LIBRARIES}

tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp

@@ -23,7 +23,6 @@
 #include "lanelet2_extension/regulatory_elements/detection_area.hpp"
 #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
 #include "lanelet2_extension/regulatory_elements/speed_bump.hpp"
-#include "lanelet2_extension/regulatory_elements/invalid_lanelet.hpp"
 
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
@@ -44,7 +43,6 @@ using AutowareTrafficLightConstPtr = std::shared_ptr<const lanelet::autoware::Au
 using DetectionAreaConstPtr = std::shared_ptr<const lanelet::autoware::DetectionArea>;
 using NoStoppingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoStoppingArea>;
 using SpeedBumpConstPtr = std::shared_ptr<const lanelet::autoware::SpeedBump>;
-using InvalidLaneletConstPtr = std::shared_ptr<const lanelet::autoware::InvalidLanelet>;
 }  // namespace lanelet
 
 namespace lanelet::utils::query
@@ -125,13 +123,6 @@ std::vector<lanelet::NoStoppingAreaConstPtr> noStoppingAreas(
  */
 std::vector<lanelet::SpeedBumpConstPtr> speedBumps(const lanelet::ConstLanelets & lanelets);
 
-/**
- * [invalidLanelets extracts Invalid Lanelet regulatory elements from lanelets]
- * @param lanelets [input lanelets]
- * @return         [invalid lanelets that are associated with input lanelets]
- */
-std::vector<lanelet::InvalidLaneletConstPtr> invalidLanelets(const lanelet::ConstLanelets & lanelets);
-
 // query all polygons that has given type in lanelet2 map
 lanelet::ConstPolygons3d getAllPolygonsByType(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type);

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

@@ -241,16 +241,6 @@ visualization_msgs::msg::MarkerArray speedBumpsAsMarkerArray(
   const std::vector<lanelet::SpeedBumpConstPtr> & sb_reg_elems, const std_msgs::msg::ColorRGBA & c,
   const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
 
-/**
- * [InvalidLaneletsAsMarkerArray creates marker array to visualize invalid lanelets]
- * @param  il_reg_elems [invalid lanelet regulatory elements]
- * @param  c            [color of the marker]
- * @param  duration     [lifetime of the marker]
- */
-visualization_msgs::msg::MarkerArray invalidLaneletsAsMarkerArray(
-  const std::vector<lanelet::InvalidLaneletConstPtr> & il_reg_elems, const std_msgs::msg::ColorRGBA & c,
-  const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
-
 /**
  * [pedestrianMarkingsAsMarkerArray creates marker array to visualize pedestrian markings]
  * @param  pedestrian_markings [pedestrian marking polygon]

tmp/lanelet2_extension/lib/query.cpp

@@ -240,34 +240,6 @@ std::vector<lanelet::SpeedBumpConstPtr> query::speedBumps(const lanelet::ConstLa
   return sb_reg_elems;
 }
 
-std::vector<lanelet::InvalidLaneletConstPtr> query::invalidLanelets(const lanelet::ConstLanelets & lanelets)
-{
-  std::vector<lanelet::InvalidLaneletConstPtr> il_reg_elems;
-
-  for (const auto & ll : lanelets) {
-    std::vector<lanelet::InvalidLaneletConstPtr> ll_il_re =
-      ll.regulatoryElementsAs<lanelet::autoware::InvalidLanelet>();
-
-    // insert unique id into array
-    for (const auto & il_ptr : ll_il_re) {
-      lanelet::Id id = il_ptr->id();
-      bool unique_id = true;
-
-      for (const auto & il_reg_elem : il_reg_elems) {
-        if (id == il_reg_elem->id()) {
-          unique_id = false;
-          break;
-        }
-      }
-
-      if (unique_id) {
-        il_reg_elems.push_back(il_ptr);
-      }
-    }
-  }
-  return il_reg_elems;
-}
-
 lanelet::ConstPolygons3d query::getAllPolygonsByType(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type)
 {

tmp/lanelet2_extension/lib/visualization.cpp

@@ -792,70 +792,6 @@ visualization_msgs::msg::MarkerArray visualization::speedBumpsAsMarkerArray(
   return marker_array;
 }
 
-visualization_msgs::msg::MarkerArray visualization::invalidLaneletsAsMarkerArray(
-  const std::vector<lanelet::InvalidLaneletConstPtr> & il_reg_elems, const std_msgs::msg::ColorRGBA & c,
-  const rclcpp::Duration & duration)
-{
-  visualization_msgs::msg::MarkerArray marker_array;
-  visualization_msgs::msg::Marker marker;
-  visualization_msgs::msg::Marker line_marker;
-
-  if (il_reg_elems.empty()) {
-    return marker_array;
-  }
-
-  marker.header.frame_id = "map";
-  marker.header.stamp = rclcpp::Time();
-  marker.frame_locked = true;
-  marker.ns = "invalid_lanelet";
-  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 & il_reg_elem : il_reg_elems) {
-    marker.points.clear();
-    marker.colors.clear();
-    marker.id = static_cast<int32_t>(il_reg_elem->id());
-
-    // area visualization
-    const auto invalid_lanelet = il_reg_elem->invalidLanelet();
-
-    geometry_msgs::msg::Polygon geom_poly;
-    utils::conversion::toGeomMsgPoly(invalid_lanelet, &geom_poly);
-
-    std::vector<geometry_msgs::msg::Polygon> 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
-
-    marker_array.markers.push_back(marker);
-  }  // for regulatory elements
-
-  return marker_array;
-}
-
 visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerArray(
   const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c)
 {