feat(lanelet2_extension): add no parking area

tmp/lanelet2_extension/CMakeLists.txt

@@ -34,6 +34,7 @@ ament_auto_add_library(lanelet2_extension_lib SHARED
   lib/autoware_osm_parser.cpp
   lib/autoware_traffic_light.cpp
   lib/detection_area.cpp
+  lib/no_parking_area.cpp
   lib/no_stopping_area.cpp
   lib/message_conversion.cpp
   lib/mgrs_projector.cpp

tmp/lanelet2_extension/docs/lanelet2_format_extension.md

@@ -269,3 +269,51 @@ _An example:_
     <tag k="area" v="yes"/>
   </way>
 ```
+
+### No Stopping Area
+
+The area with `no_stopping_area` tag can be used to prohibit even a few seconds of stopping, even for traffic jams or at traffic lights.
+The ref_line can be set arbitrarily, and the ego-vehicle should stop at this line if it cannot pass through the area.
+
+_An example:_
+
+```xml
+  <way id='9853' visible='true' version='1'>
+    <nd ref='9849' />
+    <nd ref='9850' />
+    <nd ref='9851' />
+    <nd ref='9852' />
+    <tag k='area' v='yes' />
+    <tag k='type' v='no_stopping_area' />
+  </way>
+
+  <relation id='9854' visible='true' version='1'>
+    <member type='way' ref='9853' role='refers' />
+    <member type='way' ref='9848' role='ref_line' />
+    <tag k='subtype' v='no_stopping_area' />
+    <tag k='type' v='regulatory_element' />
+  </relation>
+```
+
+### No Parking Area
+
+The area with `no_parking_area` tag can be used to prohibit parking. Stopping for a few seconds is allowed in this area.
+
+_An example:_
+
+```xml
+  <way id='9853' visible='true' version='1'>
+    <nd ref='9849' />
+    <nd ref='9850' />
+    <nd ref='9851' />
+    <nd ref='9852' />
+    <tag k='area' v='yes' />
+    <tag k='type' v='no_parking_area' />
+  </way>
+
+  <relation id='9854' visible='true' version='1'>
+    <member type='way' ref='9853' role='refers' />
+    <tag k='subtype' v='no_parking_area' />
+    <tag k='type' v='regulatory_element' />
+  </relation>
+```

tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp

@@ -0,0 +1,72 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// 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__NO_PARKING_AREA_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_PARKING_AREA_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 NoParkingArea : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<NoParkingArea>;
+  static constexpr char RuleName[] = "no_parking_area";
+
+  static Ptr make(Id id, const AttributeMap & attributes, const Polygons3d & no_parking_areas)
+  {
+    return Ptr{new NoParkingArea(id, attributes, no_parking_areas)};
+  }
+
+  /**
+   * @brief get the relevant no parking area
+   * @return no parking area
+   */
+  [[nodiscard]] ConstPolygons3d noParkingAreas() const;
+  [[nodiscard]] Polygons3d noParkingAreas();
+
+  /**
+   * @brief add a new no parking area
+   * @param primitive no parking area to add
+   */
+  void addNoParkingArea(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a no parking area
+   * @param primitive the primitive
+   * @return true if the no parking area existed and was removed
+   */
+  bool removeNoParkingArea(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<NoParkingArea>;
+  NoParkingArea(Id id, const AttributeMap & attributes, const Polygons3d & no_parking_area);
+  explicit NoParkingArea(const lanelet::RegulatoryElementDataPtr & data);
+};
+static lanelet::RegisterRegulatoryElement<NoParkingArea> regNoParkingArea;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_PARKING_AREA_HPP_

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

@@ -21,6 +21,7 @@
 
 #include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
 #include "lanelet2_extension/regulatory_elements/detection_area.hpp"
+#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp"
 #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
 #include "lanelet2_extension/regulatory_elements/speed_bump.hpp"
 
@@ -42,7 +43,9 @@ using TrafficSignConstPtr = std::shared_ptr<const lanelet::TrafficSign>;
 using TrafficLightConstPtr = std::shared_ptr<const lanelet::TrafficLight>;
 using AutowareTrafficLightConstPtr = std::shared_ptr<const lanelet::autoware::AutowareTrafficLight>;
 using DetectionAreaConstPtr = std::shared_ptr<const lanelet::autoware::DetectionArea>;
+using NoParkingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoParkingArea>;
 using NoStoppingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoStoppingArea>;
+using NoParkingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoParkingArea>;
 using SpeedBumpConstPtr = std::shared_ptr<const lanelet::autoware::SpeedBump>;
 }  // namespace lanelet
 
@@ -117,6 +120,13 @@ std::vector<lanelet::DetectionAreaConstPtr> detectionAreas(const lanelet::ConstL
 std::vector<lanelet::NoStoppingAreaConstPtr> noStoppingAreas(
   const lanelet::ConstLanelets & lanelets);
 
+/**
+ * [noParkingArea extracts NoParking Area regulatory elements from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [no parking areas that are associated with input lanelets]
+ */
+std::vector<lanelet::NoParkingAreaConstPtr> noParkingAreas(const lanelet::ConstLanelets & lanelets);
+
 /**
  * [speedBumps extracts Speed Bump regulatory elements from lanelets]
  * @param lanelets [input lanelets]

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

@@ -20,6 +20,7 @@
 // NOLINTBEGIN(readability-identifier-naming)
 
 #include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp"
 #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
 #include "lanelet2_extension/utility/query.hpp"
 
@@ -221,6 +222,16 @@ visualization_msgs::msg::MarkerArray detectionAreasAsMarkerArray(
   const std::vector<lanelet::DetectionAreaConstPtr> & da_reg_elems,
   const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
 
+/**
+ * [noParkingAreasAsMarkerArray creates marker array to visualize detection areas]
+ * @param  no_reg_elems [no parking area regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ */
+visualization_msgs::msg::MarkerArray noParkingAreasAsMarkerArray(
+  const std::vector<lanelet::NoParkingAreaConstPtr> & no_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
+
 /**
  * [noStoppingAreasAsMarkerArray creates marker array to visualize detection areas]
  * @param  no_reg_elems [mp stopping area regulatory elements]

tmp/lanelet2_extension/lib/no_parking_area.cpp

@@ -0,0 +1,131 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// 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/no_parking_area.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;
+}
+
+template <typename T>
+Optional<T> tryGetFront(const std::vector<T> & vec)
+{
+  if (vec.empty()) {
+    return {};
+  }
+  return vec.front();
+}
+
+template <typename T>
+RuleParameters toRuleParameters(const std::vector<T> & primitives)
+{
+  auto cast_func = [](const auto & elem) { return static_cast<RuleParameter>(elem); };
+  return utils::transform(primitives, cast_func);
+}
+
+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 constructNoParkingAreaData(
+  Id id, const AttributeMap & attributes, const Polygons3d & NoParkingAreas)
+{
+  RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(NoParkingAreas)}};
+
+  auto data = std::make_shared<RegulatoryElementData>(id, std::move(rpm), attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "no_parking_area";
+  return data;
+}
+}  // namespace
+
+NoParkingArea::NoParkingArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getConstPoly(data->parameters, RoleName::Refers).empty()) {
+    throw InvalidInputError("no parking area defined!");
+  }
+}
+
+NoParkingArea::NoParkingArea(
+  Id id, const AttributeMap & attributes, const Polygons3d & no_parking_areas)
+: NoParkingArea(constructNoParkingAreaData(id, attributes, no_parking_areas))
+{
+}
+
+ConstPolygons3d NoParkingArea::noParkingAreas() const
+{
+  return getConstPoly(parameters(), RoleName::Refers);
+}
+Polygons3d NoParkingArea::noParkingAreas()
+{
+  return getPoly(parameters(), RoleName::Refers);
+}
+
+void NoParkingArea::addNoParkingArea(const Polygon3d & primitive)
+{
+  parameters()["no_parking_area"].emplace_back(primitive);
+}
+
+bool NoParkingArea::removeNoParkingArea(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("no_parking_area")->second);
+}
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)

tmp/lanelet2_extension/lib/query.cpp

@@ -212,6 +212,35 @@ std::vector<lanelet::NoStoppingAreaConstPtr> query::noStoppingAreas(
   return no_reg_elems;
 }
 
+std::vector<lanelet::NoParkingAreaConstPtr> query::noParkingAreas(
+  const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::NoParkingAreaConstPtr> no_pa_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::NoParkingAreaConstPtr> ll_no_pa_re =
+      ll.regulatoryElementsAs<lanelet::autoware::NoParkingArea>();
+
+    // insert unique NoParkingArea into array
+    for (const auto & no_pa_ptr : ll_no_pa_re) {
+      lanelet::Id id = no_pa_ptr->id();
+      bool unique_id = true;
+
+      for (const auto & no_reg_elem : no_pa_reg_elems) {
+        if (id == no_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id) {
+        no_pa_reg_elems.push_back(no_pa_ptr);
+      }
+    }
+  }
+  return no_pa_reg_elems;
+}
+
 std::vector<lanelet::SpeedBumpConstPtr> query::speedBumps(const lanelet::ConstLanelets & lanelets)
 {
   std::vector<lanelet::SpeedBumpConstPtr> sb_reg_elems;