diff --git a/autoware_lanelet2_extension/CMakeLists.txt b/autoware_lanelet2_extension/CMakeLists.txt index 9e1b405..ed930f5 100644 --- a/autoware_lanelet2_extension/CMakeLists.txt +++ b/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 diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/Forward.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/Forward.hpp index 0b4aa11..ba2810d 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/Forward.hpp +++ b/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; using CrosswalkConstPtr = std::shared_ptr; } // namespace format_v1 +namespace format_v2 +{ +using BusStopAreaConstPtr = std::shared_ptr; +} // namespace format_v2 + } // namespace lanelet // NOLINTEND(readability-identifier-naming) diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp new file mode 100644 index 0000000..f2f2be2 --- /dev/null +++ b/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 + +#include + +#include + +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; + 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; + 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_ diff --git a/autoware_lanelet2_extension/lib/bus_stop_area.cpp b/autoware_lanelet2_extension/lib/bus_stop_area.cpp new file mode 100644 index 0000000..b2f5a5a --- /dev/null +++ b/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 + +#include + +#include +#include +#include +#include + +namespace lanelet::autoware +{ +namespace +{ +template +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 +Optional tryGetFront(const std::vector & vec) +{ + if (vec.empty()) { + return {}; + } + return vec.front(); +} + +template +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return static_cast(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(¶m); + if (p != nullptr) { + result.push_back(*p); + } + } + return result; +} + +ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role) +{ + auto cast_func = [](auto & poly) { return static_cast(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(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, ¶meters().find("bus_stop_area")->second); +} + +RegisterRegulatoryElement regBusStopArea; +} // namespace format_v2 + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/autoware_lanelet2_extension/test/src/test_regulatory_elements.cpp b/autoware_lanelet2_extension/test/src/test_regulatory_elements.cpp index a7d9ff8..61efe62 100644 --- a/autoware_lanelet2_extension/test/src/test_regulatory_elements.cpp +++ b/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 @@ -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);