feat(lanelet2_extension): add crosswalk regulatory element

tmp/lanelet2_extension/CMakeLists.txt

@@ -33,6 +33,7 @@ add_definitions(${GeographicLib_DEFINITIONS})
 ament_auto_add_library(lanelet2_extension_lib SHARED
   lib/autoware_osm_parser.cpp
   lib/autoware_traffic_light.cpp
+  lib/crosswalk.cpp
   lib/detection_area.cpp
   lib/no_parking_area.cpp
   lib/no_stopping_area.cpp

tmp/lanelet2_extension/docs/lanelet2_format_extension.md

@@ -202,6 +202,35 @@ The following illustrates how light_bulbs are registered to traffic_light regula
 </relation>
 ```
 
+### Crosswalk
+
+Original Lanelet2 format only requires `subtype=crosswalk` tag to be specified in the corresponding lanelet. However, Autoware requires a regulatory element to be defined on top of that in order to:
+
+- explicitly define the relevant driving lanes even in 3D environment
+- optionally define stop lines associated with the crosswalk
+- enable accurate definition of complex polygons for crosswalk
+
+For the details, refer to this [GitHub discussion](https://github.com/orgs/autowarefoundation/discussions/3036).
+Crosswalk regulatory element can be tied to `ref_line`, `crosswalk_polygon` and `refers`.
+
+![crosswalk_regulatory elements](crosswalk_regulatory_element.svg)
+
+- `ref_line`: Stop line for the crosswalk.
+- `crosswalk_polygon`: Accurate area of the crosswalk.
+- `refers`: Lanelet that indicates the moving direction of crosswalk users.
+
+_An example:_
+
+```xml
+<relation id="10751">
+  <member type="way" role="ref_line" ref="8123"/>
+  <member type="way" role="crosswalk_polygon" ref="4047"/>
+  <member type="relation" role="refers" ref="2206"/>
+  <tag k="type" v="regulatory_element"/>
+  <tag k="subtype" v="crosswalk"/>
+</relation>
+```
+
 ### Safety Slow Down for Crosswalks
 
 If you wish ego vehicle to slow down to a certain speed from a certain distance while passing over a

tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp

@@ -27,17 +27,17 @@
 
 namespace lanelet::autoware
 {
-struct AutowareRoleNameString
-{
-  static constexpr const char LightBulbs[] = "light_bulbs";
-};
-
 class AutowareTrafficLight : public lanelet::TrafficLight
 {
 public:
   using Ptr = std::shared_ptr<AutowareTrafficLight>;
   static constexpr char RuleName[] = "traffic_light";
 
+  struct AutowareRoleNameString
+  {
+    static constexpr const char LightBulbs[] = "light_bulbs";
+  };
+
   //! Directly construct a stop line from its required rule parameters.
   //! Might modify the input data in oder to get correct tags.
   static Ptr make(

tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp

@@ -0,0 +1,92 @@
+// 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__CROSSWALK_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__CROSSWALK_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 Crosswalk : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<Crosswalk>;
+  static constexpr char RuleName[] = "crosswalk";
+
+  struct AutowareRoleNameString
+  {
+    static constexpr const char CrosswalkPolygon[] = "crosswalk_polygon";
+  };
+
+  static Ptr make(
+    Id id, const AttributeMap & attributes, const Lanelet & crosswalk_lanelet,
+    const Polygon3d & crosswalk_area, const LineStrings3d & stop_line)
+  {
+    return Ptr{new Crosswalk(id, attributes, crosswalk_lanelet, crosswalk_area, stop_line)};
+  }
+
+  /**
+   * @brief get the relevant crosswalk area
+   * @return crosswalk area
+   */
+  [[nodiscard]] ConstPolygons3d crosswalkAreas() const;
+
+  /**
+   * @brief get the relevant crosswalk line
+   * @return stop line
+   */
+  [[nodiscard]] ConstLineStrings3d stopLines() const;
+
+  /**
+   * @brief get the relevant crosswalk lanelet
+   * @return lanelet
+   */
+  [[nodiscard]] ConstLanelet crosswalkLanelet() const;
+
+  /**
+   * @brief add a new crosswalk area
+   * @param primitive crosswalk area to add
+   */
+  void addCrosswalkArea(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a crosswalk area
+   * @param primitive the primitive
+   * @return true if the crosswalk area existed and was removed
+   */
+  bool removeCrosswalkArea(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<Crosswalk>;
+  Crosswalk(
+    Id id, const AttributeMap & attributes, const Lanelet & crosswalk_lanelet,
+    const Polygon3d & crosswalk_area, const LineStrings3d & stop_line);
+  explicit Crosswalk(const lanelet::RegulatoryElementDataPtr & data);
+};
+static lanelet::RegisterRegulatoryElement<Crosswalk> regCrosswalk;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__CROSSWALK_HPP_

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

@@ -20,6 +20,7 @@
 // NOLINTBEGIN(readability-identifier-naming)
 
 #include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+#include "lanelet2_extension/regulatory_elements/crosswalk.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"
@@ -47,6 +48,7 @@ using NoParkingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoParking
 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>;
+using CrosswalkConstPtr = std::shared_ptr<const lanelet::autoware::Crosswalk>;
 }  // namespace lanelet
 
 namespace lanelet::utils::query
@@ -134,6 +136,13 @@ std::vector<lanelet::NoParkingAreaConstPtr> noParkingAreas(const lanelet::ConstL
  */
 std::vector<lanelet::SpeedBumpConstPtr> speedBumps(const lanelet::ConstLanelets & lanelets);
 
+/**
+ * [crosswalks extracts Crosswalk regulatory elements from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [crosswalks that are associated with input lanelets]
+ */
+std::vector<lanelet::CrosswalkConstPtr> crosswalks(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

@@ -252,6 +252,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));
 
+/**
+ * [crosswalkAreasAsMarkerArray creates marker array to visualize crosswalk regulatory element]
+ * @param  sb_reg_elems [crosswalk regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ */
+visualization_msgs::msg::MarkerArray crosswalkAreasAsMarkerArray(
+  const std::vector<lanelet::CrosswalkConstPtr> & cw_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/autoware_traffic_light.cpp

@@ -100,7 +100,8 @@ LineStringsOrPolygons3d getLsOrPoly(const RuleParameterMap & paramsMap, RoleName
     rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters));
   }
   if (!lightBulbs.empty()) {
-    rpm.insert(std::make_pair(AutowareRoleNameString::LightBulbs, toRuleParameters(lightBulbs)));
+    rpm.insert(std::make_pair(
+      AutowareTrafficLight::AutowareRoleNameString::LightBulbs, toRuleParameters(lightBulbs)));
   }
 
   auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);