feat: first draft proposal implementation for handling invalid lanelets

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

tmp/lanelet2_extension/CMakeLists.txt

@@ -44,6 +44,7 @@ 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/regulatory_elements/invalid_lanelet.hpp

@@ -0,0 +1,73 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__INVALID_LANELET_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__INVALID_LANELET_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+#include <vector>
+
+namespace lanelet::autoware
+{
+class InvalidLanelet : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<InvalidLanelet>;
+  static constexpr char RuleName[] = "invalid_lanelet";
+
+  static Ptr make(Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet)
+  {
+    return Ptr{new InvalidLanelet(id, attributes, invalid_lanelet)};
+  }
+
+  /**
+   * @brief get the relevant invalid lanelet
+   * @return invalid lanelet
+   */
+  [[nodiscard]] ConstPolygon3d invalidLanelet() const;
+  [[nodiscard]] Polygon3d invalidLanelet();
+
+  /**
+   * @brief add a new invalid lanelet
+   * @param primitive invalid lanelet to add
+   */
+  void addInvalidLanelet(const Polygon3d & primitive);
+
+  /**
+   * @brief remove an invalid lanelet
+   * @param primitive invalid lanelet to remove
+   * @return true if the invalid lanelet existed and was removed
+   */
+  bool removeInvalidLanelet(const Polygon3d & primitive);
+
+private:
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class lanelet::RegisterRegulatoryElement<InvalidLanelet>;
+  InvalidLanelet(Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet);
+  explicit InvalidLanelet(const lanelet::RegulatoryElementDataPtr & data);
+};
+static lanelet::RegisterRegulatoryElement<InvalidLanelet> regInvalidLanelet;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__INVALID_LANELET_HPP_

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

@@ -23,6 +23,7 @@
 #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>
@@ -43,6 +44,7 @@ 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
@@ -123,6 +125,13 @@ 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,6 +241,16 @@ 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/invalid_lanelet.cpp

@@ -0,0 +1,115 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/invalid_lanelet.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T & primitive, RuleParameters * member)
+{
+  if (member == nullptr) {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end()) {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+Polygons3d getPoly(const RuleParameterMap & paramsMap, RoleName role)
+{
+  auto params = paramsMap.find(role);
+  if (params == paramsMap.end()) {
+    return {};
+  }
+
+  Polygons3d result;
+  for (auto & param : params->second) {
+    auto p = boost::get<Polygon3d>(&param);
+    if (p != nullptr) {
+      result.push_back(*p);
+    }
+  }
+  return result;
+}
+
+ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role)
+{
+  auto cast_func = [](auto & poly) { return static_cast<ConstPolygon3d>(poly); };
+  return utils::transform(getPoly(params, role), cast_func);
+}
+
+RegulatoryElementDataPtr constructInvalidLaneletData(
+  Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet)
+{
+  RuleParameterMap rpm;
+  RuleParameters rule_parameters = {invalid_lanelet};
+  rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters));
+
+  auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "invalid_lanelet";
+  return data;
+}
+}  // namespace
+
+InvalidLanelet::InvalidLanelet(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getConstPoly(data->parameters, RoleName::Refers).size() != 1) {
+    throw InvalidInputError("There must be exactly one invalid lanelet defined!");
+  }
+}
+
+InvalidLanelet::InvalidLanelet(Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet)
+: InvalidLanelet(constructInvalidLaneletData(id, attributes, invalid_lanelet))
+{
+}
+
+ConstPolygon3d InvalidLanelet::invalidLanelet() const
+{
+  return getConstPoly(parameters(), RoleName::Refers).front();
+}
+
+Polygon3d InvalidLanelet::invalidLanelet() { return getPoly(parameters(), RoleName::Refers).front(); }
+
+void InvalidLanelet::addInvalidLanelet(const Polygon3d & primitive)
+{
+  parameters()["invalid_lanelet"].emplace_back(primitive);
+}
+
+bool InvalidLanelet::removeInvalidLanelet(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("invalid_lanelet")->second);
+}
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)

tmp/lanelet2_extension/lib/query.cpp

@@ -240,6 +240,34 @@ 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,6 +792,70 @@ 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)
 {