add bus_stop_area

Signed-off-by: Mamoru Sobue <[email protected]>

autoware_lanelet2_extension/CMakeLists.txt

@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED
   lib/landmark.cpp
   lib/no_parking_area.cpp
   lib/no_stopping_area.cpp
+  lib/bus_stop_area.cpp
   lib/message_conversion.cpp
   lib/mgrs_projector.cpp
   lib/query.cpp

autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/Forward.hpp

@@ -37,6 +37,11 @@ class SpeedBump;
 class VirtualTrafficLight;
 }  // namespace format_v1
 
+namespace format_v2
+{
+class BusStopArea;
+}  // namespace format_v2
+
 }  // namespace lanelet::autoware
 
 namespace lanelet
@@ -54,6 +59,11 @@ using SpeedBumpConstPtr = std::shared_ptr<const lanelet::autoware::SpeedBump>;
 using CrosswalkConstPtr = std::shared_ptr<const lanelet::autoware::Crosswalk>;
 }  // namespace format_v1
 
+namespace format_v2
+{
+using BusStopAreaConstPtr = std::shared_ptr<const lanelet::autoware::format_v2::BusStopArea>;
+}  // namespace format_v2
+
 }  // namespace lanelet
 
 // NOLINTEND(readability-identifier-naming)

autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp

@@ -0,0 +1,77 @@
+// Copyright 2024 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 AUTOWARE_LANELET2_EXTENSION__REGULATORY_ELEMENTS__BUS_STOP_AREA_HPP_
+#define AUTOWARE_LANELET2_EXTENSION__REGULATORY_ELEMENTS__BUS_STOP_AREA_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <autoware_lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+
+// TODO(Mamoru Sobue): when updating existing elements from format_v1, prefix this with inline
+namespace format_v2
+{
+class BusStopArea : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<BusStopArea>;
+  static constexpr char RuleName[] = "bus_stop_area";
+
+  static Ptr make(Id id, const AttributeMap & attributes, const Polygons3d & bus_stop_areas)
+  {
+    return Ptr{new BusStopArea(id, attributes, bus_stop_areas)};
+  }
+
+  /**
+   * @brief get the relevant bus stop area
+   * @return bus stop area
+   */
+  [[nodiscard]] ConstPolygons3d BusStopAreas() const;
+  [[nodiscard]] Polygons3d BusStopAreas();
+
+  /**
+   * @brief add a new bus stop are
+   * @param primitive bus stop area to add
+   */
+  void addBusStopArea(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a bus stop area
+   * @param primitive the primitive
+   * @return true if the bus stop area existed and was removed
+   */
+  bool removeBusStopArea(const Polygon3d & primitive);
+
+private:
+  BusStopArea(Id id, const AttributeMap & attributes, const Polygons3d & bus_stop_area);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<BusStopArea>;
+  explicit BusStopArea(const lanelet::RegulatoryElementDataPtr & data);
+};
+}  // namespace format_v2
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // AUTOWARE_LANELET2_EXTENSION__REGULATORY_ELEMENTS__BUS_STOP_AREA_HPP_

autoware_lanelet2_extension/lib/bus_stop_area.cpp

@@ -0,0 +1,137 @@
+// Copyright 2024 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 "autoware_lanelet2_extension/regulatory_elements/bus_stop_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 constructBusStopAreaData(
+  Id id, const AttributeMap & attributes, const Polygons3d & BusStopAreas)
+{
+  RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(BusStopAreas)}};
+
+  auto data = std::make_shared<RegulatoryElementData>(id, std::move(rpm), attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "bus_stop_area";
+  return data;
+}
+}  // namespace
+
+// TODO(Mamoru Sobue): remove this when format_v2 has been released
+namespace format_v2
+{
+BusStopArea::BusStopArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getConstPoly(data->parameters, RoleName::Refers).empty()) {
+    throw InvalidInputError("no parking area defined!");
+  }
+}
+
+BusStopArea::BusStopArea(Id id, const AttributeMap & attributes, const Polygons3d & bus_stop_areas)
+: BusStopArea(constructBusStopAreaData(id, attributes, bus_stop_areas))
+{
+}
+
+ConstPolygons3d BusStopArea::BusStopAreas() const
+{
+  return getConstPoly(parameters(), RoleName::Refers);
+}
+Polygons3d BusStopArea::BusStopAreas()
+{
+  return getPoly(parameters(), RoleName::Refers);
+}
+
+void BusStopArea::addBusStopArea(const Polygon3d & primitive)
+{
+  parameters()["bus_stop_area"].emplace_back(primitive);
+}
+
+bool BusStopArea::removeBusStopArea(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("bus_stop_area")->second);
+}
+
+RegisterRegulatoryElement<BusStopArea> regBusStopArea;
+}  // namespace format_v2
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)