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/docs/extra_lanelet_subtypes.md b/autoware_lanelet2_extension/docs/extra_lanelet_subtypes.md index b7b84eb..a39776b 100644 --- a/autoware_lanelet2_extension/docs/extra_lanelet_subtypes.md +++ b/autoware_lanelet2_extension/docs/extra_lanelet_subtypes.md @@ -43,3 +43,27 @@ Sample pedestrian lane in .osm format is shown below: ``` + +## Bicycle Lane (format_version >= 2) + +Although `bicycle_lane` subtype is already defined in the original [Lanelet2 documentation](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletAndAreaTagging.md#subtype-and-location), Autoware compatible with `format_version>=2` supports `bicycle_lane` at component level. + +- refers: `road` lanelet adjacent to the objective `bicycle_lane` should share the same boundary LineString +- specification: + - A `bicicyle_lane` is connected to/from only `bicycle_lane` lanelets and is adjacent to a `road` lanelet. Namely in Left(Right) hand side traffic rules, it is on the left(right) side of `road` lanelet. + - A `road_shoulder` lanelet must not be adjacent to a `bicycle_lane` because pullover on `bicycle_lane` is prohibited. For simplifying planning/prediction component, at formart_version 2, a `bicycle_lane` should cover the adjacent area corresponding to `road_shoulder` or `pedestrian_lane`. +- The treatment of _navigation mark_, or the shift-arrow shaped painting on the side of the road without explicit separating line, is open to dicussion. We can either identity them as part of the vehicle road or separate them as distinct bicycle lanes. + +Sample bicycle lane in .osm format is shown below: + +```xml + + + + + + + + + +``` diff --git a/autoware_lanelet2_extension/docs/extra_regulatory_elements.md b/autoware_lanelet2_extension/docs/extra_regulatory_elements.md index 1debdf9..6c24aa1 100644 --- a/autoware_lanelet2_extension/docs/extra_regulatory_elements.md +++ b/autoware_lanelet2_extension/docs/extra_regulatory_elements.md @@ -1,5 +1,22 @@ # Extra Regulatory Elements +## Right Of Way + +Users must add `right_of_way` tag to intersection lanes, namely lanes with `turn_direction` attribute. Below image illustrates how to set yield lanes(orange) for the ego lane(blue). + +![RightOfWay tagging](right_of_way.drawio.svg) + +Basically intersection lanes which are: + +- left/right turn +- straight and on the side of priority sign + +need this tag to know which lanes in their `conflicting lanes` can be ignored for object detection. + +left/right turning lane is often conflicting with lanes whose traffic lights are red when its traffic light is green, so **at least** those lanes should be registered as yield lanes. + +If ego car is going straight the intersection when the traffic light is green, then it does not need to care other lanes because it has the highest priority. But if the traffic lights do not exist and ego lane is on the side of priority road, then yield lanes should be set to explicitly ignore part of conflicting lanes. + ## Detection Area This regulatory element specifies region of interest which vehicle must pay attention whenever it is driving along the associated lanelet. When there are any obstacle in the detection area, vehicle must stop at specified stopline. @@ -147,3 +164,180 @@ An example annotation of speed_bump: ``` + +### Crosswalk + +Original Lanelet2 format only requires `subtype=crosswalk` tag to be specified in the corresponding lanelet. However, Autoware requires a regulatory element to be defined on top of that in order to: + +- explicitly define the relevant driving lanes even in 3D environment +- optionally define stop lines associated with the crosswalk +- enable accurate definition of complex polygons for crosswalk + +For the details, refer to this [GitHub discussion](https://github.com/orgs/autowarefoundation/discussions/3036). +Crosswalk regulatory element can be tied to `ref_line`, `crosswalk_polygon` and `refers`. + +![crosswalk_regulatory elements](crosswalk_regulatory_element.svg) + +- `ref_line`: Stop line for the crosswalk. +- `crosswalk_polygon`: Accurate area of the crosswalk. +- `refers`: Lanelet that indicates the moving direction of crosswalk users. + +_An example:_ + +```xml + + + + + + + +``` + +#### Traffic Lights for Crosswalks + +It can define not only traffic lights for vehicles but also for crosswalk users by using regulatory element. In this case, the crosswalk lanelet needs to refer the traffic light regulatory element. + +![crosswalk_traffic_light](crosswalk_traffic_light.svg) + +_An example:_ + +```xml + + + + + + + + +... + + + + + + + + +... + + + + + + + + + + + + +``` + +#### Safety Slow Down for Crosswalks + +If you wish ego vehicle to slow down to a certain speed from a certain distance while passing over a +certain crosswalk _even though there are no target objects around it_, you can add following tags to +the crosswalk definition on lanelet2 map: + +- `safety_slow_down_speed` **[m/s]**: The speed you want ego vehicle to drive at while passing over + the crosswalk +- `safety_slow_down_distance` **[m]**: The distance between front bumper of ego vehicle and + closest point to the crosswalk when ego vehicle slows down and drives at specified speed + +_An example:_ + +```xml + + + + + + + + +``` + +## 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 + + + + + + + + + + + + + + +``` + +## Bus Stop Area + +The area tagged with `bus_stop_area` can be used to limit the pullover space for bus-like vehicles. This area is referred from the road or road_shoulder subtype lanelets where the destination is positioned via the `BusStopArea` regulatory element. + +_An eample:_ + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + + +``` diff --git a/autoware_lanelet2_extension/docs/lanelet2_format_extension.md b/autoware_lanelet2_extension/docs/lanelet2_format_extension.md index 0039223..0d3bc8c 100644 --- a/autoware_lanelet2_extension/docs/lanelet2_format_extension.md +++ b/autoware_lanelet2_extension/docs/lanelet2_format_extension.md @@ -10,13 +10,42 @@ Users may use `autoware_lanelet2_validation` [node](../README.md#nodes) to check The following is the extra format added for Autoware: - [extra regulatory elements](extra_regulatory_elements.md) + - Right Of Way - Detection Area - Road Marking + - Speed Bump + - Crosswalk + - No Stopping Area + - No Parking Area + - Bus Stoop Area - [extra lanelet subtype](extra_lanelet_subtypes.md) - Roadside Lane + - Road Shoulder + - Pedestrian Lane + - Bicycle Lane + +Note that each `extra_regulatory_elements` and `extra_lanelet_subtypes` should faithfully represent the real world map primitives as-is for realizing appropriate behavior on public roads. Although they are not necessarily mandatory for running minimal Autoware, each ODD requires different set of `extra_regulatory_elements` and `extra_lanelet_subtypes` respectively. + +[Autoware Vector Map Requirements](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/map/map-requirements/vector-map-requirements-overview/) provides more details and guidelines for how to create the Lanelet2 maps without ambiguity while avoiding unintended behaviors due to invalid Lanlet2 map settings. ## Mandatory Tags +### Meta Info + +Users may add the `MetaInfo` element to their OSM file to indicate format version and map version of their OSM file. This information is not meant to influence Autoware vehicle's behavior, but is published as ROS message so that developers could know which map was used from ROSBAG log files. MetaInfo elements exists in the same hierarchy with `node`, `way`, and `relation` elements, otherwise JOSM wouldn't be able to load the file correctly. + +Here is an example of MetaInfo in osm file: + +```xml + + + + ... + ... + ... + +``` + ### Elevation Tags Elevation("ele") information for points(`node`) is optional in default Lanelet2 format. @@ -77,43 +106,10 @@ Here is an example of osm syntax for lanelets in intersections. ``` -### Right Of Way - -Users must add `right_of_way` tag to intersection lanes, namely lanes with `turn_direction` attribute. Below image illustrates how to set yield lanes(orange) for the ego lane(blue). - -![RightOfWay tagging](right_of_way.drawio.svg) - -Basically intersection lanes which are: - -- left/right turn -- straight and on the side of priority sign - -need this tag to know which lanes in their `conflicting lanes` can be ignored for object detection. - -left/right turning lane is often conflicting with lanes whose traffic lights are red when its traffic light is green, so **at least** those lanes should be registered as yield lanes. - -If ego car is going straight the intersection when the traffic light is green, then it does not need to care other lanes because it has the highest priority. But if the traffic lights do not exist and ego lane is on the side of priority road, then yield lanes should be set to explicitly ignore part of conflicting lanes. - ## Optional Taggings Following tags are optional tags that you may want to add depending on how you want to use your map in Autoware. -### Meta Info - -Users may add the `MetaInfo` element to their OSM file to indicate format version and map version of their OSM file. This information is not meant to influence Autoware vehicle's behavior, but is published as ROS message so that developers could know which map was used from ROSBAG log files. MetaInfo elements exists in the same hierarchy with `node`, `way`, and `relation` elements, otherwise JOSM wouldn't be able to load the file correctly. - -Here is an example of MetaInfo in osm file: - -```xml - - - - ... - ... - ... - -``` - ### Local Coordinate Expression Sometimes users might want to create Lanelet2 maps that are not georeferenced. @@ -202,99 +198,25 @@ The following illustrates how light_bulbs are registered to traffic_light regula ``` -### Crosswalk - -Original Lanelet2 format only requires `subtype=crosswalk` tag to be specified in the corresponding lanelet. However, Autoware requires a regulatory element to be defined on top of that in order to: - -- explicitly define the relevant driving lanes even in 3D environment -- optionally define stop lines associated with the crosswalk -- enable accurate definition of complex polygons for crosswalk - -For the details, refer to this [GitHub discussion](https://github.com/orgs/autowarefoundation/discussions/3036). -Crosswalk regulatory element can be tied to `ref_line`, `crosswalk_polygon` and `refers`. - -![crosswalk_regulatory elements](crosswalk_regulatory_element.svg) - -- `ref_line`: Stop line for the crosswalk. -- `crosswalk_polygon`: Accurate area of the crosswalk. -- `refers`: Lanelet that indicates the moving direction of crosswalk users. - -_An example:_ - -```xml - - - - - - - -``` - -### Traffic Lights for Crosswalks - -It can define not only traffic lights for vehicles but also for crosswalk users by using regulatory element. In this case, the crosswalk lanelet needs to refer the traffic light regulatory element. - -![crosswalk_traffic_light](crosswalk_traffic_light.svg) - -_An example:_ - -```xml - - - - - - - - -... - - - - - - - - -... - - - - - - - - - - - - -``` +### Centerline -### Safety Slow Down for Crosswalks +Note that the following explanation is not related to the Lanelet2 map format but how the Autoware handles the centerline in the Lanelet2 map. -If you wish ego vehicle to slow down to a certain speed from a certain distance while passing over a -certain crosswalk _even though there are no target objects around it_, you can add following tags to -the crosswalk definition on lanelet2 map: +Centerline is defined in Lanelet2 to guide the vehicle. By explicitly setting the centerline in the Lanelet2 map, the ego's planned path follows the centerline. +However, based on the current Autoware's usage of the centerline, there are several limitations. -- `safety_slow_down_speed` **[m/s]**: The speed you want ego vehicle to drive at while passing over - the crosswalk -- `safety_slow_down_distance` **[m]**: The distance between front bumper of ego vehicle and - closest point to the crosswalk when ego vehicle slows down and drives at specified speed +- The object's predicted path also follows the centerline. + - This may adversely affect the decision making of planning modules since the centerline is supposed to be used only by the ego's path planning. +- The coordinate transformation on the lane's frenet frame cannot be calculated correctly. +- For example, when the lateral distance between the actual road's centerline and a parked vehicle is calculated, actually the result will be the lateral distance between the (explicit) centerline and the vehicle. -_An example:_ +To solve above limitations, the `overwriteLaneletsCenterlineWithWaypoints` was implemented in addition to `overwriteLaneletsCenterline` where the centerline in all the lanes is calculated. -```xml - - - - - - - - -``` +- `overwriteLaneletsCenterlineWithWaypoints` + - The (explicit) centerline in the Lanelet2 map is converted to the new `waypoints` tag. This `waypoints` is only applied to the ego's path planning. + - Therefore, the above limitations can be solved, but the Autoware's usage of the centerline may be hard to understand. +- `overwriteLaneletsCenterline` + - The (explicit) centerline in the Lanelet2 map is used as it is. Easy to understand the Autoware's usage of the centerline, but we still have above limitations. ### No Obstacle Segmentation Area @@ -340,54 +262,6 @@ _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 - - - - - - - - - - - - - - -``` - ### No Drivable Lane A no drivable lane is a lanelet or more that are out of operation design domain (ODD), i.e., the vehicle **must not** drive autonomously in this/these lanelet/s. @@ -479,23 +353,3 @@ _An example:_ ... ``` - -### Centerline - -Note that the following explanation is not related to the Lanelet2 map format but how the Autoware handles the centerline in the Lanelet2 map. - -Centerline is defined in Lanelet2 to guide the vehicle. By explicitly setting the centerline in the Lanelet2 map, the ego's planned path follows the centerline. -However, based on the current Autoware's usage of the centerline, there are several limitations. - -- The object's predicted path also follows the centerline. - - This may adversely affect the decision making of planning modules since the centerline is supposed to be used only by the ego's path planning. -- The coordinate transformation on the lane's frenet frame cannot be calculated correctly. - - For example, when the lateral distance between the actual road's centerline and a parked vehicle is calculated, actually the result will be the lateral distance between the (explicit) centerline and the vehicle. - -To solve above limitations, the `overwriteLaneletsCenterlineWithWaypoints` was implemented in addition to `overwriteLaneletsCenterline` where the centerline in all the lanes is calculated. - -- `overwriteLaneletsCenterlineWithWaypoints` - - The (explicit) centerline in the Lanelet2 map is converted to the new `waypoints` tag. This `waypoints` is only applied to the ego's path planning. - - Therefore, the above limitations can be solved, but the Autoware's usage of the centerline may be hard to understand. -- `overwriteLaneletsCenterline` - - The (explicit) centerline in the Lanelet2 map is used as it is. Easy to understand the Autoware's usage of the centerline, but we still have above limitations. diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/mgrs_projector.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/mgrs_projector.hpp index 6656378..1adfaa2 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/mgrs_projector.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/mgrs_projector.hpp @@ -31,7 +31,7 @@ namespace lanelet::projection { -inline namespace format_v1 +inline namespace format_v2 { class MGRSProjector : public Projector { @@ -111,7 +111,7 @@ class MGRSProjector : public Projector */ mutable std::string projected_grid_; }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::projection diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp index d27ee96..4f77ca5 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp @@ -28,7 +28,7 @@ namespace lanelet::projection { -inline namespace format_v1 +inline namespace format_v2 { class TransverseMercatorProjector : public Projector { @@ -56,7 +56,7 @@ class TransverseMercatorProjector : public Projector double origin_y_; double central_meridian_; }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::projection 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..5796200 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 @@ -25,7 +25,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class AutowareTrafficLight; class Crosswalk; @@ -35,24 +35,26 @@ class NoStoppingArea; class RoadMarking; class SpeedBump; class VirtualTrafficLight; -} // namespace format_v1 +class BusStopArea; +} // namespace format_v2 } // namespace lanelet::autoware namespace lanelet { -inline namespace format_v1 +inline namespace format_v2 { using TrafficSignConstPtr = std::shared_ptr; using TrafficLightConstPtr = std::shared_ptr; using AutowareTrafficLightConstPtr = std::shared_ptr; +using CrosswalkConstPtr = 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; -using CrosswalkConstPtr = std::shared_ptr; -} // namespace format_v1 +using BusStopAreaConstPtr = std::shared_ptr; +using BusStopAreaConstPtr = std::shared_ptr; +} // namespace format_v2 } // namespace lanelet diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp index e2da92f..f072f6d 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp @@ -29,7 +29,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class AutowareTrafficLight : public lanelet::TrafficLight { @@ -86,7 +86,7 @@ class AutowareTrafficLight : public lanelet::TrafficLight friend class RegisterRegulatoryElement; explicit AutowareTrafficLight(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware 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..ea0ecb5 --- /dev/null +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp @@ -0,0 +1,76 @@ +// 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 +{ + +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/include/autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp index 596e03d..39f8ce5 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp @@ -26,7 +26,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class Crosswalk : public lanelet::RegulatoryElement { @@ -87,7 +87,7 @@ class Crosswalk : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit Crosswalk(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/detection_area.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/detection_area.hpp index 847a4a5..e538303 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/detection_area.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/detection_area.hpp @@ -28,7 +28,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class DetectionArea : public lanelet::RegulatoryElement { @@ -91,7 +91,7 @@ class DetectionArea : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit DetectionArea(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp index 589412d..acc3f2a 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp @@ -26,7 +26,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class NoParkingArea : public lanelet::RegulatoryElement { @@ -67,7 +67,7 @@ class NoParkingArea : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit NoParkingArea(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp index 77d9f2a..16e7c5b 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp @@ -26,7 +26,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class NoStoppingArea : public lanelet::RegulatoryElement { @@ -89,7 +89,7 @@ class NoStoppingArea : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit NoStoppingArea(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/road_marking.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/road_marking.hpp index 750931d..3231542 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/road_marking.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/road_marking.hpp @@ -26,7 +26,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class RoadMarking : public lanelet::RegulatoryElement { @@ -67,7 +67,7 @@ class RoadMarking : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit RoadMarking(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/speed_bump.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/speed_bump.hpp index e7b4688..9c5a12a 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/speed_bump.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/speed_bump.hpp @@ -28,7 +28,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class SpeedBump : public lanelet::RegulatoryElement { @@ -69,7 +69,7 @@ class SpeedBump : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit SpeedBump(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp index 0eb0afb..6042392 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp @@ -26,7 +26,7 @@ namespace lanelet::autoware { -inline namespace format_v1 +inline namespace format_v2 { class VirtualTrafficLight : public lanelet::RegulatoryElement { @@ -74,7 +74,7 @@ class VirtualTrafficLight : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit VirtualTrafficLight(const lanelet::RegulatoryElementDataPtr & data); }; -} // namespace format_v1 +} // namespace format_v2 } // namespace lanelet::autoware diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp index 4af3bf1..c6cefd5 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp @@ -40,7 +40,7 @@ namespace lanelet::utils::query { -inline namespace format_v1 +inline namespace format_v2 { /** * [crosswalkLanelets extracts crosswalk lanelets] @@ -207,7 +207,7 @@ std::vector stopLinesLanelet(const lanelet::ConstLan */ std::vector stopSignStopLines( const lanelet::ConstLanelets & lanelets, const std::string & stop_sign_id = "stop_sign"); -} // namespace format_v1 +} // namespace format_v2 /** * [laneletLayer converts laneletLayer into lanelet vector] diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/version.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/version.hpp index 183d4ea..5ff294f 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/version.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/version.hpp @@ -27,10 +27,11 @@ namespace lanelet::autoware enum class Version : int { none = 0, format_v1, + format_v2, }; // current format_version -static constexpr Version version = Version::format_v1; +static constexpr Version version = Version::format_v2; } // namespace lanelet::autoware #endif // AUTOWARE_LANELET2_EXTENSION__VERSION_HPP_ diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp index ed4f335..3886c56 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp @@ -38,7 +38,7 @@ namespace lanelet::visualization { -inline namespace format_v1 +inline namespace format_v2 { /** * [autowareTrafficLightsAsMarkerArray creates marker array to visualize traffic @@ -217,7 +217,7 @@ visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaForRunOutAsMarker visualization_msgs::msg::MarkerArray hatchedRoadMarkingsAreaAsMarkerArray( const lanelet::ConstPolygons3d & hatched_road_markings_area, const std_msgs::msg::ColorRGBA & area_color, const std_msgs::msg::ColorRGBA & line_color); -} // namespace format_v1 +} // namespace format_v2 /** * [lanelet2Triangle converts lanelet into vector of triangles. Used for 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..200709a --- /dev/null +++ b/autoware_lanelet2_extension/lib/bus_stop_area.cpp @@ -0,0 +1,133 @@ +// 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 + +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 lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/autoware_lanelet2_extension/lib/query.cpp b/autoware_lanelet2_extension/lib/query.cpp index 8b1ad6d..1d11914 100644 --- a/autoware_lanelet2_extension/lib/query.cpp +++ b/autoware_lanelet2_extension/lib/query.cpp @@ -55,7 +55,7 @@ namespace lanelet::utils namespace query { -inline namespace format_v1 +inline namespace format_v2 { lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets & lls) { @@ -699,7 +699,7 @@ std::vector stopSignStopLines( } return stoplines; } -} // namespace format_v1 +} // namespace format_v2 } // namespace query // returns all lanelets in laneletLayer - don't know how to convert diff --git a/autoware_lanelet2_extension/lib/visualization.cpp b/autoware_lanelet2_extension/lib/visualization.cpp index a96dcb7..2b24907 100644 --- a/autoware_lanelet2_extension/lib/visualization.cpp +++ b/autoware_lanelet2_extension/lib/visualization.cpp @@ -351,7 +351,7 @@ namespace lanelet namespace visualization { -inline namespace format_v1 +inline namespace format_v2 { visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray( const std::vector & tl_reg_elems, @@ -1075,7 +1075,7 @@ visualization_msgs::msg::MarkerArray hatchedRoadMarkingsAreaAsMarkerArray( return marker_array; } -} // namespace format_v1 +} // namespace format_v2 } // namespace visualization void visualization::lanelet2Triangle(