diff --git a/.clang-tidy b/.clang-tidy index fcf5a6fc..5d5692a7 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -425,6 +425,8 @@ CheckOptions: value: "" - key: portability-simd-intrinsics.Suggest value: "0" + - key: readability-function-cognitive-complexity.IgnoreMacros + value: "1" - key: readability-else-after-return.WarnOnUnfixable value: "1" - key: readability-identifier-naming.NamespaceCase diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 746d6bd3..bb407eb5 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -64,7 +64,7 @@ jobs: clang-tidy-differential: runs-on: ubuntu-latest - container: ros:galactic + container: ros:humble needs: build-and-test-differential steps: - name: Check out repository @@ -83,7 +83,7 @@ jobs: if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test-with-reverse-depends.yaml b/.github/workflows/build-and-test-with-reverse-depends.yaml index 73c45b56..6c1c949f 100644 --- a/.github/workflows/build-and-test-with-reverse-depends.yaml +++ b/.github/workflows/build-and-test-with-reverse-depends.yaml @@ -30,7 +30,9 @@ jobs: - name: Import depends of reverse depends run: | yq -i 'del(.repositories.* | select(.url == "https://github.com/autowarefoundation/autoware_common.git"))' reverse_depends/build_depends.repos + yq -i 'del(.repositories.* | select(.url == "https://github.com/tier4/autoware_auto_msgs.git"))' reverse_depends/build_depends.repos vcs import reverse_depends < reverse_depends/build_depends.repos + rm -rf reverse_depends/map/lanelet2_extension - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} diff --git a/README.md b/README.md index db5f8106..fb07757a 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,5 @@ # autoware_common Commonly referenced ROS library packages for [Autoware](https://github.com/autowarefoundation/autoware). + +Note that the packages under `tmp/` may introduce breaking changes and is planned to be replaced by re-designed packages in the future. diff --git a/build_depends.repos b/build_depends.repos index 56f46b6f..4b86fbe3 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -1 +1,5 @@ repositories: + tmp/autoware_auto_msgs: + type: git + url: https://github.com/tier4/autoware_auto_msgs.git + version: tier4/main diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt new file mode 100644 index 00000000..a0db7fe7 --- /dev/null +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.14) +project(lanelet2_extension) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) + +find_library(GeographicLib_LIBRARIES + NAMES Geographic +) + +find_library(PUGIXML_LIBRARIES + NAMES pugixml +) + +find_path(PUGIXML_INCLUDE_DIRS + NAMES pugixml.hpp + PATH_SUFFIXES pugixml +) + +include_directories( + ${GeographicLib_INCLUDE_DIRS} + ${PUGIXML_INCLUDE_DIRS} +) + +add_definitions(${GeographicLib_DEFINITIONS}) + +ament_auto_add_library(lanelet2_extension_lib SHARED + lib/autoware_osm_parser.cpp + lib/autoware_traffic_light.cpp + lib/detection_area.cpp + lib/no_stopping_area.cpp + lib/message_conversion.cpp + lib/mgrs_projector.cpp + lib/query.cpp + lib/road_marking.cpp + lib/utilities.cpp + lib/virtual_traffic_light.cpp + lib/visualization.cpp + lib/route_checker.cpp +) +target_link_libraries(lanelet2_extension_lib + ${GeographicLib_LIBRARIES} +) + +ament_auto_add_executable(lanelet2_extension_sample src/sample_code.cpp) +add_dependencies(lanelet2_extension_sample lanelet2_extension_lib) +target_link_libraries(lanelet2_extension_sample + lanelet2_extension_lib +) + +ament_auto_add_executable(autoware_lanelet2_validation src/validation.cpp) +add_dependencies(autoware_lanelet2_validation lanelet2_extension_lib) +target_link_libraries(autoware_lanelet2_validation + ${catkin_LIBRARIES} + ${PUGIXML_LIBRARIES} + lanelet2_extension_lib +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(message_conversion-test test/src/test_message_conversion.cpp) + target_link_libraries(message_conversion-test lanelet2_extension_lib) + ament_add_ros_isolated_gtest(projector-test test/src/test_projector.cpp) + target_link_libraries(projector-test lanelet2_extension_lib) + ament_add_ros_isolated_gtest(query-test test/src/test_query.cpp) + target_link_libraries(query-test lanelet2_extension_lib) + ament_add_ros_isolated_gtest(regulatory_elements-test test/src/test_regulatory_elements.cpp) + target_link_libraries(regulatory_elements-test lanelet2_extension_lib) + ament_add_ros_isolated_gtest(utilities-test test/src/test_utilities.cpp) + target_link_libraries(utilities-test lanelet2_extension_lib) + ament_add_ros_isolated_gtest(route-test test/src/test_route_checker.cpp) + target_link_libraries(route-test lanelet2_extension_lib) +endif() + +ament_auto_package() diff --git a/tmp/lanelet2_extension/README.md b/tmp/lanelet2_extension/README.md new file mode 100644 index 00000000..af88db61 --- /dev/null +++ b/tmp/lanelet2_extension/README.md @@ -0,0 +1,93 @@ +# lanelet2_extension package + +This package contains external library for Lanelet2 and is meant to ease the use of Lanelet2 in Autoware. + +## Lanelet Format for Autoware + +Autoware uses extended Lanelet2 Format for Autoware, which means you need to add some tags to default OSM file if you want to fully use Lanelet2 maps. For details about custom tags, please refer to this [document](./docs/lanelet2_format_extension.md). + +## Code API + +### IO + +#### Autoware OSM Parser + +Autoware Lanelet2 Format uses .osm extension as original Lanelet2. +However, there are some custom tags that is used by the parser. + +Currently, this includes: + +- overwriting x,y values with `local_x` and `local_y` tags. +- reading `` tag which contains information about map format version and map version. + +The parser is registered as "autoware_osm_handler" as lanelet parser + +### Projection + +#### MGRS Projector + +MGRS projector projects latitude longitude into MGRS Coordinates. + +### Regulatory Elements + +#### Autoware Traffic Light + +Autoware Traffic Light class allows you to retrieve information about traffic lights. +Autoware Traffic Light class contains following members: + +- traffic light shape +- light bulbs information of traffic lights +- stopline associated to traffic light + +### Utility + +#### Message Conversion + +This contains functions to convert lanelet map objects into ROS messages. +Currently it contains following conversions: + +- lanelet::LaneletMapPtr to/from autoware_auto_mapping_msgs::msg::HADMapBin +- lanelet::Point3d to geometry_msgs::Point +- lanelet::Point2d to geometry_msgs::Point +- lanelet::BasicPoint3d to geometry_msgs::Point + +#### Query + +This module contains functions to retrieve various information from maps. +e.g. crosswalks, trafficlights, stoplines + +#### Utilities + +This module contains other useful functions related to Lanelet. +e.g. matching waypoint with lanelets + +#### Route Checker + +This module contains a function to check the loading route is valid or not. +If it is invalid, puts warning without dying. +The case valid is when the route is created on the same map as the current one. + +### Visualization + +Visualization contains functions to convert lanelet objects into visualization marker messages. +Currently it contains following conversions: + +- lanelet::Lanelet to Triangle Markers +- lanelet::LineString to LineStrip Markers +- TrafficLights to Triangle Markers + +## Nodes + +### lanelet2_extension_sample + +Code for this explains how this lanelet2_extension library is used. +The executable is not meant to do anything. + +### autoware_lanelet2_extension + +This node checks if an .osm file follows the Autoware version of Lanelet2 format. +You can check by running: + +```sh +ros2 run lanelet2_extension autoware_lanelet2_validation --ros-args -p map_file:= +``` diff --git a/tmp/lanelet2_extension/docs/detection_area.png b/tmp/lanelet2_extension/docs/detection_area.png new file mode 100644 index 00000000..0ae10872 Binary files /dev/null and b/tmp/lanelet2_extension/docs/detection_area.png differ diff --git a/tmp/lanelet2_extension/docs/extra_lanelet_subtypes.md b/tmp/lanelet2_extension/docs/extra_lanelet_subtypes.md new file mode 100644 index 00000000..b7b84eb9 --- /dev/null +++ b/tmp/lanelet2_extension/docs/extra_lanelet_subtypes.md @@ -0,0 +1,45 @@ +# Extra Lanelet Subtypes + +## Roadside Lane + +The subtypes for this lanelet classify the outer lanes adjacent to the driving lane.Since the list of lanelet subtypes defined in this [link](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletAndAreaTagging.md) cannot represent the shoulder lane and pedestrian lane described below, two new subtypes are defined.When parking on the street, it is necessary to distinguish between a shoulder lane which can be used by vehicles, and a pedestrian lane which can be used by pedestrians and bicycles.If you park in a shoulder lane, you can use the entire lane for temporary parking, but if you park in a pedestrian lane, you must leave a space of at least 75cm. + +### Road shoulder subtype + +- refers: lanelet with subtype attribute. Subtype explains what the type of roadside it represents. If there is an area outside of this roadside lane that is open to traffic, such as a sidewalk or bike lane, select the road_shoulder subtype. + +![Road shoulder](road_shoulder.svg) + +Sample road shoulder in .osm format is shown below: + +```xml + + + + + + + + + +``` + +### Pedestrian lane subtype + +- refers: lanelet with subtype attribute. Subtype explains what the type of roadside it represents. If there are no passable areas outside of this roadside lane, select the pedestrian_lane subtype. + +![Pedestrian lane](pedestrian_lane.svg) + +Sample pedestrian lane in .osm format is shown below: + +```xml + + + + + + + + + +``` diff --git a/tmp/lanelet2_extension/docs/extra_regulatory_elements.md b/tmp/lanelet2_extension/docs/extra_regulatory_elements.md new file mode 100644 index 00000000..3dae51bb --- /dev/null +++ b/tmp/lanelet2_extension/docs/extra_regulatory_elements.md @@ -0,0 +1,63 @@ +# Extra Regulatory Elements + +## 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. + +- refers: refers to detection area polygon. There could be multiple detection areas registered to a single regulatory element. +- ref_line: refers to stop line of the detection area + +![Detection area](detection_area.png) + +Sample detection area in .osm format is shown below: + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +``` + +## Road Marking + +This regulatory element specifies related road markings to a lanelet as shown below. + +\* Note that the stopline in the image is for stoplines that are for reference, and normal stoplines should be expressed using TrafficSign regulatory element. + +refers: linestring with type attribute. Type explains what road marking it represents (e.g. stopline). + +![Road marking](road_mark.png) diff --git a/tmp/lanelet2_extension/docs/lanelet2_format_extension.md b/tmp/lanelet2_extension/docs/lanelet2_format_extension.md new file mode 100644 index 00000000..6a206d22 --- /dev/null +++ b/tmp/lanelet2_extension/docs/lanelet2_format_extension.md @@ -0,0 +1,186 @@ +# Modifying Lanelet2 format for Autoware + +## Overview + +About the basics of the default format, please refer to main [Lanelet2 repository](https://github.com/fzi-forschungszentrum-informatik/Lanelet2). (see [here](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletPrimitives.md) about primitives) + +In addition to default Lanelet2 Format, users should add following mandatory/optional tags to their osm lanelet files as explained in reset of this document. +Users may use `autoware_lanelet2_validation` [node](../README.md#nodes) to check if their maps are valid. + +The following is the extra format added for Autoware: + +- [extra regulatory elements](extra_regulatory_elements.md) + - Detection Area + - Road Marking +- [extra lanelet subtype](extra_lanelet_subtypes.md) + - Roadside Lane + +## Mandatory Tags + +### Elevation Tags + +Elevation("ele") information for points(`node`) is optional in default Lanelet2 format. +However, some of Autoware packages(e.g. trafficlight_recognizer) need elevation to be included in HD map. Therefore, users must make sure that all points in their osm maps contain elevation tags. + +Here is an example osm syntax for node object. + +```xml + + + +``` + +### TrafficLights + +Default Lanelet2 format uses LineString(`way`) or Polygon class to represent the shape of a traffic light. For Autoware, traffic light objects must be represented only by LineString to avoid confusion, where start point is at bottom left edge and end point is at bottom right edge. Also, "height" tag must be added in order to represent the size in vertical direction (not the position). + +The Following image illustrates how LineString is used to represent shape of Traffic Light in Autoware. +![How LineString is used to represent shape of Traffic Light in Autoware](traffic_light.png) + +Here is an example osm syntax for traffic light object. + +```xml + + + + + + + +``` + +### Turn Directions + +Users must add "turn_direction" tags to lanelets within intersections to indicate vehicle's turning direction. You do not need this tags for lanelets that are not in intersections. If you do not have this tag, Autoware will not be able to light up turning indicators. +This tags only take following values: + +- left +- right +- straight + +Following image illustrates how lanelets should be tagged. + +![Turn Directions: How lanelets should be tagged](turn_direction.png) + +Here is an example of osm syntax for lanelets in intersections. + +```xml + + + + + + + + + + +``` + +## 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. +In such a case, users may use "local_x", "local_y" taggings to express local positions instead of latitude and longitude. +Autoware Osm Parser will overwrite x,y positions with these tags when they are present. +For z values, use "ele" tags as default Lanelet2 Format. +You would still need to fill in lat and lon attributes so that parser does not crush, but their values could be anything. + +Here is example `node` element in osm with "local_x", "local_y" taggings: + +```xml + + + + + + +``` + +### Light Bulbs in Traffic Lights + +Default Lanelet format can only express shape (base + height) of traffic lights. +However, region_tlr node in Autoware uses positions of each light bulbs to recognize color of traffic light. If users may wish to use this node, "light_bulbs"(`way`) element must be registered to traffic_light regulatory_element object define position and color of each light bulb in traffic lights. If you are using other trafficlight_recognizer nodes(e.g. tlr_mxnet), which only uses bounding box of traffic light, then you do not need to add this object. + +"light_bulbs" object is defined using LineString(`way`), and each node of line string is placed at the center of each light bulb. Also, each node should have "color" and optionally "arrow" tags to describe its type. Also, "traffic_light_id" tag is used to indicate which ID of relevant traffic_light element. + +"color" tag is used to express the color of the light bulb. Currently only three values are used: + +- red +- yellow +- green + +"arrow" tag is used to express the direction of the arrow of light bulb: + +- up +- right +- left +- up_right +- up_left + +Following image illustrates how "light_bulbs" LineString should be created. + +![How "light_bulbs" LineString should be created](light_bulbs.png) + +Here is an example of osm syntax for light_bulb object: + +```xml + + + + + + + + + + + + + + + + + + + + + + + + +``` + +After creating "light_bulbs" elements, you have to register them to traffic_light regulatory element as role "light_bulbs". +The following illustrates how light_bulbs are registered to traffic_light regulatory elements. + +![How light_bulbs are registered to traffic_light regulatory elements](traffic_light_regulatory_element.png) + +```xml + + + + + + + +``` diff --git a/tmp/lanelet2_extension/docs/light_bulbs.png b/tmp/lanelet2_extension/docs/light_bulbs.png new file mode 100644 index 00000000..dc1e6e14 Binary files /dev/null and b/tmp/lanelet2_extension/docs/light_bulbs.png differ diff --git a/tmp/lanelet2_extension/docs/pedestrian_lane.svg b/tmp/lanelet2_extension/docs/pedestrian_lane.svg new file mode 100644 index 00000000..526506ad --- /dev/null +++ b/tmp/lanelet2_extension/docs/pedestrian_lane.svg @@ -0,0 +1 @@ +
type: lanelet
subtype: pedestrian_lane
type: lanelet subtype: p...
Unaccessible area
(e.g. fences, plants, trees, etc.)
Unaccessible area...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/tmp/lanelet2_extension/docs/road_mark.png b/tmp/lanelet2_extension/docs/road_mark.png new file mode 100644 index 00000000..98b41025 Binary files /dev/null and b/tmp/lanelet2_extension/docs/road_mark.png differ diff --git a/tmp/lanelet2_extension/docs/road_shoulder.svg b/tmp/lanelet2_extension/docs/road_shoulder.svg new file mode 100644 index 00000000..40231247 --- /dev/null +++ b/tmp/lanelet2_extension/docs/road_shoulder.svg @@ -0,0 +1 @@ +
type: lanelet
subtype: road_shoulder
type: lanelet subtype: r...
Accessible area
(e.g. sidewalks, bike lanes, etc.)
Accessible area...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/tmp/lanelet2_extension/docs/traffic_light.png b/tmp/lanelet2_extension/docs/traffic_light.png new file mode 100644 index 00000000..aa97c7bc Binary files /dev/null and b/tmp/lanelet2_extension/docs/traffic_light.png differ diff --git a/tmp/lanelet2_extension/docs/traffic_light_regulatory_element.png b/tmp/lanelet2_extension/docs/traffic_light_regulatory_element.png new file mode 100644 index 00000000..17b75677 Binary files /dev/null and b/tmp/lanelet2_extension/docs/traffic_light_regulatory_element.png differ diff --git a/tmp/lanelet2_extension/docs/turn_direction.png b/tmp/lanelet2_extension/docs/turn_direction.png new file mode 100644 index 00000000..3fd63585 Binary files /dev/null and b/tmp/lanelet2_extension/docs/turn_direction.png differ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp new file mode 100644 index 00000000..8645ddbf --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp @@ -0,0 +1,63 @@ +// Copyright 2015-2019 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. +// +// Authors: Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_ +#define LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include + +#include +#include + +namespace lanelet::io_handlers +{ +class AutowareOsmParser : public OsmParser +{ +public: + using OsmParser::OsmParser; + + /** + * [parse parse osm file to laneletMap. It is generally same as default + * OsmParser, but it will overwrite x and y value with local_x and local_y + * tags if present] + * @param filename [path to osm file] + * @param errors [any errors caught during parsing] + * @return [returns LaneletMap] + */ + std::unique_ptr parse( + const std::string & filename, ErrorMessages & errors) const override; + + /** + * [parseVersions parses MetaInfo tags from osm file] + * @param filename [path to osm file] + * @param format_version [parsed information about map format version] + * @param map_version [parsed information about map version] + */ + static void parseVersions( + const std::string & filename, std::string * format_version, std::string * map_version); + + static constexpr const char * extension() { return ".osm"; } + + static constexpr const char * name() { return "autoware_osm_handler"; } +}; + +} // namespace lanelet::io_handlers + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp new file mode 100644 index 00000000..37b6bc07 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp @@ -0,0 +1,116 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_ +#define LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +#include +#include + +namespace lanelet::projection +{ +class MGRSProjector : public Projector +{ +public: + explicit MGRSProjector(Origin origin = Origin({0.0, 0.0})); + + /** + * [MGRSProjector::forward projects gps lat/lon to MGRS 100km grid] + * @param gps [point with latitude longitude information] + * @return [projected point in MGRS coordinate] + */ + BasicPoint3d forward(const GPSPoint & gps) const override; + + /** + * [MGRSProjector::forward projects gps lat/lon to MGRS xyz coordinate] + * @param gps [point with latitude longitude information] + * @param precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m, + * 4=10m, 5=1m] + * @return [projected point in MGRS coordinate] + */ + BasicPoint3d forward(const GPSPoint & gps, const int precision) const; + + /** + * [MGRSProjector::reverse projects point within MGRS 100km grid into gps + * lat/lon (WGS84)] + * @param mgrs_point [3d point in MGRS 100km grid] + * @return [projected point in WGS84] + */ + GPSPoint reverse(const BasicPoint3d & mgrs) const override; + + /** + * [MGRSProjector::reverse projects point within MGRS grid into gps lat/lon + * (WGS84)] + * @param mgrs_point [3d point in MGRS grid] + * @param mgrs_code [MGRS grid code] + * @return [projected point in WGS84] + */ + static GPSPoint reverse(const BasicPoint3d & mgrs_point, const std::string & mgrs_code); + + /** + * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection] + * @param mgrs_code [MGRS code. Minimum requirement is GZD and 100 km Grid + * Square ID. e.g. "4QFJ"] + */ + void setMGRSCode(const std::string & mgrs_code); + + /** + * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection from + * gps lat/lon values] + * @param gps [gps point used to find GMRS Grid] + * @param precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m, + * 4=10m, 5=1m] + */ + void setMGRSCode(const GPSPoint & gps, const int precision = 0); + + /** + * [getProjectedMGRSGrid returns mgrs] + * @return [description] + */ + std::string getProjectedMGRSGrid() const { return projected_grid_; } + + /** + * [isMGRSCodeSet checks if mgrs code is set for reverse projection] + * @return [true if mgrs_code member is set] + */ + bool isMGRSCodeSet() const { return !mgrs_code_.empty(); } + +private: + /** + * mgrs grid code used for reverse function + */ + std::string mgrs_code_; + + /** + * mgrs grid code that was last projected in previous forward function. + * reverse function will use this if isMGRSCodeSet() returns false. + */ + mutable std::string projected_grid_; +}; + +} // namespace lanelet::projection + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp new file mode 100644 index 00000000..ba1da38e --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp @@ -0,0 +1,90 @@ +// Copyright 2015-2019 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. +// +// Authors: Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +namespace lanelet::autoware +{ +struct AutowareRoleNameString +{ + static constexpr const char LightBulbs[] = "light_bulbs"; +}; + +class AutowareTrafficLight : public lanelet::TrafficLight +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "traffic_light"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine = {}, const LineStrings3d & lightBulbs = {}) + { + return Ptr{new AutowareTrafficLight(id, attributes, trafficLights, stopLine, lightBulbs)}; + } + + /** + * @brief get the relevant traffic light bulbs + * @return the traffic light bulbs + * + * There might be multiple traffic light bulbs but they are required to show + * the same signal. + */ + [[nodiscard]] ConstLineStrings3d lightBulbs() const; + + /** + * @brief add a new traffic light bulb + * @param primitive the traffic light bulb to add + * + * Traffic light bulbs are represented as linestrings with each point + * expressing position of each light bulb (lamp). + */ + void addLightBulbs(const LineStringOrPolygon3d & primitive); + + /** + * @brief remove a traffic light bulb + * @param primitive the primitive + * @return true if the traffic light bulb existed and was removed + */ + bool removeLightBulbs(const LineStringOrPolygon3d & 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; + AutowareTrafficLight( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine, const LineStrings3d & lightBulbs); + explicit AutowareTrafficLight(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regAutowareTraffic; + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp new file mode 100644 index 00000000..4af1ef23 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp @@ -0,0 +1,96 @@ +// Copyright 2015-2019 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. +// +// Authors: Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +namespace lanelet::autoware +{ +class DetectionArea : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "detection_area"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine) + { + return Ptr{new DetectionArea(id, attributes, detectionAreas, stopLine)}; + } + + /** + * @brief get the relevant detection_areas + * @return detection_areas + */ + [[nodiscard]] ConstPolygons3d detectionAreas() const; + [[nodiscard]] Polygons3d detectionAreas(); + + /** + * @brief add a new detection area + * @param primitive detection area to add + */ + void addDetectionArea(const Polygon3d & primitive); + + /** + * @brief remove a detection area + * @param primitive the primitive + * @return true if the detection area existed and was removed + */ + bool removeDetectionArea(const Polygon3d & primitive); + + /** + * @brief get the stop line for the detection area + * @return the stop line as LineString + */ + [[nodiscard]] ConstLineString3d stopLine() const; + [[nodiscard]] LineString3d stopLine(); + + /** + * @brief set a new stop line, overwrite the old one + * @param stopLine new stop line + */ + void setStopLine(const LineString3d & stopLine); + + //! Deletes the stop line + void removeStopLine(); + +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; + DetectionArea( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine); + explicit DetectionArea(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regDetectionArea; + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp new file mode 100644 index 00000000..156b190c --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp @@ -0,0 +1,94 @@ +// Copyright 2021 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_STOPPING_AREA_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +namespace lanelet::autoware +{ +class NoStoppingArea : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "no_stopping_area"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make( + Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_areas, + const Optional & stopLine = {}) + { + return Ptr{new NoStoppingArea(id, attributes, no_stopping_areas, stopLine)}; + } + + /** + * @brief get the relevant no stopping area + * @return no stopping area + */ + [[nodiscard]] ConstPolygons3d noStoppingAreas() const; + [[nodiscard]] Polygons3d noStoppingAreas(); + + /** + * @brief add a new no stopping area + * @param primitive no stopping area to add + */ + void addNoStoppingArea(const Polygon3d & primitive); + + /** + * @brief remove a no stopping area + * @param primitive the primitive + * @return true if the no stopping area existed and was removed + */ + bool removeNoStoppingArea(const Polygon3d & primitive); + + /** + * @brief get the stop line for the no stopping area + * @return the stop line as LineString + */ + [[nodiscard]] Optional stopLine() const; + [[nodiscard]] Optional stopLine(); + + /** + * @brief set a new stop line, overwrite the old one + * @param stopLine new stop line + */ + void setStopLine(const LineString3d & stopLine); + + //! Deletes the stop line + void removeStopLine(); + +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; + NoStoppingArea( + Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_area, + const Optional & stopLine); + explicit NoStoppingArea(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regNoStoppingArea; + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp new file mode 100644 index 00000000..f2241399 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp @@ -0,0 +1,72 @@ +// Copyright 2020 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__ROAD_MARKING_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +namespace lanelet::autoware +{ +class RoadMarking : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "road_marking"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make(Id id, const AttributeMap & attributes, const LineString3d & road_marking) + { + return Ptr{new RoadMarking(id, attributes, road_marking)}; + } + + /** + * @brief get the relevant road marking + * @return road marking + */ + [[nodiscard]] ConstLineString3d roadMarking() const; + [[nodiscard]] LineString3d roadMarking(); + + /** + * @brief add a new road marking + * @param primitive road marking to add + */ + void setRoadMarking(const LineString3d & road_marking); + + /** + * @brief remove a road marking + */ + void removeRoadMarking(); + +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; + RoadMarking(Id id, const AttributeMap & attributes, const LineString3d & road_marking); + explicit RoadMarking(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regRoadMarking; + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp new file mode 100644 index 00000000..e0c7b8d9 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp @@ -0,0 +1,82 @@ +// Copyright 2021 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__VIRTUAL_TRAFFIC_LIGHT_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +namespace lanelet::autoware +{ +class VirtualTrafficLight : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + static constexpr char RuleName[] = "virtual_traffic_light"; + + static Ptr make( + const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light) + { + return Ptr{new VirtualTrafficLight(id, attributes, virtual_traffic_light)}; + } + + [[nodiscard]] ConstLineString3d getVirtualTrafficLight() const + { + return getParameters(RoleName::Refers).front(); + } + + [[nodiscard]] Optional getStopLine() const + { + const auto vec = getParameters(RoleName::RefLine); + if (vec.empty()) { + return {}; + } + return vec.front(); + } + + [[nodiscard]] ConstLineString3d getStartLine() const + { + return getParameters("start_line").front(); + } + + [[nodiscard]] ConstLineStrings3d getEndLines() const + { + return getParameters("end_line"); + } + +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; + + VirtualTrafficLight( + const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light); + + explicit VirtualTrafficLight(const lanelet::RegulatoryElementDataPtr & data); +}; + +static lanelet::RegisterRegulatoryElement regVirtualTrafficLight; + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp new file mode 100644 index 00000000..bd128141 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp @@ -0,0 +1,98 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_ +#define LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include +#include +#include + +#include +#include +#include + +namespace lanelet::utils::conversion +{ +/** + * [toBinMsg converts lanelet2 map to ROS message. Similar implementation to + * lanelet::io_handlers::BinHandler::write()] + * @param map [lanelet map data] + * @param msg [converted ROS message. Only "data" field is filled] + */ +void toBinMsg(const lanelet::LaneletMapPtr & map, autoware_auto_mapping_msgs::msg::HADMapBin * msg); + +/** + * [fromBinMsg converts ROS message into lanelet2 data. Similar implementation + * to lanelet::io_handlers::BinHandler::parse()] + * @param msg [ROS message for lanelet map] + * @param map [Converted lanelet2 data] + */ +void fromBinMsg(const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map); +void fromBinMsg( + const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map, + lanelet::traffic_rules::TrafficRulesPtr * traffic_rules, + lanelet::routing::RoutingGraphPtr * routing_graph); + +/** + * [toGeomMsgPt converts various point types to geometry_msgs point] + * @param src [input point(geometry_msgs::msg::Point3, + * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ] + * @param dst [converted geometry_msgs point] + */ +void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst); +void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst); +void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst); +void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst); + +/** + * [toGeomMsgPt converts various point types to geometry_msgs point] + * @param src [input point(geometry_msgs::msg::Point3, + * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ] + * @return [converted geometry_msgs point] + */ +geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src); +geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src); +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src); +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src); + +lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src); +void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst); + +/** + * [toGeomMsgPoly converts lanelet polygon to geometry_msgs polygon] + * @param ll_poly [input polygon] + * @param geom_poly [converted geometry_msgs point] + */ +void toGeomMsgPoly( + const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly); + +/** + * [toGeomMsgPt32 converts Eigen::Vector3d(lanelet:BasicPoint3d to + * geometry_msgs::msg::Point32)] + * @param src [input point] + * @param dst [converted point] + */ +void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst); + +} // namespace lanelet::utils::conversion + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp new file mode 100644 index 00000000..43fa5ef2 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -0,0 +1,262 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__UTILITY__QUERY_HPP_ +#define LANELET2_EXTENSION__UTILITY__QUERY_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "lanelet2_extension/regulatory_elements/detection_area.hpp" +#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace lanelet +{ +using TrafficSignConstPtr = std::shared_ptr; +using TrafficLightConstPtr = std::shared_ptr; +using AutowareTrafficLightConstPtr = std::shared_ptr; +using DetectionAreaConstPtr = std::shared_ptr; +using NoStoppingAreaConstPtr = std::shared_ptr; +} // namespace lanelet + +namespace lanelet::utils::query +{ +/** + * [laneletLayer converts laneletLayer into lanelet vector] + * @param ll_Map [input lanelet map] + * @return [all lanelets in the map] + */ +lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapConstPtr & ll_Map); + +/** + * [subtypeLanelets extracts Lanelet that has given subtype attribute] + * @param lls [input lanelets with various subtypes] + * @param subtype [subtype of lanelets to be retrieved (e.g. + * lanelet::AttributeValueString::Road)] + * @return [lanelets with given subtype] + */ +lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets & lls, const char subtype[]); + +/** + * [crosswalkLanelets extracts crosswalk lanelets] + * @param lls [input lanelets with various subtypes] + * @return [crosswalk lanelets] + */ +lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets & lls); +lanelet::ConstLanelets walkwayLanelets(const lanelet::ConstLanelets & lls); + +/** + * [roadLanelets extracts road lanelets] + * @param lls [input lanelets with subtype road] + * @return [road lanelets] + */ +lanelet::ConstLanelets roadLanelets(const lanelet::ConstLanelets & lls); + +/** + * [shoulderLanelets extracts shoulder lanelets] + * @param lls [input lanelets with subtype shoulder] + * @return [shoulder lanelets] + */ +lanelet::ConstLanelets shoulderLanelets(const lanelet::ConstLanelets & lls); +/** + * [trafficLights extracts Traffic Light regulatory element from lanelets] + * @param lanelets [input lanelets] + * @return [traffic light that are associated with input lanelets] + */ +std::vector trafficLights(const lanelet::ConstLanelets & lanelets); + +/** + * [autowareTrafficLights extracts Autoware Traffic Light regulatory element + * from lanelets] + * @param lanelets [input lanelets] + * @return [autoware traffic light that are associated with input + * lanelets] + */ +std::vector autowareTrafficLights( + const lanelet::ConstLanelets & lanelets); + +/** + * [detectionAreas extracts Detection Area regulatory elements from lanelets] + * @param lanelets [input lanelets] + * @return [detection areas that are associated with input lanelets] + */ +std::vector detectionAreas(const lanelet::ConstLanelets & lanelets); + +/** + * [noStoppingArea extracts NoStopping Area regulatory elements from lanelets] + * @param lanelets [input lanelets] + * @return [no stopping areas that are associated with input lanelets] + */ +std::vector noStoppingAreas( + const lanelet::ConstLanelets & lanelets); + +// query all obstacle polygons in lanelet2 map +lanelet::ConstPolygons3d getAllObstaclePolygons( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all parking lots in lanelet2 map +lanelet::ConstPolygons3d getAllParkingLots(const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all partitions in lanelet2 map +lanelet::ConstLineStrings3d getAllPartitions(const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all pedestrian markings in lanelet2 map +lanelet::ConstLineStrings3d getAllPedestrianMarkings( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all parking spaces in lanelet2 map +lanelet::ConstLineStrings3d getAllParkingSpaces( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query linked parking spaces from lanelet +lanelet::ConstLineStrings3d getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr & lanelet_map_ptr); +lanelet::ConstLineStrings3d getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, + const lanelet::ConstPolygons3d & all_parking_lots); +// query linked lanelets from parking space +bool getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet); +bool getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet); +lanelet::ConstLanelets getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots); +lanelet::ConstLanelets getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// get linked parking lot from lanelet +bool getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot); +// get linked parking lot from current pose of ego car +bool getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot); +// get linked parking lot from parking space +bool getLinkedParkingLot( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot); + +// query linked parking space from parking lot +lanelet::ConstLineStrings3d getLinkedParkingSpaces( + const lanelet::ConstPolygon3d & parking_lot, + const lanelet::ConstLineStrings3d & all_parking_spaces); +// query linked lanelets from parking lot +lanelet::ConstLanelets getLinkedLanelets( + const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets); + +/** + * [stopLinesLanelets extracts stoplines that are associated to lanelets] + * @param lanelets [input lanelets] + * @return [stop lines that are associated with input lanelets] + */ +std::vector stopLinesLanelets(const lanelet::ConstLanelets & lanelets); + +/** + * [stopLinesLanelet extracts stop lines that are associated with a given + * lanelet] + * @param ll [input lanelet] + * @return [stop lines that are associated with input lanelet] + */ +std::vector stopLinesLanelet(const lanelet::ConstLanelet & ll); + +/** + * [stopSignStopLines extracts stoplines that are associated with stop signs] + * @param lanelets [input lanelets] + * @param stop_sign_id [sign id of stop sign] + * @return [array of stoplines] + */ +std::vector stopSignStopLines( + const lanelet::ConstLanelets & lanelets, const std::string & stop_sign_id = "stop_sign"); + +ConstLanelets getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point, + const double range); +ConstLanelets getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, + const double range); + +ConstLanelets getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point); + +ConstLanelets getAllNeighbors(const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getAllNeighborsLeft( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getAllNeighborsRight( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getAllNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point); + +bool getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelet * closest_lanelet_ptr); + +bool getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelets * current_lanelets_ptr); + +/** + * [getSucceedingLaneletSequences retrieves a sequence of lanelets after the given lanelet. + * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence + * does not include input lanelet.] + * @param graph [input lanelet routing graph] + * @param lanelet [input lanelet] + * @param length [minimum length of retrieved lanelet sequence] + * @return [lanelet sequence that follows given lanelet] + */ +std::vector getSucceedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length); + +/** + * [getPrecedingLaneletSequences retrieves a sequence of lanelets before the given lanelet. + * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence + * does not include input lanelet.] + * @param graph [input lanelet routing graph] + * @param lanelet [input lanelet] + * @param length [minimum length of retrieved lanelet sequence] + * @return [lanelet sequence that leads to given lanelet] + */ +std::vector getPrecedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length, const lanelet::ConstLanelets & exclude_lanelets = {}); + +} // namespace lanelet::utils::query + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__UTILITY__QUERY_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp new file mode 100644 index 00000000..572d9e3c --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp @@ -0,0 +1,36 @@ +// Copyright 2022 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__UTILITY__ROUTE_CHECKER_HPP_ +#define LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include +#include + +#include +#include + +namespace lanelet::utils::route +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_planning_msgs::msg::HADMapRoute; + +bool isRouteValid(const HADMapRoute & route, const lanelet::LaneletMapPtr lanelet_map_ptr_); +} // namespace lanelet::utils::route + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp new file mode 100644 index 00000000..a31d9d69 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -0,0 +1,91 @@ +// Copyright 2015-2019 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. +// +// Authors: Kenji Miyake, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_ +#define LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include + +#include +#include + +#include +#include + +#include + +namespace lanelet::utils +{ +lanelet::LineString3d generateFineCenterline( + const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0); +lanelet::ConstLineString3d getCenterlineWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); +lanelet::ConstLineString3d getRightBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); +lanelet::ConstLineString3d getLeftBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); + +lanelet::ConstLanelet getExpandedLanelet( + const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset); + +lanelet::ConstLanelets getExpandedLanelets( + const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset); + +/** + * @brief Apply a patch for centerline because the original implementation + * doesn't have enough quality + */ +void overwriteLaneletsCenterline( + lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0, + const bool force_overwrite = false); + +lanelet::ConstLanelets getConflictingLanelets( + const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet); + +bool lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); + +bool lineStringToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); + +double getLaneletLength2d(const lanelet::ConstLanelet & lanelet); +double getLaneletLength3d(const lanelet::ConstLanelet & lanelet); +double getLaneletLength2d(const lanelet::ConstLanelets & lanelet_sequence); +double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence); + +lanelet::ArcCoordinates getArcCoordinates( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose); + +lanelet::ConstLineString3d getClosestSegment( + const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring); + +lanelet::CompoundPolygon3d getPolygonFromArcLength( + const lanelet::ConstLanelets & lanelets, const double s1, const double s2); +double getLaneletAngle( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point); +bool isInLanelet( + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet, + const double radius = 0.0); +geometry_msgs::msg::Pose getClosestCenterPose( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point); + +} // namespace lanelet::utils + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp new file mode 100644 index 00000000..822d89f0 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -0,0 +1,277 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_ +#define LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" +#include "lanelet2_extension/utility/query.hpp" + +#include + +#include +#include + +#include +#include + +#include +#include + +namespace lanelet::visualization +{ +/** + * [lanelet2Triangle converts lanelet into vector of triangles. Used for + * triangulation] + * @param ll [input lanelet] + * @param triangles [array of polygon message, each containing 3 vertices] + */ +void lanelet2Triangle( + const lanelet::ConstLanelet & ll, std::vector * triangles); + +/** + * [polygon2Triangle converts polygon into vector of triangles] + * @param polygon [input polygon] + * @param triangles [array of polygon message, each containing 3 vertices] + */ +void polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon, + std::vector * triangles); + +/** + * [lanelet2Polygon converts lanelet into a polygon] + * @param ll [input lanelet] + * @param polygon [polygon message containing shape of the input lanelet.] + */ +void lanelet2Polygon(const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon); + +/** + * [initLineStringMarker initializes marker to visualize shape of linestring] + * @param marker [output marker message] + * @param frame_id [frame id of the marker] + * @param ns [namespace of the marker] + * @param c [color of the marker] + */ +void initLineStringMarker( + visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c); + +/** + * [pushLineStringMarker pushes marker vertices to visualize shape of linestring] + * @param marker [output marker message] + * @param ls [input linestring] + * @param c [color of the marker] + * @param lss [thickness of the marker] + */ +void pushLineStringMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & c, const float lss = 0.1); + +/** + * [initArrowsMarker initializes marker to visualize arrows with TRIANGLE_LIST] + * @param marker [output marker message] + * @param frame_id [frame id of the marker] + * @param ns [namespace of the marker] + * @param c [color of the marker] + */ +void initArrowsMarker( + visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c); + +/** + * [pushArrowsMarker pushes marker to visualize arrows with TRIANGLE_LIST] + * @param marker [output marker message] + * @param ls [input linestring] + * @param c [color of the marker] + */ +void pushArrowsMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & c); + +/** + * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic + * lights] + * @param marker [created marker] + * @param ns [namespace of the marker] + * @param duration [lifetime of the marker] + */ +void initTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const std::string & ns, + const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); + +/** + * [pushTrafficLightTriangleMarker pushes marker vertices to visualize shape of traffic + * lights] + * @param marker [created marker] + * @param ls [linestring that represents traffic light shape] + * @param cl [color of the marker] + * @param scale [scale of the marker] + */ +void pushTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & cl, const double scale = 1.0); + +/** + * [laneletsBoundaryAsMarkerArray create marker array to visualize shape of + * boundaries of lanelets] + * @param lanelets [input lanelets] + * @param c [color of the boundary] + * @param viz_centerline [flag to visualize centerline or not] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c, + const bool viz_centerline, const std::string & additional_namespace = ""); +/** + * [laneletsAsTriangleMarkerArray create marker array to visualize shape of the + * lanelet] + * @param ns [namespace of the marker] + * @param lanelets [input lanelets] + * @param c [color of the marker] + * @return [created marker] + */ +visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray( + const std::string & ns, const lanelet::ConstLanelets & lanelets, + const std_msgs::msg::ColorRGBA & c); + +/** + * [laneletDirectionAsMarkerArray create marker array to visualize direction of + * the lanelet] + * @param lanelets [input lanelets] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray laneletDirectionAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace = ""); + +/** + * [lineStringsAsMarkerArray creates marker array to visualize shape of + * linestrings] + * @param line_strings [input linestrings] + * @param name_space [namespace of the marker] + * @param c [color of the marker] + * @param lss [thickness of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray( + const std::vector & line_strings, const std::string & name_space, + const std_msgs::msg::ColorRGBA & c, const float lss); + +/** + * [autowareTrafficLightsAsMarkerArray creates marker array to visualize traffic + * lights] + * @param tl_reg_elems [traffic light regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0), + const double scale = 1.0); + +/** + * [generateTrafficLightIdMaker creates marker array to visualize traffic id + * lights] + * @param tl_reg_elems [traffic light regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray generateTrafficLightIdMaker( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0), + const double scale = 1.0); + +/** + * [trafficLightsAsTriangleMarkerArray creates marker array to visualize shape + * of traffic lights] + * @param tl_reg_elems [traffic light regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray trafficLightsAsTriangleMarkerArray( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0), + const double scale = 1.0); + +/** + * [detectionAreasAsMarkerArray creates marker array to visualize detection areas] + * @param da_reg_elems [detection area regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + */ +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)); + +/** + * [noStoppingAreasAsMarkerArray creates marker array to visualize detection areas] + * @param no_reg_elems [mp stopping area regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + */ +visualization_msgs::msg::MarkerArray noStoppingAreasAsMarkerArray( + const std::vector & no_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); + +/** + * [pedestrianMarkingsAsMarkerArray creates marker array to visualize pedestrian markings] + * @param pedestrian_markings [pedestrian marking polygon] + * @param c [color of the marker] + */ +visualization_msgs::msg::MarkerArray pedestrianMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c); + +/** + * [parkingLotsAsMarkerArray creates marker array to visualize parking lots] + * @param parking_lots [parking lot polygon] + * @param c [color of the marker] + */ +visualization_msgs::msg::MarkerArray parkingLotsAsMarkerArray( + const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c); + +/** + * [parkingSpacesAsMarkerArray creates marker array to visualize parking spaces] + * @param parking_spaces [parking space line string] + * @param c [color of the marker] + */ +visualization_msgs::msg::MarkerArray parkingSpacesAsMarkerArray( + const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c); + +/** + * [detectionAreasAsMarkerArray creates marker array to visualize lanelet_id] + * @param road_lanelets [road lanelets] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @param scale [scale of the marker] + * @return visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray generateLaneletIdMarker( + const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c, + const double scale = 0.5); + +visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray( + const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c); + +} // namespace lanelet::visualization + +// NOLINTEND(readability-identifier-naming) + +#endif // LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_ diff --git a/tmp/lanelet2_extension/lib/autoware_osm_parser.cpp b/tmp/lanelet2_extension/lib/autoware_osm_parser.cpp new file mode 100644 index 00000000..2a74f361 --- /dev/null +++ b/tmp/lanelet2_extension/lib/autoware_osm_parser.cpp @@ -0,0 +1,90 @@ +// Copyright 2015-2019 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. +// +// Authors: Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/io/autoware_osm_parser.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace lanelet::io_handlers +{ +std::unique_ptr AutowareOsmParser::parse( + const std::string & filename, ErrorMessages & errors) const +{ + auto map = OsmParser::parse(filename, errors); + + // overwrite x and y values if there are local_x, local_y tags + for (Point3d point : map->pointLayer) { + if (point.hasAttribute("local_x")) { + point.x() = point.attribute("local_x").asDouble().value(); + } + if (point.hasAttribute("local_y")) { + point.y() = point.attribute("local_y").asDouble().value(); + } + } + + // rerun align function in just in case + for (Lanelet & lanelet : map->laneletLayer) { + LineString3d new_left; + LineString3d new_right; + std::tie(new_left, new_right) = geometry::align(lanelet.leftBound(), lanelet.rightBound()); + lanelet.setLeftBound(new_left); + lanelet.setRightBound(new_right); + } + + return map; +} + +namespace +{ +RegisterParser regParser; +} // namespace + +void AutowareOsmParser::parseVersions( + const std::string & filename, std::string * format_version, std::string * map_version) +{ + if (format_version == nullptr || map_version == nullptr) { + std::cerr << __FUNCTION__ << ": either format_version or map_version is null pointer!"; + return; + } + + pugi::xml_document doc; + auto result = doc.load_file(filename.c_str()); + if (!result) { + throw lanelet::ParseError( + std::string("Errors occurred while parsing osm file: ") + result.description()); + } + + auto osmNode = doc.child("osm"); + auto metainfo = osmNode.child("MetaInfo"); + if (metainfo.attribute("format_version")) { + *format_version = metainfo.attribute("format_version").value(); + } + if (metainfo.attribute("map_version")) { + *map_version = metainfo.attribute("map_version").value(); + } +} + +} // namespace lanelet::io_handlers + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/autoware_traffic_light.cpp b/tmp/lanelet2_extension/lib/autoware_traffic_light.cpp new file mode 100644 index 00000000..63d185b6 --- /dev/null +++ b/tmp/lanelet2_extension/lib/autoware_traffic_light.cpp @@ -0,0 +1,146 @@ +// Copyright 2015-2019 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. +// +// Authors: Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.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 +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return static_cast(elem); }; + return utils::transform(primitives, cast_func); +} + +template <> +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return elem.asRuleParameter(); }; + return utils::transform(primitives, cast_func); +} + +LineStringsOrPolygons3d getLsOrPoly(const RuleParameterMap & paramsMap, RoleName role) +{ + auto params = paramsMap.find(role); + if (params == paramsMap.end()) { + return {}; + } + LineStringsOrPolygons3d result; + for (auto & param : params->second) { + auto l = boost::get(¶m); + if (l != nullptr) { + result.push_back(*l); + } + auto p = boost::get(¶m); + if (p != nullptr) { + result.push_back(*p); + } + } + return result; +} + +[[maybe_unused]] ConstLineStringsOrPolygons3d getConstLsOrPoly( + const RuleParameterMap & params, RoleName role) +{ + auto cast_func = [](auto & lsOrPoly) { + return static_cast(lsOrPoly); + }; + return utils::transform(getLsOrPoly(params, role), cast_func); +} + +[[maybe_unused]] RegulatoryElementDataPtr constructAutowareTrafficLightData( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine, const LineStrings3d & lightBulbs) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(trafficLights)}}; + + if (!!stopLine) { + RuleParameters rule_parameters = {*stopLine}; + rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters)); + } + if (!lightBulbs.empty()) { + rpm.insert(std::make_pair(AutowareRoleNameString::LightBulbs, toRuleParameters(lightBulbs))); + } + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = AttributeValueString::TrafficLight; + return data; +} +} // namespace + +AutowareTrafficLight::AutowareTrafficLight(const RegulatoryElementDataPtr & data) +: TrafficLight(data) +{ +} + +AutowareTrafficLight::AutowareTrafficLight( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine, const LineStrings3d & lightBulbs) +: TrafficLight(id, attributes, trafficLights, stopLine) +{ + for (const auto & lightBulb : lightBulbs) { + addLightBulbs(lightBulb); + } +} + +ConstLineStrings3d AutowareTrafficLight::lightBulbs() const +{ + return getParameters(AutowareRoleNameString::LightBulbs); +} + +void AutowareTrafficLight::addLightBulbs(const LineStringOrPolygon3d & primitive) +{ + parameters()[AutowareRoleNameString::LightBulbs].emplace_back(primitive.asRuleParameter()); +} + +bool AutowareTrafficLight::removeLightBulbs(const LineStringOrPolygon3d & primitive) +{ + return findAndErase( + primitive.asRuleParameter(), ¶meters().find(AutowareRoleNameString::LightBulbs)->second); +} + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/detection_area.cpp b/tmp/lanelet2_extension/lib/detection_area.cpp new file mode 100644 index 00000000..54a38996 --- /dev/null +++ b/tmp/lanelet2_extension/lib/detection_area.cpp @@ -0,0 +1,154 @@ +// Copyright 2015-2019 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. +// +// Authors: Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/regulatory_elements/detection_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 +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return static_cast(elem); }; + return utils::transform(primitives, cast_func); +} + +// template <> +// RuleParameters toRuleParameters(const std::vector& primitives) +// { +// auto cast_func = [](const auto& elem) { return elem.asRuleParameter(); }; +// 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 constructDetectionAreaData( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(detectionAreas)}}; + + RuleParameters rule_parameters = {stopLine}; + rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters)); + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "detection_area"; + return data; +} +} // namespace + +DetectionArea::DetectionArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) +{ + if (getConstPoly(data->parameters, RoleName::Refers).empty()) { + throw InvalidInputError("No detection area defined!"); + } + if (getParameters(RoleName::RefLine).size() != 1) { + throw InvalidInputError("There must be exactly one stopline defined!"); + } +} + +DetectionArea::DetectionArea( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine) +: DetectionArea(constructDetectionAreaData(id, attributes, detectionAreas, stopLine)) +{ +} + +ConstPolygons3d DetectionArea::detectionAreas() const +{ + return getConstPoly(parameters(), RoleName::Refers); +} +Polygons3d DetectionArea::detectionAreas() { return getPoly(parameters(), RoleName::Refers); } + +void DetectionArea::addDetectionArea(const Polygon3d & primitive) +{ + parameters()["detection_area"].emplace_back(primitive); +} + +bool DetectionArea::removeDetectionArea(const Polygon3d & primitive) +{ + return findAndErase(primitive, ¶meters().find("detection_area")->second); +} + +ConstLineString3d DetectionArea::stopLine() const +{ + return getParameters(RoleName::RefLine).front(); +} + +LineString3d DetectionArea::stopLine() +{ + return getParameters(RoleName::RefLine).front(); +} + +void DetectionArea::setStopLine(const LineString3d & stopLine) +{ + parameters()[RoleName::RefLine] = {stopLine}; +} + +void DetectionArea::removeStopLine() { parameters()[RoleName::RefLine] = {}; } + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/message_conversion.cpp b/tmp/lanelet2_extension/lib/message_conversion.cpp new file mode 100644 index 00000000..ae204fb5 --- /dev/null +++ b/tmp/lanelet2_extension/lib/message_conversion.cpp @@ -0,0 +1,192 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/utility/message_conversion.hpp" + +#include "lanelet2_extension/projection/mgrs_projector.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace lanelet::utils::conversion +{ +void toBinMsg(const lanelet::LaneletMapPtr & map, autoware_auto_mapping_msgs::msg::HADMapBin * msg) +{ + if (msg == nullptr) { + std::cerr << __FUNCTION__ << "msg is null pointer!"; + return; + } + + std::stringstream ss; + boost::archive::binary_oarchive oa(ss); + oa << *map; + auto id_counter = lanelet::utils::getId(); + oa << id_counter; + + std::string data_str(ss.str()); + + msg->data.clear(); + msg->data.assign(data_str.begin(), data_str.end()); +} + +void fromBinMsg(const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map) +{ + if (!map) { + std::cerr << __FUNCTION__ << ": map is null pointer!"; + return; + } + + std::string data_str; + data_str.assign(msg.data.begin(), msg.data.end()); + + std::stringstream ss; + ss << data_str; + boost::archive::binary_iarchive oa(ss); + oa >> *map; + lanelet::Id id_counter = 0; + oa >> id_counter; + lanelet::utils::registerId(id_counter); + // *map = std::move(laneletMap); +} + +void fromBinMsg( + const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map, + lanelet::traffic_rules::TrafficRulesPtr * traffic_rules, + lanelet::routing::RoutingGraphPtr * routing_graph) +{ + fromBinMsg(msg, map); + *traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + *routing_graph = lanelet::routing::RoutingGraph::build(*map, **traffic_rules); +} + +void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!"; + return; + } + dst->x = src.x; + dst->y = src.y; + dst->z = src.z; +} +void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!"; + return; + } + dst->x = src.x(); + dst->y = src.y(); + dst->z = src.z(); +} +void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!"; + return; + } + dst->x = src.x(); + dst->y = src.y(); + dst->z = src.z(); +} +void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!" << std::endl; + return; + } + dst->x = src.x(); + dst->y = src.y(); + dst->z = 0; +} + +void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!" << std::endl; + return; + } + dst->x = static_cast(src.x()); + dst->y = static_cast(src.y()); + dst->z = static_cast(src.z()); +} + +geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} +geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} + +lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src) +{ + lanelet::ConstPoint3d dst; + toLaneletPoint(src, &dst); + return dst; +} + +void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst) +{ + *dst = lanelet::Point3d(lanelet::InvalId, src.x, src.y, src.z); +} + +void toGeomMsgPoly(const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly) +{ + geom_poly->points.clear(); + geom_poly->points.reserve(ll_poly.size()); + for (const auto & ll_pt : ll_poly) { + geometry_msgs::msg::Point32 geom_pt32; + utils::conversion::toGeomMsgPt32(ll_pt.basicPoint(), &geom_pt32); + geom_poly->points.push_back(geom_pt32); + } +} + +} // namespace lanelet::utils::conversion + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/mgrs_projector.cpp b/tmp/lanelet2_extension/lib/mgrs_projector.cpp new file mode 100644 index 00000000..d4e1ce79 --- /dev/null +++ b/tmp/lanelet2_extension/lib/mgrs_projector.cpp @@ -0,0 +1,130 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/projection/mgrs_projector.hpp" + +#include +#include +#include +#include +#include + +namespace lanelet::projection +{ +MGRSProjector::MGRSProjector(Origin origin) : Projector(origin) {} + +BasicPoint3d MGRSProjector::forward(const GPSPoint & gps) const +{ + BasicPoint3d mgrs_point(forward(gps, 0)); + return mgrs_point; +} + +BasicPoint3d MGRSProjector::forward(const GPSPoint & gps, const int precision) const +{ + std::string prev_projected_grid = projected_grid_; + + BasicPoint3d mgrs_point{0., 0., gps.ele}; + BasicPoint3d utm_point{0., 0., gps.ele}; + int zone{}; + bool is_north{}; + std::string mgrs_code; + + try { + GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, is_north, utm_point.x(), utm_point.y()); + GeographicLib::MGRS::Forward( + zone, is_north, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code); + } catch (const GeographicLib::GeographicErr & err) { + std::cerr << err.what() << std::endl; + return mgrs_point; + } + + // get mgrs values from utm values + mgrs_point.x() = fmod(utm_point.x(), 1e5); + mgrs_point.y() = fmod(utm_point.y(), 1e5); + projected_grid_ = mgrs_code; + + if (!prev_projected_grid.empty() && prev_projected_grid != projected_grid_) { + std::cerr << "Projected MGRS Grid changed from last projection. Projected point" + "might be far away from previously projected point." + << std::endl + << "You may want to use different projector."; + } + + return mgrs_point; +} + +GPSPoint MGRSProjector::reverse(const BasicPoint3d & mgrs_point) const +{ + GPSPoint gps{0., 0., 0.}; + // reverse function cannot be used if mgrs_code_ is not set + if (isMGRSCodeSet()) { + gps = reverse(mgrs_point, mgrs_code_); + } else if (!projected_grid_.empty()) { + gps = reverse(mgrs_point, projected_grid_); + } else { + std::cerr << "cannot run reverse operation if mgrs code is not set in projector." << std::endl + << "use setMGRSCode function or explicitly give mgrs code as an argument."; + } + return gps; +} + +GPSPoint MGRSProjector::reverse(const BasicPoint3d & mgrs_point, const std::string & mgrs_code) +{ + GPSPoint gps{0., 0., mgrs_point.z()}; + BasicPoint3d utm_point{0., 0., gps.ele}; + + int zone{}; + int prec{}; + bool is_north{}; + try { + GeographicLib::MGRS::Reverse( + mgrs_code, zone, is_north, utm_point.x(), utm_point.y(), prec, false); + utm_point.x() += fmod(mgrs_point.x(), pow(10, 5 - prec)); + utm_point.y() += fmod(mgrs_point.y(), pow(10, 5 - prec)); + GeographicLib::UTMUPS::Reverse(zone, is_north, utm_point.x(), utm_point.y(), gps.lat, gps.lon); + } catch (const GeographicLib::GeographicErr & err) { + std::cerr << "Failed to convert from MGRS to WGS"; + return gps; + } + + return gps; +} + +void MGRSProjector::setMGRSCode(const std::string & mgrs_code) { mgrs_code_ = mgrs_code; } + +void MGRSProjector::setMGRSCode(const GPSPoint & gps, const int precision) +{ + BasicPoint3d utm_point{0., 0., gps.ele}; + int zone{}; + bool is_north{}; + std::string mgrs_code; + + try { + GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, is_north, utm_point.x(), utm_point.y()); + GeographicLib::MGRS::Forward( + zone, is_north, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code); + } catch (const GeographicLib::GeographicErr & err) { + std::cerr << err.what() << std::endl; + } + + setMGRSCode(mgrs_code); +} + +} // namespace lanelet::projection + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/no_stopping_area.cpp b/tmp/lanelet2_extension/lib/no_stopping_area.cpp new file mode 100644 index 00000000..3d83a03c --- /dev/null +++ b/tmp/lanelet2_extension/lib/no_stopping_area.cpp @@ -0,0 +1,154 @@ +// Copyright 2021 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_stopping_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 constructNoStoppingAreaData( + Id id, const AttributeMap & attributes, const Polygons3d & noStoppingAreas, + const Optional & stopLine = {}) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(noStoppingAreas)}}; + if (!!stopLine) { + rpm.insert({RoleNameString::RefLine, {*stopLine}}); + } + + auto data = std::make_shared(id, std::move(rpm), attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "no_stopping_area"; + return data; +} +} // namespace + +NoStoppingArea::NoStoppingArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) +{ + if (getConstPoly(data->parameters, RoleName::Refers).empty()) { + throw InvalidInputError("no stopping area defined!"); + } + if (getParameters(RoleName::RefLine).size() > 1) { + throw InvalidInputError("There can not exist more than one stop line!"); + } +} + +NoStoppingArea::NoStoppingArea( + Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_areas, + const Optional & stopLine) +: NoStoppingArea(constructNoStoppingAreaData(id, attributes, no_stopping_areas, stopLine)) +{ +} + +ConstPolygons3d NoStoppingArea::noStoppingAreas() const +{ + return getConstPoly(parameters(), RoleName::Refers); +} +Polygons3d NoStoppingArea::noStoppingAreas() { return getPoly(parameters(), RoleName::Refers); } + +void NoStoppingArea::addNoStoppingArea(const Polygon3d & primitive) +{ + parameters()["no_stopping_area"].emplace_back(primitive); +} + +bool NoStoppingArea::removeNoStoppingArea(const Polygon3d & primitive) +{ + return findAndErase(primitive, ¶meters().find("no_stopping_area")->second); +} + +Optional NoStoppingArea::stopLine() const +{ + return tryGetFront(getParameters(RoleName::RefLine)); +} + +Optional NoStoppingArea::stopLine() +{ + return tryGetFront(getParameters(RoleName::RefLine)); +} + +void NoStoppingArea::setStopLine(const LineString3d & stopLine) +{ + parameters()[RoleName::RefLine] = {stopLine}; +} + +void NoStoppingArea::removeStopLine() { parameters()[RoleName::RefLine] = {}; } + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp new file mode 100644 index 00000000..1e5bd8b7 --- /dev/null +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -0,0 +1,884 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/utility/query.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/utilities.hpp" + +#include +#include + +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#else +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#endif + +#include +#include +#include +#include +#include +#include + +using lanelet::utils::to2D; + +namespace lanelet::utils +{ +// returns all lanelets in laneletLayer - don't know how to convert +// PrimitiveLayer -> std::vector +lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map) +{ + lanelet::ConstLanelets lanelets; + if (!ll_map) { + std::cerr << "No map received!"; + return lanelets; + } + + for (const auto & li : ll_map->laneletLayer) { + lanelets.push_back(li); + } + + return lanelets; +} + +lanelet::ConstLanelets query::subtypeLanelets( + const lanelet::ConstLanelets & lls, const char subtype[]) +{ + lanelet::ConstLanelets subtype_lanelets; + + for (const auto & ll : lls) { + if (ll.hasAttribute(lanelet::AttributeName::Subtype)) { + const lanelet::Attribute & attr = ll.attribute(lanelet::AttributeName::Subtype); + if (attr.value() == subtype) { + subtype_lanelets.push_back(ll); + } + } + } + + return subtype_lanelets; +} + +lanelet::ConstLanelets query::crosswalkLanelets(const lanelet::ConstLanelets & lls) +{ + return query::subtypeLanelets(lls, lanelet::AttributeValueString::Crosswalk); +} + +lanelet::ConstLanelets query::walkwayLanelets(const lanelet::ConstLanelets & lls) +{ + return query::subtypeLanelets(lls, lanelet::AttributeValueString::Walkway); +} + +lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets & lls) +{ + return query::subtypeLanelets(lls, lanelet::AttributeValueString::Road); +} + +lanelet::ConstLanelets query::shoulderLanelets(const lanelet::ConstLanelets & lls) +{ + return query::subtypeLanelets(lls, "road_shoulder"); +} + +std::vector query::trafficLights( + const lanelet::ConstLanelets & lanelets) +{ + std::vector tl_reg_elems; + + for (const auto & ll : lanelets) { + std::vector ll_tl_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (const auto & tl_ptr : ll_tl_re) { + lanelet::Id id = tl_ptr->id(); + bool unique_id = true; + for (const auto & tl_reg_elem : tl_reg_elems) { + if (id == tl_reg_elem->id()) { + unique_id = false; + break; + } + } + if (unique_id) { + tl_reg_elems.push_back(tl_ptr); + } + } + } + return tl_reg_elems; +} + +std::vector query::autowareTrafficLights( + const lanelet::ConstLanelets & lanelets) +{ + std::vector tl_reg_elems; + + for (const auto & ll : lanelets) { + std::vector ll_tl_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (const auto & tl_ptr : ll_tl_re) { + lanelet::Id id = tl_ptr->id(); + bool unique_id = true; + + for (const auto & tl_reg_elem : tl_reg_elems) { + if (id == tl_reg_elem->id()) { + unique_id = false; + break; + } + } + + if (unique_id) { + tl_reg_elems.push_back(tl_ptr); + } + } + } + return tl_reg_elems; +} + +std::vector query::detectionAreas( + const lanelet::ConstLanelets & lanelets) +{ + std::vector da_reg_elems; + + for (const auto & ll : lanelets) { + std::vector ll_da_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (const auto & da_ptr : ll_da_re) { + lanelet::Id id = da_ptr->id(); + bool unique_id = true; + + for (const auto & da_reg_elem : da_reg_elems) { + if (id == da_reg_elem->id()) { + unique_id = false; + break; + } + } + + if (unique_id) { + da_reg_elems.push_back(da_ptr); + } + } + } + return da_reg_elems; +} + +std::vector query::noStoppingAreas( + const lanelet::ConstLanelets & lanelets) +{ + std::vector no_reg_elems; + + for (const auto & ll : lanelets) { + std::vector ll_no_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (const auto & no_ptr : ll_no_re) { + lanelet::Id id = no_ptr->id(); + bool unique_id = true; + + for (const auto & no_reg_elem : no_reg_elems) { + if (id == no_reg_elem->id()) { + unique_id = false; + break; + } + } + + if (unique_id) { + no_reg_elems.push_back(no_ptr); + } + } + } + return no_reg_elems; +} + +lanelet::ConstPolygons3d query::getAllObstaclePolygons( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstPolygons3d obstacle_polygons; + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "obstacle") { + obstacle_polygons.push_back(poly); + } + } + return obstacle_polygons; +} + +lanelet::ConstPolygons3d query::getAllParkingLots( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstPolygons3d parking_lots; + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "parking_lot") { + parking_lots.push_back(poly); + } + } + return parking_lots; +} + +lanelet::ConstLineStrings3d query::getAllPartitions( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLineStrings3d partitions; + for (const auto & ls : lanelet_map_ptr->lineStringLayer) { + const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "guard_rail" || type == "fence" || type == "wall") { + partitions.push_back(ls); + } + } + return partitions; +} + +lanelet::ConstLineStrings3d query::getAllPedestrianMarkings( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLineStrings3d pedestrian_markings; + for (const auto & ls : lanelet_map_ptr->lineStringLayer) { + const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "pedestrian_marking") { + pedestrian_markings.push_back(ls); + } + } + return pedestrian_markings; +} + +lanelet::ConstLineStrings3d query::getAllParkingSpaces( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLineStrings3d parking_spaces; + for (const auto & ls : lanelet_map_ptr->lineStringLayer) { + const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "parking_space") { + parking_spaces.push_back(ls); + } + } + return parking_spaces; +} + +bool query::getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet) +{ + const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); + const auto & all_road_lanelets = query::roadLanelets(all_lanelets); + const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + return query::getLinkedLanelet( + parking_space, all_road_lanelets, all_parking_lots, linked_lanelet); +} + +bool query::getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet) +{ + const auto & linked_lanelets = + getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); + if (linked_lanelets.empty()) { + return false; + } + + double min_distance = std::numeric_limits::max(); + for (const auto & lanelet : linked_lanelets) { + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); + if (distance < min_distance) { + *linked_lanelet = lanelet; + min_distance = distance; + } + } + return true; +} + +lanelet::ConstLanelets query::getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); + const auto & all_road_lanelets = query::roadLanelets(all_lanelets); + const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + + return query::getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); +} + +lanelet::ConstLanelets query::getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstLanelets linked_lanelets; + + // get lanelets within same parking lot + lanelet::ConstPolygon3d linked_parking_lot; + if (!getLinkedParkingLot(parking_space, all_parking_lots, &linked_parking_lot)) { + return linked_lanelets; + } + const auto & candidate_lanelets = getLinkedLanelets(linked_parking_lot, all_road_lanelets); + + // get lanelets that are close to parking space and facing to parking space + for (const auto & lanelet : candidate_lanelets) { + // check if parking space is close to lanelet + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); + constexpr double distance_thresh = 5.0; + if (distance > distance_thresh) { + continue; + } + + // check if parking space is facing lanelet + const Eigen::Vector3d direction = + parking_space.back().basicPoint() - parking_space.front().basicPoint(); + const Eigen::Vector3d new_pt = parking_space.front().basicPoint() - direction * distance_thresh; + + const lanelet::Point3d check_line_p1(lanelet::InvalId, new_pt.x(), new_pt.y(), new_pt.z()); + const lanelet::Point3d check_line_p2(lanelet::InvalId, parking_space.back().basicPoint()); + const lanelet::LineString3d check_line(lanelet::InvalId, {check_line_p1, check_line_p2}); + + const double new_distance = boost::geometry::distance( + to2D(check_line).basicLineString(), lanelet.polygon2d().basicPolygon()); + if (new_distance < std::numeric_limits::epsilon()) { + linked_lanelets.push_back(lanelet); + } + } + + return linked_lanelets; +} + +// get overlapping lanelets +lanelet::ConstLanelets query::getLinkedLanelets( + const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets) +{ + lanelet::ConstLanelets linked_lanelets; + for (const auto & lanelet : all_road_lanelets) { + const double distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + linked_lanelets.push_back(lanelet); + } + } + return linked_lanelets; +} + +lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + const auto & all_parking_spaces = query::getAllParkingSpaces(lanelet_map_ptr); + const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + return getLinkedParkingSpaces(lanelet, all_parking_spaces, all_parking_lots); +} + +lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, + const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstLineStrings3d linked_parking_spaces; + + // get parking spaces that are in same parking lot. + lanelet::ConstPolygon3d linked_parking_lot; + if (!getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) { + return linked_parking_spaces; + } + const auto & possible_parking_spaces = + getLinkedParkingSpaces(linked_parking_lot, all_parking_spaces); + + // check for parking spaces that are within 5m and facing towards lanelet + for (const auto & parking_space : possible_parking_spaces) { + // check if parking space is close to lanelet + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); + constexpr double distance_thresh = 5.0; + if (distance > distance_thresh) { + continue; + } + + // check if parking space is facing lanelet + const Eigen::Vector3d direction = + parking_space.back().basicPoint() - parking_space.front().basicPoint(); + const Eigen::Vector3d new_pt = parking_space.front().basicPoint() - direction * distance_thresh; + + const lanelet::Point3d check_line_p1(lanelet::InvalId, new_pt.x(), new_pt.y(), new_pt.z()); + const lanelet::Point3d check_line_p2(lanelet::InvalId, parking_space.back().basicPoint()); + const lanelet::LineString3d check_line(lanelet::InvalId, {check_line_p1, check_line_p2}); + + const double new_distance = boost::geometry::distance( + to2D(check_line).basicLineString(), lanelet.polygon2d().basicPolygon()); + if (new_distance < std::numeric_limits::epsilon()) { + linked_parking_spaces.push_back(parking_space); + } + } + return linked_parking_spaces; +} + +// get overlapping parking lot +bool query::getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot) +{ + for (const auto & parking_lot : all_parking_lots) { + const double distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + *linked_parking_lot = parking_lot; + return true; + } + } + return false; +} + +// get overlapping parking lot +bool query::getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot) +{ + for (const auto & parking_lot : all_parking_lots) { + const double distance = + boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + *linked_parking_lot = parking_lot; + return true; + } + } + return false; +} + +// get overlapping parking lot +bool query::getLinkedParkingLot( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) +{ + for (const auto & parking_lot : all_parking_lots) { + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + *linked_parking_lot = parking_lot; + return true; + } + } + return false; +} + +lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( + const lanelet::ConstPolygon3d & parking_lot, + const lanelet::ConstLineStrings3d & all_parking_spaces) +{ + lanelet::ConstLineStrings3d linked_parking_spaces; + for (const auto & parking_space : all_parking_spaces) { + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + linked_parking_spaces.push_back(parking_space); + } + } + return linked_parking_spaces; +} + +// return all stop lines and ref lines from a given set of lanelets +std::vector query::stopLinesLanelets( + const lanelet::ConstLanelets & lanelets) +{ + std::vector stoplines; + + for (const auto & ll : lanelets) { + std::vector ll_stoplines; + ll_stoplines = query::stopLinesLanelet(ll); + stoplines.insert(stoplines.end(), ll_stoplines.begin(), ll_stoplines.end()); + } + + return stoplines; +} + +// return all stop and ref lines from a given lanelet +std::vector query::stopLinesLanelet(const lanelet::ConstLanelet & ll) +{ + std::vector stoplines; + + // find stop lines referenced by right of way reg. elems. + std::vector> right_of_way_reg_elems = + ll.regulatoryElementsAs(); + + if (!right_of_way_reg_elems.empty()) { + // lanelet has a right of way elem element + for (auto j = right_of_way_reg_elems.begin(); j < right_of_way_reg_elems.end(); j++) { + if ((*j)->getManeuver(ll) == lanelet::ManeuverType::Yield) { + // lanelet has a yield reg. elem. + lanelet::Optional row_stopline_opt = (*j)->stopLine(); + if (!!row_stopline_opt) { + stoplines.push_back(row_stopline_opt.get()); + } + } + } + } + + // find stop lines referenced by traffic lights + std::vector> traffic_light_reg_elems = + ll.regulatoryElementsAs(); + + if (!traffic_light_reg_elems.empty()) { + // lanelet has a traffic light elem element + for (auto j = traffic_light_reg_elems.begin(); j < traffic_light_reg_elems.end(); j++) { + lanelet::Optional traffic_light_stopline_opt = (*j)->stopLine(); + if (!!traffic_light_stopline_opt) { + stoplines.push_back(traffic_light_stopline_opt.get()); + } + } + } + // find stop lines referenced by traffic signs + std::vector> traffic_sign_reg_elems = + ll.regulatoryElementsAs(); + + if (!traffic_sign_reg_elems.empty()) { + // lanelet has a traffic sign reg elem - can have multiple ref lines (but + // stop sign shod have 1 + for (auto j = traffic_sign_reg_elems.begin(); j < traffic_sign_reg_elems.end(); j++) { + lanelet::ConstLineStrings3d traffic_sign_stoplines = (*j)->refLines(); + if (!traffic_sign_stoplines.empty()) { + stoplines.push_back(traffic_sign_stoplines.front()); + } + } + } + return stoplines; +} + +std::vector query::stopSignStopLines( + const lanelet::ConstLanelets & lanelets, const std::string & stop_sign_id) +{ + std::vector stoplines; + + std::set checklist; + + for (const auto & ll : lanelets) { + // find stop lines referenced by traffic signs + std::vector> traffic_sign_reg_elems = + ll.regulatoryElementsAs(); + + if (!traffic_sign_reg_elems.empty()) { + // lanelet has a traffic sign reg elem - can have multiple ref lines (but + // stop sign shod have 1 + for (const auto & ts : traffic_sign_reg_elems) { + // skip if traffic sign is not stop sign + if (ts->type() != stop_sign_id) { + continue; + } + + lanelet::ConstLineStrings3d traffic_sign_stoplines = ts->refLines(); + + // only add new items + if (!traffic_sign_stoplines.empty()) { + auto id = traffic_sign_stoplines.front().id(); + if (checklist.find(id) == checklist.end()) { + checklist.insert(id); + stoplines.push_back(traffic_sign_stoplines.front()); + } + } + } + } + } + return stoplines; +} + +ConstLanelets query::getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point, + const double range) +{ + ConstLanelets near_lanelets; + for (const auto & ll : lanelets) { + lanelet::BasicPolygon2d poly = ll.polygon2d().basicPolygon(); + double distance = lanelet::geometry::distance(poly, search_point); + if (distance <= range) { + near_lanelets.push_back(ll); + } + } + return near_lanelets; +} + +ConstLanelets query::getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, + const double range) +{ + return getLaneletsWithinRange( + lanelets, lanelet::BasicPoint2d(search_point.x, search_point.y), range); +} + +ConstLanelets query::getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + return graph->besides(lanelet); +} + +ConstLanelets query::getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point) +{ + const auto lanelets = + getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits::epsilon()); + ConstLanelets road_slices; + for (const auto & llt : lanelets) { + const auto tmp_road_slice = getLaneChangeableNeighbors(graph, llt); + road_slices.insert(road_slices.end(), tmp_road_slice.begin(), tmp_road_slice.end()); + } + return road_slices; +} + +ConstLanelets query::getAllNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + ConstLanelets lanelets; + + ConstLanelets left_lanelets = getAllNeighborsLeft(graph, lanelet); + ConstLanelets right_lanelets = getAllNeighborsRight(graph, lanelet); + + std::reverse(left_lanelets.begin(), left_lanelets.end()); + lanelets.insert(lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + lanelets.push_back(lanelet); + lanelets.insert(lanelets.end(), right_lanelets.begin(), right_lanelets.end()); + + return lanelets; +} + +ConstLanelets query::getAllNeighborsRight( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + ConstLanelets lanelets; + auto right_lane = + (!!graph->right(lanelet)) ? graph->right(lanelet) : graph->adjacentRight(lanelet); + while (!!right_lane) { + lanelets.push_back(right_lane.get()); + right_lane = (!!graph->right(right_lane.get())) ? graph->right(right_lane.get()) + : graph->adjacentRight(right_lane.get()); + } + return lanelets; +} + +ConstLanelets query::getAllNeighborsLeft( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + ConstLanelets lanelets; + auto left_lane = (!!graph->left(lanelet)) ? graph->left(lanelet) : graph->adjacentLeft(lanelet); + while (!!left_lane) { + lanelets.push_back(left_lane.get()); + left_lane = (!!graph->left(left_lane.get())) ? graph->left(left_lane.get()) + : graph->adjacentLeft(left_lane.get()); + } + return lanelets; +} + +ConstLanelets query::getAllNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point) +{ + const auto lanelets = + getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits::epsilon()); + ConstLanelets road_slices; + for (const auto & llt : lanelets) { + const auto tmp_road_slice = getAllNeighbors(graph, llt); + road_slices.insert(road_slices.end(), tmp_road_slice.begin(), tmp_road_slice.end()); + } + return road_slices; +} + +bool query::getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelet * closest_lanelet_ptr) +{ + if (closest_lanelet_ptr == nullptr) { + std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" + << std::endl; + return false; + } + + if (lanelets.empty()) { + return false; + } + + bool found = false; + + lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); + + // find by distance + lanelet::ConstLanelets candidate_lanelets; + { + double min_distance = std::numeric_limits::max(); + for (const auto & llt : lanelets) { + double distance = + boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); + + if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { + candidate_lanelets.push_back(llt); + } else if (distance < min_distance) { + found = true; + candidate_lanelets.clear(); + candidate_lanelets.push_back(llt); + min_distance = distance; + } + } + } + + // find by angle + { + double min_angle = std::numeric_limits::max(); + double pose_yaw = tf2::getYaw(search_pose.orientation); + for (const auto & llt : candidate_lanelets) { + lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); + double segment_angle = std::atan2( + segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); + double angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw)); + if (angle_diff < min_angle) { + min_angle = angle_diff; + *closest_lanelet_ptr = llt; + } + } + } + + return found; +} + +bool query::getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelets * current_lanelets_ptr) +{ + if (current_lanelets_ptr == nullptr) { + std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" + << std::endl; + return false; + } + + if (lanelets.empty()) { + return false; + } + + lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); + for (const auto & llt : lanelets) { + if (lanelet::geometry::inside(llt, search_point)) { + current_lanelets_ptr->push_back(llt); + } + } + + return !current_lanelets_ptr->empty(); // return found +} + +std::vector> getSucceedingLaneletSequencesRecursive( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length) +{ + std::vector> succeeding_lanelet_sequences; + + const auto next_lanelets = graph->following(lanelet); + const double lanelet_length = utils::getLaneletLength3d(lanelet); + + // end condition of the recursive function + if (next_lanelets.empty() || lanelet_length >= length) { + succeeding_lanelet_sequences.push_back({lanelet}); + return succeeding_lanelet_sequences; + } + + for (const auto & next_lanelet : next_lanelets) { + // get lanelet sequence after next_lanelet + auto tmp_lanelet_sequences = + getSucceedingLaneletSequencesRecursive(graph, next_lanelet, length - lanelet_length); + for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) { + tmp_lanelet_sequence.push_front(lanelet); + succeeding_lanelet_sequences.push_back(tmp_lanelet_sequence); + } + } + return succeeding_lanelet_sequences; +} + +std::vector> getPrecedingLaneletSequencesRecursive( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length, const lanelet::ConstLanelets & exclude_lanelets) +{ + std::vector> preceding_lanelet_sequences; + + const auto prev_lanelets = graph->previous(lanelet); + const double lanelet_length = utils::getLaneletLength3d(lanelet); + + // end condition of the recursive function + if (prev_lanelets.empty() || lanelet_length >= length) { + preceding_lanelet_sequences.push_back({lanelet}); + return preceding_lanelet_sequences; + } + + for (const auto & prev_lanelet : prev_lanelets) { + if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) { + // if prev_lanelet is included in exclude_lanelets, + // remove prev_lanelet from preceding_lanelet_sequences + continue; + } + + // get lanelet sequence after prev_lanelet + auto tmp_lanelet_sequences = getPrecedingLaneletSequencesRecursive( + graph, prev_lanelet, length - lanelet_length, exclude_lanelets); + for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) { + tmp_lanelet_sequence.push_back(lanelet); + preceding_lanelet_sequences.push_back(tmp_lanelet_sequence); + } + } + + if (preceding_lanelet_sequences.empty()) { + preceding_lanelet_sequences.push_back({lanelet}); + } + return preceding_lanelet_sequences; +} + +std::vector query::getSucceedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length) +{ + std::vector lanelet_sequences_vec; + const auto next_lanelets = graph->following(lanelet); + for (const auto & next_lanelet : next_lanelets) { + const auto lanelet_sequences_deq = + getSucceedingLaneletSequencesRecursive(graph, next_lanelet, length); + for (const auto & lanelet_sequence : lanelet_sequences_deq) { + lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end()); + } + } + return lanelet_sequences_vec; +} + +std::vector query::getPrecedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length, const lanelet::ConstLanelets & exclude_lanelets) +{ + std::vector lanelet_sequences_vec; + const auto prev_lanelets = graph->previous(lanelet); + for (const auto & prev_lanelet : prev_lanelets) { + if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) { + // if prev_lanelet is included in exclude_lanelets, + // remove prev_lanelet from preceding_lanelet_sequences + continue; + } + // convert deque into vector + const auto lanelet_sequences_deq = + getPrecedingLaneletSequencesRecursive(graph, prev_lanelet, length, exclude_lanelets); + for (const auto & lanelet_sequence : lanelet_sequences_deq) { + lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end()); + } + } + return lanelet_sequences_vec; +} + +} // namespace lanelet::utils + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/road_marking.cpp b/tmp/lanelet2_extension/lib/road_marking.cpp new file mode 100644 index 00000000..fd2a2023 --- /dev/null +++ b/tmp/lanelet2_extension/lib/road_marking.cpp @@ -0,0 +1,77 @@ +// Copyright 2020 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/road_marking.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace lanelet::autoware +{ +namespace +{ +RegulatoryElementDataPtr constructRoadMarkingData( + Id id, const AttributeMap & attributes, const LineString3d & road_marking) +{ + RuleParameterMap rpm; + RuleParameters rule_parameters = {road_marking}; + rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters)); + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "road_marking"; + return data; +} +} // namespace + +RoadMarking::RoadMarking(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) +{ + if (getParameters(RoleName::Refers).size() != 1) { + throw InvalidInputError("There must be exactly one road marking defined!"); + } +} + +RoadMarking::RoadMarking(Id id, const AttributeMap & attributes, const LineString3d & road_marking) +: RoadMarking(constructRoadMarkingData(id, attributes, road_marking)) +{ +} + +ConstLineString3d RoadMarking::roadMarking() const +{ + return getParameters(RoleName::Refers).front(); +} + +LineString3d RoadMarking::roadMarking() +{ + return getParameters(RoleName::Refers).front(); +} + +void RoadMarking::setRoadMarking(const LineString3d & road_marking) +{ + parameters()[RoleName::Refers] = {road_marking}; +} + +void RoadMarking::removeRoadMarking() { parameters()[RoleName::Refers] = {}; } + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/route_checker.cpp b/tmp/lanelet2_extension/lib/route_checker.cpp new file mode 100644 index 00000000..2ba24773 --- /dev/null +++ b/tmp/lanelet2_extension/lib/route_checker.cpp @@ -0,0 +1,46 @@ +// Copyright 2022 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/utility/route_checker.hpp" + +#include + +namespace lanelet::utils +{ +bool route::isRouteValid( + const HADMapRoute & route_msg, const lanelet::LaneletMapPtr lanelet_map_ptr_) +{ + for (const auto & route_section : route_msg.segments) { + for (const auto & primitive : route_section.primitives) { + const auto id = primitive.id; + try { + lanelet_map_ptr_->laneletLayer.get(id); + } catch (const std::exception & e) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; + return false; + } + } + } + return true; +} + +} // namespace lanelet::utils + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/utilities.cpp b/tmp/lanelet2_extension/lib/utilities.cpp new file mode 100644 index 00000000..808f8f5a --- /dev/null +++ b/tmp/lanelet2_extension/lib/utilities.cpp @@ -0,0 +1,729 @@ +// Copyright 2015-2019 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. +// +// Authors: Kenji Miyake, Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/utility/utilities.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/query.hpp" + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include +#include +#include + +namespace lanelet::utils +{ +namespace +{ +[[maybe_unused]] bool exists(const std::vector & array, const int element) +{ + return std::find(array.begin(), array.end(), element) != array.end(); +} + +/** + * [getContactingLanelets retrieves id of lanelets which has distance 0m to + * search_point] + * @param lanelet_map [pointer to lanelet] + * @param trafficRules [traffic rules to ignore lanelets that are not + * traversable] + * @param search_point [2D point used for searching] + * @param contacting_lanelet_ids [array of lanelet ids that is contacting with + * search_point] + */ +[[maybe_unused]] void getContactingLanelets( + const lanelet::LaneletMapPtr lanelet_map, + const lanelet::traffic_rules::TrafficRulesPtr traffic_rules, + const lanelet::BasicPoint2d & search_point, std::vector * contacting_lanelet_ids) +{ + if (!lanelet_map) { + std::cerr << "No lanelet map is set!" << std::endl; + return; + } + + if (contacting_lanelet_ids == nullptr) { + std::cerr << __FUNCTION__ << " contacting_lanelet_ids is null pointer!" << std::endl; + return; + } + + for (const auto & ll : lanelet_map->laneletLayer) { + if (!traffic_rules->canPass(ll)) { + continue; + } + lanelet::BasicPolygon2d poly = ll.polygon2d().basicPolygon(); + double distance = lanelet::geometry::distance(poly, search_point); + if (distance < std::numeric_limits::epsilon()) { + contacting_lanelet_ids->push_back(static_cast(ll.id())); + } + } +} + +std::vector calculateSegmentDistances(const lanelet::ConstLineString3d & line_string) +{ + std::vector segment_distances; + segment_distances.reserve(line_string.size() - 1); + + for (size_t i = 1; i < line_string.size(); ++i) { + const auto distance = lanelet::geometry::distance(line_string[i], line_string[i - 1]); + segment_distances.push_back(distance); + } + + return segment_distances; +} + +std::vector calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) +{ + const auto segment_distances = calculateSegmentDistances(line_string); + + std::vector accumulated_lengths{0}; + accumulated_lengths.reserve(segment_distances.size() + 1); + std::partial_sum( + std::begin(segment_distances), std::end(segment_distances), + std::back_inserter(accumulated_lengths)); + + return accumulated_lengths; +} + +std::pair findNearestIndexPair( + const std::vector & accumulated_lengths, const double target_length) +{ + // List size + const auto N = accumulated_lengths.size(); + + // Front + if (target_length < accumulated_lengths.at(1)) { + return std::make_pair(0, 1); + } + + // Back + if (target_length > accumulated_lengths.at(N - 2)) { + return std::make_pair(N - 2, N - 1); + } + + // Middle + for (std::size_t i = 1; i < N; ++i) { + if ( + accumulated_lengths.at(i - 1) <= target_length && + target_length <= accumulated_lengths.at(i)) { + return std::make_pair(i - 1, i); + } + } + + // Throw an exception because this never happens + throw std::runtime_error("No nearest point found."); +} + +std::vector resamplePoints( + const lanelet::ConstLineString3d & line_string, const int num_segments) +{ + // Calculate length + const auto line_length = static_cast(lanelet::geometry::length(line_string)); + + // Calculate accumulated lengths + const auto accumulated_lengths = calculateAccumulatedLengths(line_string); + if (accumulated_lengths.size() < 2) return {}; + + // Create each segment + std::vector resampled_points; + for (auto i = 0; i <= num_segments; ++i) { + // Find two nearest points + const auto target_length = (static_cast(i) / num_segments) * line_length; + const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length); + + // Apply linear interpolation + const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; + const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; + const auto direction_vector = (front_point - back_point); + + const auto back_length = accumulated_lengths.at(index_pair.first); + const auto front_length = accumulated_lengths.at(index_pair.second); + const auto segment_length = front_length - back_length; + const auto target_point = + back_point + (direction_vector * (target_length - back_length) / segment_length); + + // Add to list + resampled_points.emplace_back(target_point); + } + + return resampled_points; +} +lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +lanelet::ConstLanelet combineLanelets(const lanelet::ConstLanelets & lanelets) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + lanelet::Points3d centers; + for (const auto & llt : lanelets) { + for (const auto & pt : llt.leftBound()) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : llt.rightBound()) { + rights.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : llt.centerline()) { + centers.push_back(lanelet::Point3d(pt)); + } + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + const auto center_line = lanelet::LineString3d(lanelet::InvalId, centers); + auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + combined_lanelet.setCenterline(center_line); + return std::move(combined_lanelet); +} + +} // namespace + +lanelet::LineString3d generateFineCenterline( + const lanelet::ConstLanelet & lanelet_obj, const double resolution) +{ + // Get length of longer border + const double left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const double right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2; + const lanelet::Point3d center_point( + lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), + center_basic_point.z()); + centerline.push_back(center_point); + } + return centerline; +} + +lanelet::ConstLineString3d getCenterlineWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const double right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2; + + const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized(); + + const auto offset_center_basic_point = center_basic_point + vec_right_2_left * offset; + + const lanelet::Point3d center_point( + lanelet::utils::getId(), offset_center_basic_point.x(), offset_center_basic_point.y(), + offset_center_basic_point.z()); + centerline.push_back(center_point); + } + return static_cast(centerline); +} + +lanelet::ConstLineString3d getRightBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const double right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d rightBound(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + const auto vec_left_2_right = (right_points.at(i) - left_points.at(i)).normalized(); + + const auto offset_right_basic_point = right_points.at(i) + vec_left_2_right * offset; + + const lanelet::Point3d rightBound_point( + lanelet::utils::getId(), offset_right_basic_point.x(), offset_right_basic_point.y(), + offset_right_basic_point.z()); + rightBound.push_back(rightBound_point); + } + return static_cast(rightBound); +} + +lanelet::ConstLineString3d getLeftBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const double right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d leftBound(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + + const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized(); + + const auto offset_left_basic_point = left_points.at(i) + vec_right_2_left * offset; + + const lanelet::Point3d leftBound_point( + lanelet::utils::getId(), offset_left_basic_point.x(), offset_left_basic_point.y(), + offset_left_basic_point.z()); + leftBound.push_back(leftBound_point); + } + return static_cast(leftBound); +} + +lanelet::ConstLanelet getExpandedLanelet( + const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset) +{ + using lanelet::geometry::offsetNoThrow; + using lanelet::geometry::internal::checkForInversion; + + const auto & orig_left_bound_2d = lanelet_obj.leftBound2d().basicLineString(); + const auto & orig_right_bound_2d = lanelet_obj.rightBound2d().basicLineString(); + + // Note: The lanelet::geometry::offset throws exception when the undesired inversion is found. + // Use offsetNoThrow until the logic is updated to handle the inversion. + // TODO(Horibe) update + auto expanded_left_bound_2d = offsetNoThrow(orig_left_bound_2d, left_offset); + auto expanded_right_bound_2d = offsetNoThrow(orig_right_bound_2d, right_offset); + + rclcpp::Clock clock{RCL_ROS_TIME}; + try { + checkForInversion(orig_left_bound_2d, expanded_left_bound_2d, left_offset); + checkForInversion(orig_right_bound_2d, expanded_right_bound_2d, right_offset); + } catch (const lanelet::GeometryError & e) { + RCLCPP_ERROR_THROTTLE( + rclcpp::get_logger("lanelet2_extension"), clock, 1000, + "Fail to expand lanelet. output may be undesired. Lanelet points interval in map data could " + "be too narrow."); + } + + // Note: modify front and back points so that the successive lanelets will not have any + // longitudinal space between them. + { // front + const double diff_x = orig_right_bound_2d.front().x() - orig_left_bound_2d.front().x(); + const double diff_y = orig_right_bound_2d.front().y() - orig_left_bound_2d.front().y(); + const double theta = std::atan2(diff_y, diff_x); + expanded_right_bound_2d.front().x() = + orig_right_bound_2d.front().x() - right_offset * std::cos(theta); + expanded_right_bound_2d.front().y() = + orig_right_bound_2d.front().y() - right_offset * std::sin(theta); + expanded_left_bound_2d.front().x() = + orig_left_bound_2d.front().x() - left_offset * std::cos(theta); + expanded_left_bound_2d.front().y() = + orig_left_bound_2d.front().y() - left_offset * std::sin(theta); + } + { // back + const double diff_x = orig_right_bound_2d.back().x() - orig_left_bound_2d.back().x(); + const double diff_y = orig_right_bound_2d.back().y() - orig_left_bound_2d.back().y(); + const double theta = std::atan2(diff_y, diff_x); + expanded_right_bound_2d.back().x() = + orig_right_bound_2d.back().x() - right_offset * std::cos(theta); + expanded_right_bound_2d.back().y() = + orig_right_bound_2d.back().y() - right_offset * std::sin(theta); + expanded_left_bound_2d.back().x() = + orig_left_bound_2d.back().x() - left_offset * std::cos(theta); + expanded_left_bound_2d.back().y() = + orig_left_bound_2d.back().y() - left_offset * std::sin(theta); + } + + const auto toPoints3d = [](const lanelet::BasicLineString2d & ls2d, const double z) { + lanelet::Points3d output; + for (const auto & pt : ls2d) { + output.push_back(lanelet::Point3d(lanelet::InvalId, pt.x(), pt.y(), z)); + } + return output; + }; + + // Original z value cannot be used directly since the offset function can vary the points size. + const lanelet::Points3d ex_lefts = + toPoints3d(expanded_left_bound_2d, lanelet_obj.leftBound3d().basicLineString().at(0).z()); + const lanelet::Points3d ex_rights = + toPoints3d(expanded_right_bound_2d, lanelet_obj.rightBound3d().basicLineString().at(0).z()); + + const auto & extended_left_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_lefts); + const auto & expanded_right_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_rights); + const auto & lanelet = lanelet::Lanelet( + lanelet_obj.id(), extended_left_bound_3d, expanded_right_bound_3d, lanelet_obj.attributes()); + + return lanelet; +} + +lanelet::ConstLanelets getExpandedLanelets( + const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset) +{ + lanelet::ConstLanelets lanelets; + for (const auto & llt : lanelet_obj) { + lanelets.push_back(getExpandedLanelet(llt, left_offset, right_offset)); + } + return lanelets; +} + +void overwriteLaneletsCenterline( + lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool force_overwrite) +{ + for (auto & lanelet_obj : lanelet_map->laneletLayer) { + if (force_overwrite || !lanelet_obj.hasCustomCenterline()) { + const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution); + lanelet_obj.setCenterline(fine_center_line); + } + } +} + +lanelet::ConstLanelets getConflictingLanelets( + const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet) +{ + const auto & llt_or_areas = graph->conflicting(lanelet); + lanelet::ConstLanelets lanelets; + lanelets.reserve(llt_or_areas.size()); + for (const auto & l_or_a : llt_or_areas) { + auto llt_opt = l_or_a.lanelet(); + if (!!llt_opt) { + lanelets.push_back(llt_opt.get()); + } + } + return lanelets; +} + +bool lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +{ + if (polygon == nullptr) { + std::cerr << __func__ << ": polygon is null pointer! Failed to convert to polygon." + << std::endl; + return false; + } + if (linestring.size() != 2) { + std::cerr << __func__ << ": linestring" << linestring.id() << " must have 2 points! (" + << linestring.size() << " != 2)" << std::endl + << "Failed to convert to polygon."; + return false; + } + if (!linestring.hasAttribute("width")) { + std::cerr << __func__ << ": linestring" << linestring.id() + << " does not have width tag. Failed to convert to polygon."; + return false; + } + + const Eigen::Vector3d direction = + linestring.back().basicPoint() - linestring.front().basicPoint(); + const double width = linestring.attributeOr("width", 0.0); + const Eigen::Vector3d direction_left = + (Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()) * direction).normalized(); + + const Eigen::Vector3d eigen_p1 = linestring.front().basicPoint() + direction_left * width / 2; + const Eigen::Vector3d eigen_p2 = linestring.back().basicPoint() + direction_left * width / 2; + const Eigen::Vector3d eigen_p3 = linestring.back().basicPoint() - direction_left * width / 2; + const Eigen::Vector3d eigen_p4 = linestring.front().basicPoint() - direction_left * width / 2; + + const lanelet::Point3d p1(lanelet::InvalId, eigen_p1.x(), eigen_p1.y(), eigen_p1.z()); + const lanelet::Point3d p2(lanelet::InvalId, eigen_p2.x(), eigen_p2.y(), eigen_p2.z()); + const lanelet::Point3d p3(lanelet::InvalId, eigen_p3.x(), eigen_p3.y(), eigen_p3.z()); + const lanelet::Point3d p4(lanelet::InvalId, eigen_p4.x(), eigen_p4.y(), eigen_p4.z()); + + *polygon = lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); + + return true; +} + +bool lineStringToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +{ + if (polygon == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __func__ << ": polygon is null pointer! Failed to convert to polygon."); + return false; + } + if (linestring.size() < 4) { + if (linestring.size() < 3 || linestring.front().id() == linestring.back().id()) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __func__ << ": linestring" << linestring.id() + << " must have more than different 3 points! (size is " << linestring.size() << ")" + << std::endl + << "Failed to convert to polygon."); + return false; + } + } + + lanelet::Polygon3d llt_poly; + + for (const auto & lp : linestring) { + llt_poly.push_back(lanelet::Point3d( + lanelet::InvalId, lp.basicPoint().x(), lp.basicPoint().y(), lp.basicPoint().z())); + } + + if (linestring.front().id() == linestring.back().id()) { + llt_poly.pop_back(); + } + + *polygon = llt_poly; + + return true; +} + +double getLaneletLength2d(const lanelet::ConstLanelet & lanelet) +{ + return static_cast( + boost::geometry::length(lanelet::utils::to2D(lanelet.centerline()).basicLineString())); +} + +double getLaneletLength3d(const lanelet::ConstLanelet & lanelet) +{ + return static_cast(boost::geometry::length(lanelet.centerline().basicLineString())); +} + +double getLaneletLength2d(const lanelet::ConstLanelets & lanelet_sequence) +{ + double length = 0; + for (const auto & llt : lanelet_sequence) { + length += getLaneletLength2d(llt); + } + return length; +} + +double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence) +{ + double length = 0; + for (const auto & llt : lanelet_sequence) { + length += getLaneletLength3d(llt); + } + return length; +} + +lanelet::ArcCoordinates getArcCoordinates( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) +{ + lanelet::ConstLanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); + + double length = 0; + lanelet::ArcCoordinates arc_coordinates; + for (const auto & llt : lanelet_sequence) { + const auto & centerline_2d = lanelet::utils::to2D(llt.centerline()); + if (llt == closest_lanelet) { + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); + arc_coordinates = lanelet::geometry::toArcCoordinates( + centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + arc_coordinates.length += length; + break; + } + length += static_cast(boost::geometry::length(centerline_2d)); + } + return arc_coordinates; +} + +lanelet::ConstLineString3d getClosestSegment( + const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring) +{ + if (linestring.size() < 2) { + return lanelet::LineString3d(); + } + + lanelet::ConstLineString3d closest_segment; + double min_distance = std::numeric_limits::max(); + + for (size_t i = 1; i < linestring.size(); i++) { + lanelet::BasicPoint3d prev_basic_pt = linestring[i - 1].basicPoint(); + lanelet::BasicPoint3d current_basic_pt = linestring[i].basicPoint(); + + lanelet::Point3d prev_pt( + lanelet::InvalId, prev_basic_pt.x(), prev_basic_pt.y(), prev_basic_pt.z()); + lanelet::Point3d current_pt( + lanelet::InvalId, current_basic_pt.x(), current_basic_pt.y(), current_basic_pt.z()); + + lanelet::LineString3d current_segment(lanelet::InvalId, {prev_pt, current_pt}); + double distance = lanelet::geometry::distance2d( + lanelet::utils::to2D(current_segment).basicLineString(), search_pt); + if (distance < min_distance) { + closest_segment = current_segment; + min_distance = distance; + } + } + return closest_segment; +} + +lanelet::CompoundPolygon3d getPolygonFromArcLength( + const lanelet::ConstLanelets & lanelets, const double s1, const double s2) +{ + const auto combined_lanelet = combineLanelets(lanelets); + const auto total_length = getLaneletLength2d(combined_lanelet); + + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = static_cast( + ratio_s1 * boost::geometry::length(combined_lanelet.leftBound().basicLineString())); + const auto s2_left = static_cast( + ratio_s2 * boost::geometry::length(combined_lanelet.leftBound().basicLineString())); + const auto s1_right = static_cast( + ratio_s1 * boost::geometry::length(combined_lanelet.rightBound().basicLineString())); + const auto s2_right = static_cast( + ratio_s2 * boost::geometry::length(combined_lanelet.rightBound().basicLineString())); + + const auto left_bound = + getLineStringFromArcLength(combined_lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = + getLineStringFromArcLength(combined_lanelet.rightBound(), s1_right, s2_right); + + const auto & lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + return lanelet.polygon3d(); +} + +double getLaneletAngle( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point) +{ + lanelet::BasicPoint2d llt_search_point(search_point.x, search_point.y); + lanelet::ConstLineString3d segment = getClosestSegment(llt_search_point, lanelet.centerline()); + return std::atan2( + segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); +} + +bool isInLanelet( + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet, + const double radius) +{ + constexpr double eps = 1.0e-9; + const lanelet::BasicPoint2d p(current_pose.position.x, current_pose.position.y); + return boost::geometry::distance(p, lanelet.polygon2d().basicPolygon()) < radius + eps; +} + +geometry_msgs::msg::Pose getClosestCenterPose( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point) +{ + lanelet::BasicPoint2d llt_search_point(search_point.x, search_point.y); + lanelet::ConstLineString3d segment = getClosestSegment(llt_search_point, lanelet.centerline()); + + const Eigen::Vector2d direction( + (segment.back().basicPoint2d() - segment.front().basicPoint2d()).normalized()); + const Eigen::Vector2d xf(segment.front().basicPoint2d()); + const Eigen::Vector2d x(search_point.x, search_point.y); + const Eigen::Vector2d p = xf + (x - xf).dot(direction) * direction; + + geometry_msgs::msg::Pose closest_pose; + closest_pose.position.x = p.x(); + closest_pose.position.y = p.y(); + closest_pose.position.z = search_point.z; + + const double lane_yaw = getLaneletAngle(lanelet, search_point); + tf2::Quaternion q; + q.setRPY(0, 0, lane_yaw); + closest_pose.orientation = tf2::toMsg(q); + + return closest_pose; +} +} // namespace lanelet::utils + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/virtual_traffic_light.cpp b/tmp/lanelet2_extension/lib/virtual_traffic_light.cpp new file mode 100644 index 00000000..89c586eb --- /dev/null +++ b/tmp/lanelet2_extension/lib/virtual_traffic_light.cpp @@ -0,0 +1,65 @@ +// Copyright 2021 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/virtual_traffic_light.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace lanelet::autoware +{ +namespace +{ +RegulatoryElementDataPtr constructVirtualTrafficLightData( + const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light) +{ + RuleParameterMap rpm; + RuleParameters rule_parameters = {virtual_traffic_light}; + rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters)); + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "virtual_traffic_light"; + return data; +} +} // namespace + +VirtualTrafficLight::VirtualTrafficLight(const RegulatoryElementDataPtr & data) +: RegulatoryElement(data) +{ + if (getParameters("start_line").size() != 1) { + throw InvalidInputError("There must be exactly one start_line defined!"); + } + if (getParameters("end_line").empty()) { + throw InvalidInputError("No end_line defined!"); + } +} + +VirtualTrafficLight::VirtualTrafficLight( + const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light) +: VirtualTrafficLight(constructVirtualTrafficLightData(id, attributes, virtual_traffic_light)) +{ +} + +} // namespace lanelet::autoware + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp new file mode 100644 index 00000000..aa867680 --- /dev/null +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -0,0 +1,1268 @@ +// Copyright 2015-2019 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +// NOLINTBEGIN(readability-identifier-naming) + +#include "lanelet2_extension/visualization/visualization.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/query.hpp" +#include "lanelet2_extension/utility/utilities.hpp" + +#include + +#include +#include + +#include +#include +#include +#include + +namespace +{ +template +bool exists(const std::unordered_set & set, const T & element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +void adjacentPoints( + const int i, const int N, const geometry_msgs::msg::Polygon & poly, + geometry_msgs::msg::Point32 * p0, geometry_msgs::msg::Point32 * p1, + geometry_msgs::msg::Point32 * p2) +{ + if (p0 == nullptr || p1 == nullptr || p2 == nullptr) { + std::cerr << __FUNCTION__ << ": either p0, p1, or p2 is null pointer!" << std::endl; + return; + } + + *p1 = poly.points[i]; + if (i == 0) { + *p0 = poly.points[N - 1]; + } else { + *p0 = poly.points[i - 1]; + } + + if (i < N - 1) { + *p2 = poly.points[i + 1]; + } else { + *p2 = poly.points[0]; + } +} + +[[maybe_unused]] double hypot( + const geometry_msgs::msg::Point32 & p0, const geometry_msgs::msg::Point32 & p1) +{ + return sqrt(pow((p1.x - p0.x), 2.0) + pow((p1.y - p0.y), 2.0)); +} + +bool isAttributeValue( + const lanelet::ConstPoint3d & p, const std::string & attr_str, const std::string & value_str) +{ + const lanelet::Attribute & attr = p.attribute(attr_str); + return attr.value() == value_str; +} + +bool isLaneletAttributeValue( + const lanelet::ConstLanelet & ll, const std::string & attr_str, const std::string & value_str) +{ + const lanelet::Attribute & attr = ll.attribute(attr_str); + return attr.value() == value_str; +} + +void initLightMarker(visualization_msgs::msg::Marker * marker, const std::string & ns) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + float s = 0.3; + + marker->header.frame_id = "map"; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->id = 0; + marker->lifetime = rclcpp::Duration(0, 0); + marker->type = visualization_msgs::msg::Marker::SPHERE; + marker->scale.x = s; + marker->scale.y = s; + marker->scale.z = s; +} + +bool inputLightMarker(visualization_msgs::msg::Marker * marker, const lanelet::ConstPoint3d & p) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return false; + } + + marker->id = static_cast(p.id()); + + geometry_msgs::msg::Point point; + marker->pose.position.x = p.x(); + marker->pose.position.y = p.y(); + marker->pose.position.z = p.z(); + + std_msgs::msg::ColorRGBA color; + marker->color.r = 0.0f; + marker->color.g = 0.0f; + marker->color.b = 0.0f; + marker->color.a = 0.3f; + + if (isAttributeValue(p, "color", "red")) { + marker->color.r = 0.3f; + marker->color.g = 0.0f; + marker->color.b = 0.0f; + } else if (isAttributeValue(p, "color", "green")) { + marker->color.r = 0.0f; + marker->color.g = 0.3f; + marker->color.b = 0.0f; + } else if (isAttributeValue(p, "color", "yellow")) { + marker->color.r = 0.3f; + marker->color.g = 0.3f; + marker->color.b = 0.0f; + } else { + marker->color.r = 0.3f; + marker->color.g = 0.3f; + marker->color.b = 0.3f; + } + return true; +} + +void initLaneletDirectionMarker(visualization_msgs::msg::Marker * marker, const std::string & ns) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + float s = 1.0; + + marker->header.frame_id = "map"; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->id = 0; + marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker->lifetime = rclcpp::Duration(0, 0); + + 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 = s; + marker->scale.y = s; + marker->scale.z = s; + marker->color.r = 1.0f; + marker->color.g = 1.0f; + marker->color.b = 1.0f; + marker->color.a = 0.999; +} + +void pushLaneletDirectionMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLanelet & ll) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + lanelet::BasicPoint3d pt[3]; + pt[0].x() = 0.0; + pt[0].y() = -0.3; + pt[0].z() = 0.0; + pt[1].x() = 0.0; + pt[1].y() = 0.3; + pt[1].z() = 0; + pt[2].x() = 1.0; + pt[2].y() = 0.0; + pt[2].z() = 0; + + lanelet::BasicPoint3d pc; + lanelet::BasicPoint3d pc2; + lanelet::ConstLineString3d center_ls = ll.centerline(); + lanelet::Attribute attr = ll.attribute("turn_direction"); + + std_msgs::msg::ColorRGBA c; + c.r = 0.5; + c.g = 0.5; + c.b = 0.5; + c.a = 0.5; + + if (isLaneletAttributeValue(ll, "turn_direction", "right")) { + c.r = 0.5; + c.g = 0.5; + c.b = 0.6; + } else if (isLaneletAttributeValue(ll, "turn_direction", "left")) { + c.r = 0.5; + c.g = 0.6; + c.b = 0.6; + } + + for (std::size_t ci = 0; ci < center_ls.size() - 1;) { + pc = center_ls[ci]; + if (center_ls.size() > 1) { + pc2 = center_ls[ci + 1]; + } else { + return; + } + + double heading = atan2(pc2.y() - pc.y(), pc2.x() - pc.x()); + + lanelet::BasicPoint3d pt_tf[3]; + + Eigen::Vector3d axis(0, 0, 1); + Eigen::Transform t(Eigen::AngleAxis(heading, axis)); + + for (int i = 0; i < 3; i++) { + pt_tf[i] = t * pt[i] + pc; + } + + geometry_msgs::msg::Point gp[3]; + + for (int i = 0; i < 3; i++) { + std_msgs::msg::ColorRGBA cn = c; + + gp[i].x = pt_tf[i].x(); + gp[i].y = pt_tf[i].y(); + gp[i].z = pt_tf[i].z(); + marker->points.push_back(gp[i]); + marker->colors.push_back(cn); + } + ci = ci + 1; + } +} + +bool isClockWise(const geometry_msgs::msg::Polygon & polygon) +{ + const int N = static_cast(polygon.points.size()); + const double x_offset = polygon.points[0].x; + const double y_offset = polygon.points[0].y; + double sum = 0.0; + for (std::size_t i = 0; i < polygon.points.size(); ++i) { + sum += (polygon.points[i].x - x_offset) * (polygon.points[(i + 1) % N].y - y_offset) - + (polygon.points[i].y - y_offset) * (polygon.points[(i + 1) % N].x - x_offset); + } + + return sum < 0.0; +} + +// Is angle AOB less than 180? +// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e +bool isAcuteAngle( + const geometry_msgs::msg::Point32 & a, const geometry_msgs::msg::Point32 & o, + const geometry_msgs::msg::Point32 & b) +{ + return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x) >= 0; +} + +// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e +bool isWithinTriangle( + const geometry_msgs::msg::Point32 & a, const geometry_msgs::msg::Point32 & b, + const geometry_msgs::msg::Point32 & c, const geometry_msgs::msg::Point32 & p) +{ + double c1 = (b.x - a.x) * (p.y - b.y) - (b.y - a.y) * (p.x - b.x); + double c2 = (c.x - b.x) * (p.y - c.y) - (c.y - b.y) * (p.x - c.x); + double c3 = (a.x - c.x) * (p.y - a.y) - (a.y - c.y) * (p.x - a.x); + + return (c1 > 0.0 && c2 > 0.0 && c3 > 0.0) || (c1 < 0.0 && c2 < 0.0 && c3 < 0.0); +} + +visualization_msgs::msg::Marker createPolygonMarker( + const std::string & name_space, const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.id = 0; + marker.ns = name_space; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + marker.pose.position.x = 0.0; + marker.pose.position.y = 0.0; + marker.pose.position.z = 0.0; + 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 = color; + return marker; +} + +void pushPolygonMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstPolygon3d & polygon, + const std_msgs::msg::ColorRGBA & color) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + if (polygon.size() < 3) { + return; + } + + geometry_msgs::msg::Polygon geom_poly; + lanelet::utils::conversion::toGeomMsgPoly(polygon, &geom_poly); + + std::vector triangles; + lanelet::visualization::polygon2Triangle(geom_poly, &triangles); + + for (const auto & tri : triangles) { + geometry_msgs::msg::Point geom_pts[3]; + for (int i = 0; i < 3; i++) { + lanelet::utils::conversion::toGeomMsgPt(tri.points[i], &geom_pts[i]); + marker->points.push_back(geom_pts[i]); + marker->colors.push_back(color); + } + } +} + +} // anonymous namespace + +namespace lanelet +{ +void visualization::lanelet2Triangle( + const lanelet::ConstLanelet & ll, std::vector * triangles) +{ + if (triangles == nullptr) { + std::cerr << __FUNCTION__ << ": triangles is null pointer!" << std::endl; + return; + } + + triangles->clear(); + geometry_msgs::msg::Polygon ll_poly; + lanelet2Polygon(ll, &ll_poly); + polygon2Triangle(ll_poly, triangles); +} + +// NOLINTBEGIN(readability-function-cognitive-complexity) +void visualization::polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon, std::vector * triangles) +{ + geometry_msgs::msg::Polygon poly = polygon; + if (!isClockWise(poly)) { + std::reverse(poly.points.begin(), poly.points.end()); + } + + // ear clipping: find smallest internal angle in polygon + int N = static_cast(poly.points.size()); + + // array of angles for each vertex + std::vector is_acute_angle; + is_acute_angle.assign(N, false); + for (int i = 0; i < N; i++) { + geometry_msgs::msg::Point32 p0; + geometry_msgs::msg::Point32 p1; + geometry_msgs::msg::Point32 p2; + + adjacentPoints(i, N, poly, &p0, &p1, &p2); + is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2); + } + + // start ear clipping + while (N >= 3) { + int clipped_vertex = -1; + + for (int i = 0; i < N; i++) { + const bool theta = is_acute_angle.at(i); + if (theta) { + geometry_msgs::msg::Point32 p0; + geometry_msgs::msg::Point32 p1; + geometry_msgs::msg::Point32 p2; + adjacentPoints(i, N, poly, &p0, &p1, &p2); + + int j_begin = (i + 2) % N; + int j_end = (i - 1 + N) % N; + bool is_ear = true; + for (int j = j_begin; j != j_end; j = (j + 1) % N) { + if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) { + is_ear = false; + break; + } + } + + if (is_ear) { + clipped_vertex = i; + break; + } + } + } + if (clipped_vertex < 0 || clipped_vertex >= N) { + // print in yellow to indicate warning + std::cerr << "\033[1;33mCould not find valid vertex for ear clipping triangulation. " + "Triangulation result might be invalid\033[0m" + << std::endl; + clipped_vertex = 0; + } + + // create triangle + geometry_msgs::msg::Point32 p0; + geometry_msgs::msg::Point32 p1; + geometry_msgs::msg::Point32 p2; + adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); + geometry_msgs::msg::Polygon triangle; + triangle.points.push_back(p0); + triangle.points.push_back(p1); + triangle.points.push_back(p2); + triangles->push_back(triangle); + + // remove vertex of center of angle + auto it = poly.points.begin(); + std::advance(it, clipped_vertex); + poly.points.erase(it); + + // remove from angle list + auto it_angle = is_acute_angle.begin(); + std::advance(it_angle, clipped_vertex); + is_acute_angle.erase(it_angle); + + // update angle list + N = static_cast(poly.points.size()); + if (clipped_vertex == N) { + clipped_vertex = 0; + } + adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); + is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2); + + int i_prev = (clipped_vertex == 0) ? N - 1 : clipped_vertex - 1; + adjacentPoints(i_prev, N, poly, &p0, &p1, &p2); + is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2); + } +} +// NOLINTEND(readability-function-cognitive-complexity) + +void visualization::lanelet2Polygon( + const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon) +{ + if (polygon == nullptr) { + std::cerr << __FUNCTION__ << ": polygon is null pointer!" << std::endl; + return; + } + + lanelet::CompoundPolygon3d ll_poly = ll.polygon3d(); + + polygon->points.clear(); + polygon->points.reserve(ll_poly.size()); + + for (const auto & pt : ll_poly) { + geometry_msgs::msg::Point32 pt32; + utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32); + polygon->points.push_back(pt32); + } +} + +visualization_msgs::msg::MarkerArray visualization::laneletDirectionAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + initLaneletDirectionMarker(&marker, additional_namespace + "lanelet direction"); + + for (const auto & ll : lanelets) { + if (ll.hasAttribute(std::string("turn_direction"))) { + pushLaneletDirectionMarker(&marker, ll); + } + } + if (marker.points.empty()) { + return marker_array; + } + marker_array.markers.push_back(marker); + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarkerArray( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) +{ + visualization_msgs::msg::MarkerArray tl_marker_array; + if (tl_reg_elems.empty()) { + return tl_marker_array; + } + visualization_msgs::msg::Marker marker_tri; + visualization_msgs::msg::Marker marker_sph; + initLightMarker(&marker_sph, "traffic_light"); + visualization::initTrafficLightTriangleMarker(&marker_tri, "traffic_light_triangle", duration); + + for (const auto & tl : tl_reg_elems) { + const auto lights = tl->trafficLights(); + for (const auto & lsp : lights) { + if (lsp.isLineString()) { // traffic lights can either polygons or linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + visualization::pushTrafficLightTriangleMarker(&marker_tri, ls, c, scale); + } + } + + tl_marker_array.markers.push_back(marker_tri); + + lanelet::ConstLineStrings3d light_bulbs = tl->lightBulbs(); + for (const auto & ls : light_bulbs) { + lanelet::ConstLineString3d l = static_cast(ls); + for (const auto & pt : l) { + if (pt.hasAttribute("color")) { + if (inputLightMarker(&marker_sph, pt)) { + tl_marker_array.markers.push_back(marker_sph); + } + } + } + } + } + + return tl_marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) +{ + visualization_msgs::msg::MarkerArray tl_id_marker_array; + + for (const auto & tl : tl_reg_elems) { + const auto lights = tl->trafficLights(); + for (const auto & lsp : lights) { + if (lsp.isLineString()) { // traffic lights can either polygons or + // linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.ns = "traffic_light_id"; + marker.id = static_cast(ls.id()); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.lifetime = duration; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = (ls.front().x() + ls.back().x()) / 2; + marker.pose.position.y = (ls.front().y() + ls.back().y()) / 2; + marker.pose.position.z = ls.front().z() + 1.0; + 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.color = c; + marker.scale.z = scale; + marker.frame_locked = true; + marker.text = std::to_string(ls.id()); + tl_id_marker_array.markers.push_back(marker); + } + } + } + + return tl_id_marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray( + const std::vector & da_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + visualization_msgs::msg::Marker line_marker; + + if (da_reg_elems.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.ns = "detection_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; + + std_msgs::msg::ColorRGBA line_c; + line_c.r = 0.5; + line_c.g = 0.5; + line_c.b = 0.5; + line_c.a = 0.999; + visualization::initLineStringMarker(&line_marker, "map", "detection_area_stopline", line_c); + + for (const auto & da_reg_elem : da_reg_elems) { + marker.points.clear(); + marker.colors.clear(); + marker.id = static_cast(da_reg_elem->id()); + + // area visualization + const auto detection_areas = da_reg_elem->detectionAreas(); + for (const auto & detection_area : detection_areas) { + geometry_msgs::msg::Polygon geom_poly; + utils::conversion::toGeomMsgPoly(detection_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 detection areas + marker_array.markers.push_back(marker); + + // stop line visualization + visualization::pushLineStringMarker(&line_marker, da_reg_elem->stopLine(), line_c, 0.5); + } // for regulatory elements + + marker_array.markers.push_back(line_marker); + 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) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + visualization_msgs::msg::Marker line_marker; + + if (no_reg_elems.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.ns = "no_stopping_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; + + std_msgs::msg::ColorRGBA line_c; + line_c.r = 0.5; + line_c.g = 0.5; + line_c.b = 0.5; + line_c.a = 0.999; + visualization::initLineStringMarker(&line_marker, "map", "no_stopping_area_stopline", line_c); + + 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_stopping_areas = no_reg_elem->noStoppingAreas(); + for (const auto & no_stopping_area : no_stopping_areas) { + geometry_msgs::msg::Polygon geom_poly; + utils::conversion::toGeomMsgPoly(no_stopping_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_stopping areas + marker_array.markers.push_back(marker); + const auto & stop_line = no_reg_elem->stopLine(); + // stop line visualization + if (stop_line) { + visualization::pushLineStringMarker(&line_marker, stop_line.value(), line_c, 0.5); + } + } // for regulatory elements + if (!line_marker.points.empty()) { + marker_array.markers.push_back(line_marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (pedestrian_markings.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("pedestrian_marking", c); + for (const auto & linestring : pedestrian_markings) { + lanelet::ConstPolygon3d polygon; + if (utils::lineStringToPolygon(linestring, &polygon)) { + pushPolygonMarker(&marker, polygon, c); + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + "pedestrian marking " << linestring.id() << " failed conversion."); + } + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::parkingLotsAsMarkerArray( + const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (parking_lots.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("parking_lots", c); + for (const auto & polygon : parking_lots) { + pushPolygonMarker(&marker, polygon, c); + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} +visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray( + const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (parking_spaces.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("parking_space", c); + for (const auto & linestring : parking_spaces) { + lanelet::ConstPolygon3d polygon; + if (utils::lineStringWithWidthToPolygon(linestring, &polygon)) { + pushPolygonMarker(&marker, polygon, c); + } else { + std::cerr << "parking space " << linestring.id() << " failed conversion." << std::endl; + } + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::generateLaneletIdMarker( + const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c, + const double scale) +{ + visualization_msgs::msg::MarkerArray markers; + for (const auto & ll : road_lanelets) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "lanelet_id"; + marker.id = static_cast(ll.id()); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + const auto centerline = ll.centerline(); + const size_t target_position_index = centerline.size() / 2; + const auto & target_position = centerline[target_position_index]; + marker.pose.position.x = target_position.x(); + marker.pose.position.y = target_position.y(); + marker.pose.position.z = target_position.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.color = c; + marker.scale.z = scale; + marker.frame_locked = true; + marker.text = std::to_string(ll.id()); + markers.markers.push_back(marker); + } + return markers; +} + +visualization_msgs::msg::MarkerArray visualization::obstaclePolygonsAsMarkerArray( + const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (obstacle_polygons.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("obstacles", c); + for (const auto & polygon : obstacle_polygons) { + pushPolygonMarker(&marker, polygon, c); + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::lineStringsAsMarkerArray( + const std::vector & line_strings, const std::string & name_space, + const std_msgs::msg::ColorRGBA & c, const float lss) +{ + visualization_msgs::msg::MarkerArray ls_marker_array; + if (line_strings.empty()) { + return ls_marker_array; + } + std::unordered_set added; + visualization_msgs::msg::Marker ls_marker; + visualization::initLineStringMarker(&ls_marker, "map", name_space, c); + + for (const auto & ls : line_strings) { + if (!exists(added, ls.id())) { + visualization::pushLineStringMarker(&ls_marker, ls, c, lss); + added.insert(ls.id()); + } + } + ls_marker_array.markers.push_back(ls_marker); + return ls_marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c, + const bool viz_centerline, const std::string & additional_namespace) +{ + const float lss = 0.1; // line string size + const float lss_center = static_cast(std::max(lss * 0.1, 0.02)); + + std::unordered_set added; + visualization_msgs::msg::Marker left_line_strip; + visualization_msgs::msg::Marker right_line_strip; + visualization_msgs::msg::Marker start_bound_line_strip; + visualization_msgs::msg::Marker center_line_strip; + visualization_msgs::msg::Marker center_arrows; + visualization::initLineStringMarker( + &left_line_strip, "map", additional_namespace + "left_lane_bound", c); + visualization::initLineStringMarker( + &right_line_strip, "map", additional_namespace + "right_lane_bound", c); + visualization::initLineStringMarker( + &start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c); + visualization::initLineStringMarker( + ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); + visualization::initArrowsMarker( + ¢er_arrows, "map", additional_namespace + "center_line_arrows", c); + + for (const auto & lll : lanelets) { + lanelet::ConstLineString3d left_ls = lll.leftBound(); + lanelet::ConstLineString3d right_ls = lll.rightBound(); + lanelet::ConstLineString3d center_ls = lll.centerline(); + lanelet::LineString3d start_bound_ls(lanelet::utils::getId()); + start_bound_ls.push_back(lanelet::Point3d( + lanelet::utils::getId(), left_ls.front().x(), left_ls.front().y(), left_ls.front().z())); + start_bound_ls.push_back(lanelet::Point3d( + lanelet::utils::getId(), right_ls.front().x(), right_ls.front().y(), right_ls.front().z())); + + if (!exists(added, left_ls.id())) { + visualization::pushLineStringMarker(&left_line_strip, left_ls, c, lss); + added.insert(left_ls.id()); + } + if (!exists(added, right_ls.id())) { + visualization::pushLineStringMarker(&right_line_strip, right_ls, c, lss); + added.insert(right_ls.id()); + } + if (!exists(added, start_bound_ls.id())) { + visualization::pushLineStringMarker(&start_bound_line_strip, start_bound_ls, c, lss); + added.insert(start_bound_ls.id()); + } + if (viz_centerline && !exists(added, center_ls.id())) { + visualization::pushLineStringMarker(¢er_line_strip, center_ls, c, lss_center); + visualization::pushArrowsMarker(¢er_arrows, center_ls, c); + added.insert(center_ls.id()); + } + } + + visualization_msgs::msg::MarkerArray marker_array; + if (!left_line_strip.points.empty()) { + marker_array.markers.push_back(left_line_strip); + } + if (!right_line_strip.points.empty()) { + marker_array.markers.push_back(right_line_strip); + } + if (!center_line_strip.points.empty()) { + marker_array.markers.push_back(center_line_strip); + } + if (!start_bound_line_strip.points.empty()) { + marker_array.markers.push_back(start_bound_line_strip); + } + if (!center_arrows.points.empty()) { + marker_array.markers.push_back(center_arrows); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::trafficLightsAsTriangleMarkerArray( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) +{ + // convert to to an array of linestrings and publish as marker array using + // existing function + + std::vector line_strings; + visualization_msgs::msg::Marker marker; + visualization::initTrafficLightTriangleMarker(&marker, "traffic_light_triangle", duration); + + for (const auto & tl : tl_reg_elems) { + lanelet::LineString3d ls; + + const auto lights = tl->trafficLights(); + for (const auto & lsp : lights) { + if (lsp.isLineString()) { // traffic lights can either polygons or linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + visualization::pushTrafficLightTriangleMarker(&marker, ls, c, scale); + } + } + } + + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.push_back(marker); + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::laneletsAsTriangleMarkerArray( + const std::string & ns, const lanelet::ConstLanelets & lanelets, + const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + if (lanelets.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.ns = ns; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + 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 & ll : lanelets) { + std::vector triangles; + lanelet2Triangle(ll, &triangles); + + for (const 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); + } + } + } + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +void visualization::initTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const std::string & ns, + const rclcpp::Duration & duration) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + marker->header.frame_id = "map"; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + 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; +} + +void visualization::pushTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & cl, const double scale) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + double h = 0.7; + if (ls.hasAttribute("height")) { + lanelet::Attribute attr = ls.attribute("height"); + h = std::stod(attr.value()); + } + + // construct triangles and add to marker + + // define polygon of traffic light border + Eigen::Vector3d v[4]; + v[0] << ls.front().x(), ls.front().y(), ls.front().z(); + v[1] << ls.back().x(), ls.back().y(), ls.back().z(); + v[2] << ls.back().x(), ls.back().y(), ls.back().z() + h; + v[3] << ls.front().x(), ls.front().y(), ls.front().z() + h; + + Eigen::Vector3d c = (v[0] + v[1] + v[2] + v[3]) / 4; + + if (scale > 0.0 && scale != 1.0) { + for (auto & i : v) { + i = (i - c) * scale + c; + } + } + geometry_msgs::msg::Point tri0[3]; + utils::conversion::toGeomMsgPt(v[0], &tri0[0]); + utils::conversion::toGeomMsgPt(v[1], &tri0[1]); + utils::conversion::toGeomMsgPt(v[2], &tri0[2]); + geometry_msgs::msg::Point tri1[3]; + utils::conversion::toGeomMsgPt(v[0], &tri1[0]); + utils::conversion::toGeomMsgPt(v[2], &tri1[1]); + utils::conversion::toGeomMsgPt(v[3], &tri1[2]); + + for (const auto & i : tri0) { + marker->points.push_back(i); + marker->colors.push_back(cl); + } + for (const auto & i : tri0) { + marker->points.push_back(i); + marker->colors.push_back(cl); + } +} + +void visualization::initLineStringMarker( + visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker is null pointer!"); + return; + } + + marker->header.frame_id = frame_id; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->action = visualization_msgs::msg::Marker::ADD; + marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + + marker->id = 0; + 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 = c; +} + +void visualization::pushLineStringMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & c, const float lss) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker is null pointer!"); + return; + } + + // fill out lane line + if (ls.size() < 2) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker line size is 1 or 0!"); + return; + } + for (auto i = ls.begin(); i + 1 != ls.end(); i++) { + geometry_msgs::msg::Point p; + const auto heading = + static_cast(std::atan2((*(i + 1)).y() - (*i).y(), (*(i + 1)).x() - (*i).x())); + + const auto x_offset = static_cast(lss * 0.5 * std::sin(heading)); + const auto y_offset = static_cast(lss * 0.5 * std::cos(heading)); + + p.x = (*i).x() + x_offset; + p.y = (*i).y() - y_offset; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*i).x() - x_offset; + p.y = (*i).y() + y_offset; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*(i + 1)).x() + x_offset; + p.y = (*(i + 1)).y() - y_offset; + p.z = (*(i + 1)).z(); + marker->points.push_back(p); + marker->colors.push_back(c); + p.x = (*(i + 1)).x() - x_offset; + p.y = (*(i + 1)).y() + y_offset; + p.z = (*(i + 1)).z(); + marker->points.push_back(p); + p.x = (*(i + 1)).x() + x_offset; + p.y = (*(i + 1)).y() - y_offset; + p.z = (*(i + 1)).z(); + marker->points.push_back(p); + p.x = (*i).x() - x_offset; + p.y = (*i).y() + y_offset; + p.z = (*i).z(); + marker->points.push_back(p); + marker->colors.push_back(c); + } +} + +void visualization::initArrowsMarker( + visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker is null pointer!"); + return; + } + + marker->header.frame_id = frame_id; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->action = visualization_msgs::msg::Marker::ADD; + marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + + marker->id = 0; + 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 = c; +} + +void visualization::pushArrowsMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & c) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker is null pointer!"); + return; + } + + // fill out lane line + if (ls.size() < 2) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker line size is 1 or 0!"); + return; + } + for (auto i = ls.begin(); i + 1 != ls.end(); i++) { + const auto heading = + static_cast(std::atan2((*(i + 1)).y() - (*i).y(), (*(i + 1)).x() - (*i).x())); + + const float sin_offset = std::sin(heading); + const float cos_offset = std::cos(heading); + const double width = 0.3; + const double height = 1.0; + + geometry_msgs::msg::Point p; + p.x = (*i).x() + sin_offset * width; + p.y = (*i).y() - cos_offset * width; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*i).x() - sin_offset * width; + p.y = (*i).y() + cos_offset * width; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*i).x() + cos_offset * height; + p.y = (*i).y() + sin_offset * height; + p.z = (*i).z(); + marker->points.push_back(p); + marker->colors.push_back(c); + } +} + +} // namespace lanelet + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/package.xml b/tmp/lanelet2_extension/package.xml new file mode 100644 index 00000000..662e9a94 --- /dev/null +++ b/tmp/lanelet2_extension/package.xml @@ -0,0 +1,37 @@ + + + + lanelet2_extension + 0.1.0 + The lanelet2_extension package contains libraries to handle Lanelet2 format data. + mitsudome-r + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_auto_mapping_msgs + autoware_auto_planning_msgs + autoware_utils + geographiclib + geometry_msgs + lanelet2_core + lanelet2_io + lanelet2_maps + lanelet2_projection + lanelet2_routing + lanelet2_traffic_rules + lanelet2_validation + pugixml-dev + rclcpp + tf2 + tf2_geometry_msgs + visualization_msgs + + ament_cmake_ros + + + ament_cmake + + diff --git a/tmp/lanelet2_extension/src/sample_code.cpp b/tmp/lanelet2_extension/src/sample_code.cpp new file mode 100644 index 00000000..d4595fbd --- /dev/null +++ b/tmp/lanelet2_extension/src/sample_code.cpp @@ -0,0 +1,125 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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/projection/mgrs_projector.hpp" +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" + +#include + +#include +#include +#include + +#include +#include + +void loadingAutowareOSMFile(const std::string & map_file_path) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet::GPSPoint gps_point; + gps_point.lat = 49.0; + gps_point.lon = 8.4; + lanelet::Origin origin(gps_point); + lanelet::projection::UtmProjector projector(lanelet::Origin(gps_point), true, false); + + // Autoware OSM file parser is registered into lanelet2 library. + // Therefore, you can used it by just specifying "autoware_osm_handler" parser + // in load function. + lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors); + + // If you want to use default parser, explicitly name the parser + lanelet_map = lanelet::load(map_file_path, "osm_handler", projector, &errors); +} + +void usingMGRSProjector() +{ + // MGRS Projector projects lat/lon to x,y,z point in MGRS 100km grid. + // The origin is automatically calculated so you don't have to select any + // origin. + lanelet::projection::MGRSProjector projector; + + // Let's convert lat/lng point into mgrs xyz point. + lanelet::GPSPoint gps_point; + gps_point.lat = 49.0; + gps_point.lon = 8.4; + gps_point.ele = 0.0; + + lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point); + std::cout << mgrs_point << std::endl; + + // You can get MGRS Code of projected grid. + std::string mgrs_grid = projector.getProjectedMGRSGrid(); + std::cout << mgrs_grid << std::endl; + + // You can also reverse project from MGRS point to lat/lon. + // You have to set which MGRS grid it is from or it will reuse last projected + // grid + lanelet::GPSPoint projected_gps_point = projector.reverse(mgrs_point); + std::cout << projected_gps_point.lat << " " << projected_gps_point.lon << std::endl; + lanelet::GPSPoint projected_gps_point2 = + lanelet::projection::MGRSProjector::reverse(mgrs_point, mgrs_grid); + std::cout << projected_gps_point2.lat << " " << projected_gps_point2.lon << " " << std::endl; +} + +void usingAutowareTrafficLight(const std::string & map_file_path) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet::projection::MGRSProjector projector; + lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors); + + for (const auto & lanelet : lanelet_map->laneletLayer) { + // You can access to traffic light element as AutowareTrafficLight class + const auto autoware_traffic_lights = + lanelet.regulatoryElementsAs(); + for (const auto & light : autoware_traffic_lights) { + // You can access to the position of each lamps(light bulb) in traffic + // light + for (const auto & light_bulb_string : light->lightBulbs()) { + std::cout << light_bulb_string.id() << std::endl; + } + // Since AutowareTrafficLight class is inheriting lanelet::TrafficLight + // class, you can also access to outline of traffic light by the same + // method. + for (const auto & light_string : light->trafficLights()) { + std::cout << light_string.id() << std::endl; + } + } + + // You can also access to same traffic light element as default TrafficLight + // class + auto traffic_lights = lanelet.regulatoryElementsAs(); + for (const auto & light : traffic_lights) { + for (const auto & light_string : light->trafficLights()) { + std::cout << light_string.id() << std::endl; + } + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_code"); + const std::string map_file_path = node->declare_parameter("map_file", ""); + loadingAutowareOSMFile(map_file_path); + usingMGRSProjector(); + usingAutowareTrafficLight(map_file_path); + return 0; +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/src/validation.cpp b/tmp/lanelet2_extension/src/validation.cpp new file mode 100644 index 00000000..4b087141 --- /dev/null +++ b/tmp/lanelet2_extension/src/validation.cpp @@ -0,0 +1,179 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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/projection/mgrs_projector.hpp" +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace +{ +namespace keyword +{ +constexpr const char * Id = "id"; +constexpr const char * Osm = "osm"; +constexpr const char * Tag = "tag"; +constexpr const char * Key = "k"; +constexpr const char * Node = "node"; +constexpr const char * Elevation = "ele"; +} // namespace keyword + +void printUsage() +{ + std::cout << "Usage:" << std::endl + << "ros2 run lanelet2_extension autoware_lanelet2_validation" + " --ros-args -p map_file:=" + << std::endl; +} +} // namespace + +void validateElevationTag(const std::string & filename) +{ + pugi::xml_document doc; + auto result = doc.load_file(filename.c_str()); + if (!result) { + std::cerr << result.description() << std::endl; + exit(1); + } + + auto osmNode = doc.child(keyword::Osm); + for (auto node = osmNode.child(keyword::Node); node; node = node.next_sibling(keyword::Node)) { + const auto id = node.attribute(keyword::Id).as_llong(lanelet::InvalId); + if (!node.find_child_by_attribute(keyword::Tag, keyword::Key, keyword::Elevation)) { + std::cerr << "failed to find elevation tag for node: " << id << std::endl; + } + } +} + +// NOLINTBEGIN(readability-function-cognitive-complexity) +void validateTrafficLight(const lanelet::LaneletMapPtr lanelet_map) +{ + if (!lanelet_map) { + std::cerr << "Missing map. Are you sure you set correct path for map?" << std::endl; + exit(1); + } + + for (const auto & lanelet : lanelet_map->laneletLayer) { + const auto autoware_traffic_lights = + lanelet.regulatoryElementsAs(); + if (autoware_traffic_lights.empty()) { + continue; + } + for (const auto & light : autoware_traffic_lights) { + if (light->lightBulbs().empty()) { + std::cerr << "regulatory element traffic light " << light->id() + << " is missing optional light_bulb member. You won't be able to use region_tlr " + "node with this map" + << std::endl; + } + for (const auto & light_string : light->lightBulbs()) { + if (!light_string.hasAttribute("traffic_light_id")) { + std::cerr << "light_bulb " << light_string.id() << " is missing traffic_light_id tag" + << std::endl; + } + } + for (const auto & base_string_or_poly : light->trafficLights()) { + if (!base_string_or_poly.isLineString()) { + std::cerr + << "traffic_light " << base_string_or_poly.id() + << " is polygon, and only linestring class is currently supported for traffic lights" + << std::endl; + } + const auto base_string = static_cast(base_string_or_poly); + if (!base_string.hasAttribute("height")) { + std::cerr << "traffic_light " << base_string.id() << " is missing height tag" + << std::endl; + } + } + } + } +} +// NOLINTEND(readability-function-cognitive-complexity) + +void validateTurnDirection(const lanelet::LaneletMapPtr lanelet_map) +{ + if (!lanelet_map) { + std::cerr << "Missing map. Are you sure you set correct path for map?" << std::endl; + exit(1); + } + + lanelet::traffic_rules::TrafficRulesPtr traffic_rules = + lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + lanelet::routing::RoutingGraphPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map, *traffic_rules); + + for (const auto & lanelet : lanelet_map->laneletLayer) { + if (!traffic_rules->canPass(lanelet)) { + continue; + } + + const auto conflicting_lanelets_or_areas = vehicle_graph->conflicting(lanelet); + if (conflicting_lanelets_or_areas.empty()) { + continue; + } + if (!lanelet.hasAttribute("turn_direction")) { + std::cerr + << "lanelet " << lanelet.id() + << " seems to be intersecting other lanelet, but does not have turn_direction tagging." + << std::endl; + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("autoware_lanelet_validation"); + + std::string map_path; + try { + map_path = node->declare_parameter("map_file"); + } catch (...) { + std::cerr << "failed find map_file parameter! No file to load" << std::endl; + printUsage(); + return 1; + } + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet::projection::MGRSProjector projector; + lanelet_map = lanelet::load(map_path, "autoware_osm_handler", projector, &errors); + + std::cout << "starting validation" << std::endl; + + validateElevationTag(map_path); + validateTrafficLight(lanelet_map); + validateTurnDirection(lanelet_map); + + std::cout << "finished validation" << std::endl; + + return 0; +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/test/src/test_message_conversion.cpp b/tmp/lanelet2_extension/test/src/test_message_conversion.cpp new file mode 100644 index 00000000..f1d930c2 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/test_message_conversion.cpp @@ -0,0 +1,174 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/query.hpp" + +#include + +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::utils::getId; +using lanelet::utils::conversion::toGeomMsgPt; + +class TestSuite : public ::testing::Test // NOLINT for gtest +{ +public: + TestSuite() : single_lanelet_map_ptr(new lanelet::LaneletMap()) + { + Point3d p1; + Point3d p2; + Point3d p3; + Point3d p4; + Point3d p5; + Point3d p6; + Point3d p7; + LineString3d traffic_light_base; + LineString3d traffic_light_bulbs; + LineString3d stop_line; + + p1 = Point3d(getId(), 0., 0., 0.); + p2 = Point3d(getId(), 0., 1., 0.); + + p3 = Point3d(getId(), 1., 0., 0.); + p4 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left(getId(), {p1, p2}); + LineString3d ls_right(getId(), {p3, p4}); + + Lanelet lanelet(getId(), ls_left, ls_right); + + single_lanelet_map_ptr->add(lanelet); + } + + ~TestSuite() override = default; + + lanelet::LaneletMapPtr single_lanelet_map_ptr; + +private: +}; + +TEST_F(TestSuite, BinMsgConversion) // NOLINT for gtest +{ + autoware_auto_mapping_msgs::msg::HADMapBin bin_msg; + lanelet::LaneletMapPtr regenerated_map(new lanelet::LaneletMap); + + lanelet::utils::conversion::toBinMsg(single_lanelet_map_ptr, &bin_msg); + + ASSERT_NE(0U, bin_msg.data.size()) << "converted bin message does not have any data"; + + lanelet::utils::conversion::fromBinMsg(bin_msg, regenerated_map); + + auto original_lanelet = lanelet::utils::query::laneletLayer(single_lanelet_map_ptr); + auto regenerated_lanelet = lanelet::utils::query::laneletLayer(regenerated_map); + + ASSERT_EQ(original_lanelet.front().id(), regenerated_lanelet.front().id()) + << "regenerated map has different id"; +} + +TEST_F(TestSuite, ToGeomMsgPt) // NOLINT for gtest +{ + Point3d lanelet_pt(getId(), -0.1, 0.2, 3.0); + + geometry_msgs::msg::Point32 geom_pt32; + geom_pt32.x = -0.1; + geom_pt32.y = 0.2; + geom_pt32.z = 3.0; + + geometry_msgs::msg::Point geom_pt; + toGeomMsgPt(geom_pt32, &geom_pt); + ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) + << " converted value is different from original geometry_msgs::msg::Point"; + + geom_pt = toGeomMsgPt(geom_pt32); + ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) + << " converted value is different from original geometry_msgs::msg::Point"; + + toGeomMsgPt(lanelet_pt.basicPoint(), &geom_pt); + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z) + << " converted value is different from original " + "lanelet::basicPoint"; + + geom_pt = toGeomMsgPt(lanelet_pt.basicPoint()); + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z) + << " converted value is different from original " + "lanelet::basicPoint"; + + toGeomMsgPt(lanelet_pt, &geom_pt); + ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z) + << " converted value is different from original lanelet::Point3d"; + + geom_pt = toGeomMsgPt(lanelet_pt); + ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z) + << " converted value is different from original lanelet::Point3d"; + + lanelet::ConstPoint2d point_2d = lanelet::utils::to2D(lanelet_pt); + + toGeomMsgPt(point_2d, &geom_pt); + ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(0.0, geom_pt.z) + << " converted value is different from original lanelet::Point2d"; + + geom_pt = toGeomMsgPt(point_2d); + ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(0.0, geom_pt.z) + << " converted value is different from original lanelet::Point2d"; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/test/src/test_projector.cpp b/tmp/lanelet2_extension/test/src/test_projector.cpp new file mode 100644 index 00000000..dacd0f59 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/test_projector.cpp @@ -0,0 +1,87 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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/projection/mgrs_projector.hpp" + +#include + +#include + +class TestSuite : public ::testing::Test // NOLINT for gtest +{ +public: + TestSuite() = default; + ~TestSuite() override = default; +}; + +TEST(TestSuite, ForwardProjection) // NOLINT for gtest +{ + lanelet::projection::MGRSProjector projector; + // lat/lon in Tokyo + lanelet::GPSPoint gps_point; + gps_point.lat = 35.652832; + gps_point.lon = 139.839478; + gps_point.ele = 12.3456789; + lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point); + + // projected z value should not change + ASSERT_DOUBLE_EQ(mgrs_point.z(), gps_point.ele) + << "Forward projected z value should be " << gps_point.ele; + + // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html + // round the projected value to mm since the above reference only gives value + // in mm precision + ASSERT_EQ(projector.getProjectedMGRSGrid(), "54SUE") << "Projected grid should be " + << "54SUE"; + double rounded_x_mm = round(mgrs_point.x() * 1000) / 1000.0; + ASSERT_DOUBLE_EQ(rounded_x_mm, 94946.081) << "Forward projected x value should be " << 94946.081; + double rounded_y_mm = round(mgrs_point.y() * 1000) / 1000.0; + ASSERT_DOUBLE_EQ(rounded_y_mm, 46063.748) << "Forward projected y value should be " << 46063.748; +} + +TEST(TestSuite, ReverseProjection) // NOLINT for gtest +{ + lanelet::projection::MGRSProjector projector; + lanelet::BasicPoint3d mgrs_point; + mgrs_point.x() = 94946.0; + mgrs_point.y() = 46063.0; + mgrs_point.z() = 12.3456789; + + projector.setMGRSCode("54SUE"); + lanelet::GPSPoint gps_point = projector.reverse(mgrs_point); + + // projected z value should not change + ASSERT_DOUBLE_EQ(gps_point.ele, mgrs_point.z()) + << "Reverse projected z value should be " << mgrs_point.z(); + + // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html + // round the projected value since the above reference only gives value up to + // precision of 1e-8 + double rounded_lat = round(gps_point.lat * 1e8) / 1e8; + ASSERT_DOUBLE_EQ(rounded_lat, 35.65282525) + << "Reverse projected latitude value should be " << 35.65282525; + double rounded_lon = round(gps_point.lon * 1e8) / 1e8; + ASSERT_DOUBLE_EQ(rounded_lon, 139.83947721) + << "Reverse projected longitude value should be " << 139.83947721; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/test/src/test_query.cpp b/tmp/lanelet2_extension/test/src/test_query.cpp new file mode 100644 index 00000000..874efb34 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/test_query.cpp @@ -0,0 +1,147 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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/utility/query.hpp" + +#include + +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::Points3d; +using lanelet::utils::getId; + +class TestSuite : public ::testing::Test // NOLINT for gtest +{ +public: + TestSuite() : sample_map_ptr(new lanelet::LaneletMap()) + { + // create sample lanelets + Point3d p1; + Point3d p2; + Point3d p3; + Point3d p4; + + p1 = Point3d(getId(), 0., 0., 0.); + p2 = Point3d(getId(), 0., 1., 0.); + + p3 = Point3d(getId(), 1., 0., 0.); + p4 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left(getId(), {p1, p2}); + LineString3d ls_right(getId(), {p3, p4}); + + Lanelet road_lanelet(getId(), ls_left, ls_right); + road_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + Lanelet crosswalk_lanelet(getId(), ls_left, ls_right); + crosswalk_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Crosswalk; + + // create sample traffic light + Point3d p5; + Point3d p6; + Point3d p7; + Point3d p8; + Point3d p9; + Point3d p10; + Point3d p11; + Point3d p12; + LineString3d traffic_light_base; + LineString3d traffic_light_bulbs; + LineString3d stop_line; + + p6 = Point3d(getId(), 0., 1., 4.); + p7 = Point3d(getId(), 1., 1., 4.); + + p8 = Point3d(getId(), 0., 1., 4.5); + p9 = Point3d(getId(), 0.5, 1., 4.5); + p10 = Point3d(getId(), 1., 1., 4.5); + + p11 = Point3d(getId(), 0., 0., 0.); + p12 = Point3d(getId(), 1., 0., 0.); + + traffic_light_base = LineString3d(getId(), Points3d{p6, p7}); + traffic_light_bulbs = LineString3d(getId(), Points3d{p8, p9, p10}); + stop_line = LineString3d(getId(), Points3d{p11, p12}); + + auto tl = lanelet::autoware::AutowareTrafficLight::make( + getId(), lanelet::AttributeMap(), {traffic_light_base}, stop_line, {traffic_light_bulbs}); + + road_lanelet.addRegulatoryElement(tl); + + // add items to map + sample_map_ptr->add(road_lanelet); + sample_map_ptr->add(crosswalk_lanelet); + } + + ~TestSuite() override = default; + + lanelet::LaneletMapPtr sample_map_ptr; + +private: +}; + +TEST_F(TestSuite, QueryLanelets) // NOLINT for gtest +{ + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr); + ASSERT_EQ(2U, all_lanelets.size()) << "failed to retrieve all lanelets"; + + lanelet::ConstLanelets subtype_lanelets = + lanelet::utils::query::subtypeLanelets(all_lanelets, lanelet::AttributeValueString::Road); + ASSERT_EQ(1U, subtype_lanelets.size()) << "failed to retrieve road lanelet by subtypeLanelets"; + + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + ASSERT_EQ(1U, road_lanelets.size()) << "failed to retrieve road lanelets"; + + lanelet::ConstLanelets crosswalk_lanelets = + lanelet::utils::query::crosswalkLanelets(all_lanelets); + ASSERT_EQ(1U, crosswalk_lanelets.size()) << "failed to retrieve crosswalk lanelets"; +} + +TEST_F(TestSuite, QueryTrafficLights) // NOLINT for gtest +{ + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr); + + auto traffic_lights = lanelet::utils::query::trafficLights(all_lanelets); + ASSERT_EQ(1U, traffic_lights.size()) << "failed to retrieve traffic lights"; + + auto autoware_traffic_lights = lanelet::utils::query::autowareTrafficLights(all_lanelets); + ASSERT_EQ(1U, autoware_traffic_lights.size()) << "failed to retrieve autoware traffic lights"; +} + +TEST_F(TestSuite, QueryStopLine) // NOLINT for gtest +{ + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr); + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + + auto stop_lines = lanelet::utils::query::stopLinesLanelets(all_lanelets); + ASSERT_EQ(1U, stop_lines.size()) << "failed to retrieve stop lines from all lanelets"; + + auto stop_lines2 = lanelet::utils::query::stopLinesLanelet(road_lanelets.front()); + ASSERT_EQ(1U, stop_lines2.size()) << "failed to retrieve stop lines from a lanelet"; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/test/src/test_regulatory_elements.cpp b/tmp/lanelet2_extension/test/src/test_regulatory_elements.cpp new file mode 100644 index 00000000..7037e951 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/test_regulatory_elements.cpp @@ -0,0 +1,145 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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/autoware_traffic_light.hpp" + +#include +#include +#include + +#include +#include + +using lanelet::LineString3d; +using lanelet::LineStringOrPolygon3d; +using lanelet::Point3d; +using lanelet::Points3d; +using lanelet::utils::getId; + +namespace +{ +template +std::vector convertToVector(T item) +{ + std::vector vector = {item}; + return vector; +} +} // namespace + +class TestSuite : public ::testing::Test // NOLINT for gtest +{ +public: + TestSuite() = default; + ~TestSuite() override = default; +}; + +TEST(TestSuite, FactoryConstructsTrafficLight) // NOLINT for gtest +{ + Point3d p1; + Point3d p2; + Point3d p3; + Point3d p4; + Point3d p5; + Point3d p6; + Point3d p7; + LineStringOrPolygon3d traffic_light_base; + LineString3d traffic_light_bulbs; + LineString3d stop_line; + + p1 = Point3d(getId(), 0., 1., 4.); + p2 = Point3d(getId(), 1., 1., 4.); + + p3 = Point3d(getId(), 0., 1., 4.5); + p4 = Point3d(getId(), 0.5, 1., 4.5); + p5 = Point3d(getId(), 1., 1., 4.5); + + p6 = Point3d(getId(), 0., 0., 0.); + p7 = Point3d(getId(), 1., 0., 0.); + + Points3d base = {p1, p2}; + Points3d bulbs = {p3, p4, p5}; + Points3d stop = {p6, p7}; + + traffic_light_base = LineString3d(getId(), base); + traffic_light_bulbs = LineString3d(getId(), bulbs); + stop_line = LineString3d(getId(), stop); + + auto tl = lanelet::autoware::AutowareTrafficLight::make( + getId(), lanelet::AttributeMap(), convertToVector(traffic_light_base), stop_line, + convertToVector(traffic_light_bulbs)); + + auto factoryTl = lanelet::RegulatoryElementFactory::create( + tl->attribute(lanelet::AttributeName::Subtype).value(), + std::const_pointer_cast(tl->constData())); + EXPECT_TRUE(!!std::dynamic_pointer_cast(factoryTl)); +} + +TEST(TestSuite, TrafficLightWorksAsExpected) // NOLINT for gtest +{ + Point3d p1; + Point3d p2; + Point3d p3; + Point3d p4; + Point3d p5; + Point3d p6; + Point3d p7; + + LineStringOrPolygon3d traffic_light_base; + LineStringOrPolygon3d traffic_light_base2; + LineString3d traffic_light_bulbs; + LineString3d traffic_light_bulbs2; + LineString3d stop_line; + + p1 = Point3d(getId(), 0., 1., 4.); + p2 = Point3d(getId(), 1., 1., 4.); + + p3 = Point3d(getId(), 0., 1., 4.5); + p4 = Point3d(getId(), 0.5, 1., 4.5); + p5 = Point3d(getId(), 1., 1., 4.5); + + p6 = Point3d(getId(), 0., 0., 0.); + p7 = Point3d(getId(), 1., 0., 0.); + + Points3d base = {p1, p2}; + Points3d bulbs = {p3, p4, p5}; + Points3d stop = {p6, p7}; + + traffic_light_base = {LineString3d(getId(), base)}; + traffic_light_base2 = {LineString3d(getId(), base)}; + traffic_light_bulbs = {LineString3d(getId(), bulbs)}; + traffic_light_bulbs2 = {LineString3d(getId(), bulbs)}; + stop_line = LineString3d(getId(), stop); + + auto tl = lanelet::autoware::AutowareTrafficLight::make( + getId(), lanelet::AttributeMap(), convertToVector(traffic_light_base), stop_line, + convertToVector(traffic_light_bulbs)); + tl->setStopLine(stop_line); + EXPECT_EQ(stop_line, tl->stopLine()); + tl->addTrafficLight(traffic_light_base2); + EXPECT_EQ(2ul, tl->trafficLights().size()); + tl->addLightBulbs(traffic_light_bulbs2); + EXPECT_EQ(2ul, tl->lightBulbs().size()); + tl->removeLightBulbs(traffic_light_bulbs); + EXPECT_EQ(1ul, tl->lightBulbs().size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/test/src/test_route_checker.cpp b/tmp/lanelet2_extension/test/src/test_route_checker.cpp new file mode 100644 index 00000000..1f4d6a89 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/test_route_checker.cpp @@ -0,0 +1,99 @@ +// Copyright 2022 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/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/route_checker.hpp" + +#include +#include + +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::utils::getId; + +class TestSuite : public ::testing::Test // NOLINT for gtest +{ +public: + TestSuite() : sample_map_ptr(new lanelet::LaneletMap()) + { + // create sample lanelets + const Point3d p1(getId(), 0.0, 0.0, 0.0); + const Point3d p2(getId(), 0.0, 1.0, 0.0); + + const LineString3d ls_left(getId(), {p1, p2}); + + const Point3d p3(getId(), 1.0, 0.0, 0.0); + const Point3d p4(getId(), 1.0, 0.0, 0.0); + + const LineString3d ls_right(getId(), {p3, p4}); + + const Lanelet lanelet(getId(), ls_left, ls_right); + + sample_map_ptr->add(lanelet); + + // create sample routes + autoware_auto_mapping_msgs::msg::MapPrimitive map_primitive; + autoware_auto_mapping_msgs::msg::HADMapSegment map_segment1; + autoware_auto_mapping_msgs::msg::HADMapSegment map_segment2; + + for (size_t i = 0; i < 2; i++) { + map_primitive.id = lanelet.id(); + map_segment1.primitives.push_back(map_primitive); + map_primitive.id = ls_left.id(); + map_segment2.primitives.push_back(map_primitive); + } + sample_route1.segments.push_back(map_segment1); + sample_route2.segments.push_back(map_segment2); + } + + ~TestSuite() override = default; + + lanelet::LaneletMapPtr sample_map_ptr; + autoware_auto_planning_msgs::msg::HADMapRoute sample_route1; // valid route + autoware_auto_planning_msgs::msg::HADMapRoute sample_route2; // invalid route + +private: +}; + +TEST_F(TestSuite, isRouteValid) // NOLINT for gtest +{ + autoware_auto_mapping_msgs::msg::HADMapBin bin_msg; + + const auto route_ptr1 = + std::make_shared(sample_route1); + const auto route_ptr2 = + std::make_shared(sample_route2); + + // toBinMsg is tested at test_message_conversion.cpp + lanelet::utils::conversion::toBinMsg(sample_map_ptr, &bin_msg); + + ASSERT_TRUE(lanelet::utils::route::isRouteValid(*route_ptr1, sample_map_ptr)) + << "The route should be valid, which should be created on the same map as the current one"; + ASSERT_FALSE(lanelet::utils::route::isRouteValid(*route_ptr2, sample_map_ptr)) + << "The route should be invalid, which should be created on the different map from the current " + "one"; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/test/src/test_utilities.cpp b/tmp/lanelet2_extension/test/src/test_utilities.cpp new file mode 100644 index 00000000..10103d55 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/test_utilities.cpp @@ -0,0 +1,123 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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/utility/utilities.hpp" + +#include +#include +#include + +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::utils::getId; + +class TestSuite : public ::testing::Test // NOLINT for gtest +{ +public: + TestSuite() : sample_map_ptr(new lanelet::LaneletMap()) + { + // create sample lanelets + Point3d p1; + Point3d p2; + Point3d p3; + Point3d p4; + Point3d p5; + Point3d p6; + Point3d p7; + Point3d p8; + Point3d p9; + Point3d p10; + + p1 = Point3d(getId(), 0., 0., 0.); + p2 = Point3d(getId(), 0., 1., 0.); + p3 = Point3d(getId(), 1., 0., 0.); + p4 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left(getId(), {p1, p2}); + LineString3d ls_right(getId(), {p3, p4}); + + p5 = Point3d(getId(), 0., 2., 0.); + p6 = Point3d(getId(), 1., 2., 0.); + + LineString3d ls_left2(getId(), {p2, p5}); + LineString3d ls_right2(getId(), {p4, p6}); + + p7 = Point3d(getId(), 0., 3., 0.); + p8 = Point3d(getId(), 1., 3., 0.); + + LineString3d ls_left3(getId(), {p5, p7}); + LineString3d ls_right3(getId(), {p6, p8}); + + p9 = Point3d(getId(), 0., 1., 0.); + p10 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left4(getId(), {p9, p5}); + LineString3d ls_right4(getId(), {p10, p6}); + + road_lanelet = Lanelet(getId(), ls_left, ls_right); + road_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + next_lanelet = Lanelet(getId(), ls_left2, ls_right2); + next_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + next_lanelet2 = Lanelet(getId(), ls_left3, ls_right3); + next_lanelet2.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + merging_lanelet = Lanelet(getId(), ls_left4, ls_right4); + merging_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + sample_map_ptr->add(road_lanelet); + sample_map_ptr->add(next_lanelet); + sample_map_ptr->add(next_lanelet2); + sample_map_ptr->add(merging_lanelet); + } + + ~TestSuite() override = default; + + lanelet::LaneletMapPtr sample_map_ptr; + Lanelet road_lanelet; + Lanelet next_lanelet; + Lanelet next_lanelet2; + Lanelet merging_lanelet; + +private: +}; + +TEST_F(TestSuite, OverwriteLaneletsCenterline) // NOLINT for gtest +{ + double resolution = 5.0; + bool force_overwrite = false; + lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr, resolution, force_overwrite); + + for (const auto & lanelet : sample_map_ptr->laneletLayer) { + ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline"; + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +// NOLINTEND(readability-identifier-naming)