feat(lanelet2_extension): add bus_stop_area implementation (#22)

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 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 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(soblin): when updating existing elements from format_v1, prefix this with inline
+namespace format_v2
+{
+class BusStopArea : public lanelet::RegulatoryElement
+{
+public:
+  using SharedPtr = std::shared_ptr<BusStopArea>;
+  static constexpr char RuleName[] = "bus_stop_area";
+
+  static SharedPtr make(Id id, const AttributeMap & attributes, const Polygons3d & bus_stop_areas)
+  {
+    return SharedPtr{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,136 @@
+// Copyright 2024 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 "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) {
+    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 & bus_stop_areas)
+{
+  RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(bus_stop_areas)}};
+
+  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(soblin): 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 bus stop 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)

autoware_lanelet2_extension/test/src/test_regulatory_elements.cpp

@@ -15,6 +15,7 @@
 // NOLINTBEGIN(readability-identifier-naming)
 
 #include "autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp"
 
 #include <boost/optional/optional_io.hpp>
 
@@ -29,6 +30,7 @@ using lanelet::LineString3d;
 using lanelet::LineStringOrPolygon3d;
 using lanelet::Point3d;
 using lanelet::Points3d;
+using lanelet::Polygon3d;
 using lanelet::utils::getId;
 
 namespace
@@ -138,6 +140,27 @@ TEST(TestSuite, TrafficLightWorksAsExpected)  // NOLINT for gtest
   EXPECT_EQ(1ul, tl->lightBulbs().size());
 }
 
+TEST(TestSuite, BusStopAreInstantiation)  // NOLINT for gtest
+{
+  /*
+    p4 <---- p3
+     |       ^
+     |       |
+     V       |
+    p1 ----> p2
+   */
+  const Point3d p1{getId(), 0, 0, 0};
+  const Point3d p2{getId(), 3, 0, 0};
+  const Point3d p3{getId(), 3, 3, 0};
+  const Point3d p4{getId(), 0, 3, 0};
+  const Polygon3d polygon{LineString3d{getId(), Points3d{p1, p2, p3, p4}}};
+  auto bus_stop_area_reg_elem = lanelet::autoware::format_v2::BusStopArea::make(
+    getId(), lanelet::AttributeMap(), convertToVector(polygon));
+  EXPECT_EQ(bus_stop_area_reg_elem->busStopAreas().size(), 1);
+  EXPECT_NO_THROW(bus_stop_area_reg_elem->busStopAreas().at(0));
+  const auto bus_stop_area = bus_stop_area_reg_elem->busStopAreas().at(0);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);