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..f625243 --- /dev/null +++ b/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 + +#include + +#include + +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; + 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; + 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..ba53743 --- /dev/null +++ b/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 + +#include + +#include +#include +#include +#include + +namespace lanelet::autoware +{ +namespace +{ +template +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 +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 & BusStopAreas) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(BusStopAreas)}}; + + 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(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, ¶meters().find("bus_stop_area")->second); +} + +RegisterRegulatoryElement regBusStopArea; +} // namespace format_v2 + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming)