diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index 6b109cbe..b4ef3b6e 100644 --- a/tmp/lanelet2_extension/CMakeLists.txt +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_library(lanelet2_extension_lib SHARED lib/autoware_osm_parser.cpp lib/autoware_traffic_light.cpp lib/detection_area.cpp + lib/no_parking_area.cpp lib/no_stopping_area.cpp lib/message_conversion.cpp lib/mgrs_projector.cpp diff --git a/tmp/lanelet2_extension/docs/lanelet2_format_extension.md b/tmp/lanelet2_extension/docs/lanelet2_format_extension.md index cb590548..06d15970 100644 --- a/tmp/lanelet2_extension/docs/lanelet2_format_extension.md +++ b/tmp/lanelet2_extension/docs/lanelet2_format_extension.md @@ -269,3 +269,51 @@ _An example:_ ``` + +### No Stopping Area + +The area with `no_stopping_area` tag can be used to prohibit even a few seconds of stopping, even for traffic jams or at traffic lights. +The ref_line can be set arbitrarily, and the ego-vehicle should stop at this line if it cannot pass through the area. + +_An example:_ + +```xml + + + + + + + + + + + + + + + +``` + +### No Parking Area + +The area with `no_parking_area` tag can be used to prohibit parking. Stopping for a few seconds is allowed in this area. + +_An example:_ + +```xml + + + + + + + + + + + + + + +``` diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp new file mode 100644 index 00000000..a4cc1e25 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp @@ -0,0 +1,72 @@ +// 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__NO_PARKING_AREA_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_PARKING_AREA_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +namespace lanelet::autoware +{ +class NoParkingArea : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "no_parking_area"; + + static Ptr make(Id id, const AttributeMap & attributes, const Polygons3d & no_parking_areas) + { + return Ptr{new NoParkingArea(id, attributes, no_parking_areas)}; + } + + /** + * @brief get the relevant no parking area + * @return no parking area + */ + [[nodiscard]] ConstPolygons3d noParkingAreas() const; + [[nodiscard]] Polygons3d noParkingAreas(); + + /** + * @brief add a new no parking area + * @param primitive no parking area to add + */ + void addNoParkingArea(const Polygon3d & primitive); + + /** + * @brief remove a no parking area + * @param primitive the primitive + * @return true if the no parking area existed and was removed + */ + bool removeNoParkingArea(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; + NoParkingArea(Id id, const AttributeMap & attributes, const Polygons3d & no_parking_area); + explicit NoParkingArea(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regNoParkingArea; + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_PARKING_AREA_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index 302e6e26..96fc0efc 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -21,6 +21,7 @@ #include "lanelet2_extension/regulatory_elements/autoware_traffic_light.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" #include "lanelet2_extension/regulatory_elements/speed_bump.hpp" @@ -42,7 +43,9 @@ using TrafficSignConstPtr = std::shared_ptr; using TrafficLightConstPtr = std::shared_ptr; using AutowareTrafficLightConstPtr = std::shared_ptr; using DetectionAreaConstPtr = std::shared_ptr; +using NoParkingAreaConstPtr = std::shared_ptr; using NoStoppingAreaConstPtr = std::shared_ptr; +using NoParkingAreaConstPtr = std::shared_ptr; using SpeedBumpConstPtr = std::shared_ptr; } // namespace lanelet @@ -117,6 +120,13 @@ std::vector detectionAreas(const lanelet::ConstL std::vector noStoppingAreas( const lanelet::ConstLanelets & lanelets); +/** + * [noParkingArea extracts NoParking Area regulatory elements from lanelets] + * @param lanelets [input lanelets] + * @return [no parking areas that are associated with input lanelets] + */ +std::vector noParkingAreas(const lanelet::ConstLanelets & lanelets); + /** * [speedBumps extracts Speed Bump regulatory elements from lanelets] * @param lanelets [input lanelets] diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index 28c3f3a1..46dbca24 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -20,6 +20,7 @@ // NOLINTBEGIN(readability-identifier-naming) #include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/query.hpp" @@ -221,6 +222,16 @@ visualization_msgs::msg::MarkerArray detectionAreasAsMarkerArray( const std::vector & da_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); +/** + * [noParkingAreasAsMarkerArray creates marker array to visualize detection areas] + * @param no_reg_elems [no parking area regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + */ +visualization_msgs::msg::MarkerArray noParkingAreasAsMarkerArray( + const std::vector & no_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); + /** * [noStoppingAreasAsMarkerArray creates marker array to visualize detection areas] * @param no_reg_elems [mp stopping area regulatory elements] diff --git a/tmp/lanelet2_extension/lib/no_parking_area.cpp b/tmp/lanelet2_extension/lib/no_parking_area.cpp new file mode 100644 index 00000000..807dd82a --- /dev/null +++ b/tmp/lanelet2_extension/lib/no_parking_area.cpp @@ -0,0 +1,131 @@ +// 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. + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/regulatory_elements/no_parking_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 constructNoParkingAreaData( + Id id, const AttributeMap & attributes, const Polygons3d & NoParkingAreas) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(NoParkingAreas)}}; + + auto data = std::make_shared(id, std::move(rpm), attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "no_parking_area"; + return data; +} +} // namespace + +NoParkingArea::NoParkingArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) +{ + if (getConstPoly(data->parameters, RoleName::Refers).empty()) { + throw InvalidInputError("no parking area defined!"); + } +} + +NoParkingArea::NoParkingArea( + Id id, const AttributeMap & attributes, const Polygons3d & no_parking_areas) +: NoParkingArea(constructNoParkingAreaData(id, attributes, no_parking_areas)) +{ +} + +ConstPolygons3d NoParkingArea::noParkingAreas() const +{ + return getConstPoly(parameters(), RoleName::Refers); +} +Polygons3d NoParkingArea::noParkingAreas() +{ + return getPoly(parameters(), RoleName::Refers); +} + +void NoParkingArea::addNoParkingArea(const Polygon3d & primitive) +{ + parameters()["no_parking_area"].emplace_back(primitive); +} + +bool NoParkingArea::removeNoParkingArea(const Polygon3d & primitive) +{ + return findAndErase(primitive, ¶meters().find("no_parking_area")->second); +} +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index c1f7ff66..daff7411 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -212,6 +212,35 @@ std::vector query::noStoppingAreas( return no_reg_elems; } +std::vector query::noParkingAreas( + const lanelet::ConstLanelets & lanelets) +{ + std::vector no_pa_reg_elems; + + for (const auto & ll : lanelets) { + std::vector ll_no_pa_re = + ll.regulatoryElementsAs(); + + // insert unique NoParkingArea into array + for (const auto & no_pa_ptr : ll_no_pa_re) { + lanelet::Id id = no_pa_ptr->id(); + bool unique_id = true; + + for (const auto & no_reg_elem : no_pa_reg_elems) { + if (id == no_reg_elem->id()) { + unique_id = false; + break; + } + } + + if (unique_id) { + no_pa_reg_elems.push_back(no_pa_ptr); + } + } + } + return no_pa_reg_elems; +} + std::vector query::speedBumps(const lanelet::ConstLanelets & lanelets) { std::vector sb_reg_elems; diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index e14215eb..2046302d 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -650,6 +650,68 @@ visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray( return marker_array; } +visualization_msgs::msg::MarkerArray visualization::noParkingAreasAsMarkerArray( + const std::vector & no_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + if (no_reg_elems.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = false; + marker.ns = "no_parking_area"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = duration; + marker.pose.position.x = 0.0; // p.x(); + marker.pose.position.y = 0.0; // p.y(); + marker.pose.position.z = 0.0; // p.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.r = 1.0f; + marker.color.g = 1.0f; + marker.color.b = 1.0f; + marker.color.a = 0.999; + + for (const auto & no_reg_elem : no_reg_elems) { + marker.points.clear(); + marker.colors.clear(); + marker.id = static_cast(no_reg_elem->id()); + + // area visualization + const auto no_parking_areas = no_reg_elem->noParkingAreas(); + for (const auto & no_parking_area : no_parking_areas) { + geometry_msgs::msg::Polygon geom_poly; + utils::conversion::toGeomMsgPoly(no_parking_area, &geom_poly); + + std::vector triangles; + polygon2Triangle(geom_poly, &triangles); + + for (auto tri : triangles) { + geometry_msgs::msg::Point tri0[3]; + + for (int i = 0; i < 3; i++) { + utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]); + marker.points.push_back(tri0[i]); + marker.colors.push_back(c); + } + } // for triangles0 + } // for no_parking areas + marker_array.markers.push_back(marker); + } // for regulatory elements + return marker_array; +} + visualization_msgs::msg::MarkerArray visualization::noStoppingAreasAsMarkerArray( const std::vector & no_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration)