feat: add map packages

map/lanelet2_extension/CMakeLists.txt

@@ -0,0 +1,90 @@
+cmake_minimum_required(VERSION 3.5)
+project(lanelet2_extension)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+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
+)
+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)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(message_conversion-test test/src/test_message_conversion.cpp)
+  target_link_libraries(message_conversion-test lanelet2_extension_lib)
+  ament_add_gtest(projector-test test/src/test_projector.cpp)
+  target_link_libraries(projector-test lanelet2_extension_lib)
+  ament_add_gtest(query-test test/src/test_query.cpp)
+  target_link_libraries(query-test lanelet2_extension_lib)
+  ament_add_gtest(regulatory_elements-test test/src/test_regulatory_elements.cpp)
+  target_link_libraries(regulatory_elements-test lanelet2_extension_lib)
+  ament_add_gtest(utilities-test test/src/test_utilities.cpp)
+  target_link_libraries(utilities-test lanelet2_extension_lib)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

map/lanelet2_extension/README.md

@@ -0,0 +1,87 @@
+# 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 `<MapMetaInfo>` 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
+
+### 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 _map_file:=<path/to/map.osm>
+```

map/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
+  <relation id="120700">
+    <member type="way" role="left" ref="34577"/>
+    <member type="way" role="right" ref="120694"/>
+    <tag k="type" v="lanelet"/>
+    <tag k="subtype" v="road_shoulder"/>
+    <tag k="speed_limit" v="10"/>
+    <tag k="location" v="urban"/>
+    <tag k="one_way" v="yes"/>
+  </relation>
+```
+
+### 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
+  <relation id="120700">
+    <member type="way" role="left" ref="34577"/>
+    <member type="way" role="right" ref="120694"/>
+    <tag k="type" v="lanelet"/>
+    <tag k="subtype" v="pedestrian_lane"/>
+    <tag k="speed_limit" v="10"/>
+    <tag k="location" v="urban"/>
+    <tag k="one_way" v="yes"/>
+  </relation>
+```

map/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.
+- refline: refers to stop line of the detection area
+
+![Detection area](detection_area.png)
+
+Sample detection area in .osm format is shown below:
+
+```xml
+  <node id=1 version='1' lat='49.00541994701' lon='8.41565013855'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=2 version='1' lat='49.00542091657' lon='8.4156469497'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=3 version='1' lat='49.00542180052' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=4 version='1' lat='49.00541994701' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=5 version='1' lat='49.00542180052' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=6 version='1' lat='49.00541994701' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <way id=11 version='1'>
+    <nd ref=1 />
+    <nd ref=2 />
+    <nd ref=3 />
+    <nd ref=4 />
+    <nd ref=1 />
+    <tag k='type' v=’detection_area’ />
+    <tag k='area' v=’yes’ />
+  </way>
+  <way id=12 version="1">
+    <nd ref=5 />
+    <nd ref=6 />
+    <tag k='type' v=stop_line’ />
+  </way>
+  <relation id="13">
+     <tag k="type" v="regulatory_element"/>
+     <tag k="subtype" v="detection_area"/>
+     <member type="way" ref="11" role="refers"/>
+     <member type="way" ref="12" role="ref_line"/>
+   </relation>
+```
+
+## 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)