diff --git a/map/autoware_lanelet2_divider/README.md b/map/autoware_lanelet2_divider/README.md new file mode 100644 index 00000000..171c5647 --- /dev/null +++ b/map/autoware_lanelet2_divider/README.md @@ -0,0 +1,152 @@ +# autoware_lanalet2_divider + +This is a lanelet2 tile generator tool for Autoware dynamic lanelet2 map loading feature. + +Working principal of this tool is sequentally: +- Take the inputs from the user +- Generate square grids according to the input MGRS grid +- Filter generated square grids with the input lanelet2_map.osm +- Generate map tiles from filtered grids and input lanelet2_map.osm with Osmium Tool + +- [autoware_lanalet2_divider](#autoware_lanalet2_divider) + - [System Requirements](#system-requirements) + - [Inputs](#inputs) + - [MGRS Grid](#mgrs-grid) + - [Grid Edge Size](#grid-edge-size) + - [Input Lanelet2 Map Path](#input-lanelet2-map-path) + - [Outputs](#outputs) + - [lanelet2_map.osm Folder](#lanelet2_maposm-folder) + - [lanelet2_map_metadata.yaml](#lanelet2_map_metadatayaml) + - [config.json Files](#configjson-files) + - [output_layers.gpkg](#output_layersgpkg) + - [Installation](#installation) + - [Poetry](#poetry) + - [Osmium Tool](#osmium-tool) + - [autoware_lanalet2_divider Python Dependencies](#autoware_lanalet2_divider-python-dependencies) + - [Running](#running) + - [Example Data](#example-data) + - [Yildiz Technical University Original Lanelet2 Map](#yildiz-technical-university-original-lanelet2-map) + - [Yildiz Technical University Extended Lanelet2 Map (Synthetic)](#yildiz-technical-university-extended-lanelet2-map-synthetic) + - [Params](#params) + +## System Requirements + +- [Python](https://www.python.org/) (tested with 3.10) +- [Poetry package](https://python-poetry.org/docs/) +- [Osmium Tool](https://github.com/osmcode/osmium-tool) + +## Inputs + +### MGRS Grid + +100 kilometer MGRS grid that the input lanelet2_map is in. This is needed for generating the grids inside of the +MGRS grid and filtering them. + +### Grid Edge Size + +This determines the vertice length of the generated map tiles. Map tiles are generated with squares and the edges of +those squares are determined by the user's input. + +### Input Lanelet2 Map Path + +The full path of the input `lanelet2_map.osm`. This file won't be corrupted. + +## Outputs + +### lanelet2_map.osm Folder + +This is a folder. Autoware's dynamic lanelet2 map loading pipeline needs this folder to find generated map-tiles. +It contains all the generated lanelet2 maps. + +### lanelet2_map_metadata.yaml + +This metadata file holds the origin points of all the generated lanelet2 map files inside the `lanelet2_map.osm` +folder. Origin point of each lanelet2 map is the coordinates of the left bottom vertice of the corresponding +square created with `mgrs_grid` and `grid_edge_size` parameters. + +### config.json Files + +Those files contain the coordinates of the vertices of the created squares. With those coordinates, **Osmium Tool** +runs an **extract** operation and divides the input lanelet2_map.osm file. + +### output_layers.gpkg + +This file contains the geographical visualization for generated data. This is not used in Autoware or somewhere else. +It is generating only for debugging the output files. [QGIS](https://qgis.org/en/site/) can be used for opening this +file. You need to see the visualizations of generated grids, filtered grids and input lanelet2 map. +

+ img_ytu_layers +

+

+ img_ytu_extended_layers_full + img_ytu_extended_layers +

+ +## Installation + +Additional to `colcon build`, you have to follow individual installation steps for the tools that are used in this +project. + +### Poetry: + +```bash +pip install poetry +``` + +### Osmium Tool + +https://github.com/osmcode/osmium-tool?tab=readme-ov-file#building + +```bash +cd +git clone https://github.com/osmcode/osmium-tool.git +cd osmium-tool +mkdir build +cd build +cmake .. +ccmake . ## optional: change CMake settings if needed +make +``` + +### autoware_lanalet2_divider Python Dependencies + +```bash +cd +pip install -r requirements.txt +``` + +## Running + +Before running the tool, you should check the `config/lanelet2_divider.param.yaml` file. You can change the default +parameters from this file. + +```bash +ros2 run autoware_lanelet2_divider lanelet2_divider --ros-args --params-file install/autoware_lanelet2_divider/share/autoware_lanelet2_divider/lanelet2_divider.param.yaml +``` + +## Example Data +### Yildiz Technical University Original Lanelet2 Map +This is an original lanelet2 map file that contains stop lines, speed bumps or other regulatory elements: +https://drive.google.com/drive/folders/1VFN0zidAHzIuQCnznVaFzefqqdRS_X5l?usp=drive_link + +

+ img_ytu_original +

+ +### Yildiz Technical University Extended Lanelet2 Map (Synthetic) +This is the extended version of the original lanelet2 map file. We extended it to 80 kilometers by hand. +Still it contains all the regulatory elements: +https://drive.google.com/drive/folders/1CRBYp19_NXzHXxL9JkO_ffbhGARoRaAV?usp=drive_link + +

+ img_ytu_extended +

+ +## Params + +| Param | Description | +|--------------------|---------------------------------------------------------| +| mgrs_grid | The 100 kilometer MGRS grid that the lanelet2 map is in | +| grid_edge_size | Wanted edge length in meters for map-tiles to generate | +| input_lanelet2_map | Full path of the lanelet2_map to divide | +| output_folder | Full path of the output folder to fill | diff --git a/map/autoware_lanelet2_divider/autoware_lanelet2_divider/__init__.py b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/__init__.py new file mode 100644 index 00000000..eb99a873 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/__init__.py @@ -0,0 +1,13 @@ +import importlib.metadata as importlib_metadata + +try: + # This will read version from pyproject.toml + __version__ = importlib_metadata.version(__package__ or __name__) +except importlib_metadata.PackageNotFoundError: + __version__ = "development" + +from .autoware_lanelet2_divider import AutowareLanlet2Divider +import autoware_lanelet2_divider.osmium_tool.osmium_tool as osmium_tool +import autoware_lanelet2_divider.xml_tool.xml_tool as xml_tool +import autoware_lanelet2_divider.data_preperation.data_preperation as data_preparation +from autoware_lanelet2_divider.debug import Debug, DebugMessageType \ No newline at end of file diff --git a/map/autoware_lanelet2_divider/autoware_lanelet2_divider/autoware_lanelet2_divider.py b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/autoware_lanelet2_divider.py new file mode 100644 index 00000000..91e5c36d --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/autoware_lanelet2_divider.py @@ -0,0 +1,61 @@ +import rclpy +from rclpy.node import Node + +import autoware_lanelet2_divider.osmium_tool.osmium_tool as osmium_tool +import autoware_lanelet2_divider.xml_tool.xml_tool as xml_tool +import autoware_lanelet2_divider.data_preperation.data_preperation as data_preparation +from autoware_lanelet2_divider.debug import Debug, DebugMessageType + +import os + +class AutowareLanlet2Divider(Node): + def __init__(self): + super().__init__('autoware_lanelet2_divider') + Debug.log('Autoware Lanelet2 Divider Node has been started.', DebugMessageType.INFO) + + self.declare_params() + + self.input_lanelet2_map_path = self.get_parameter('input_lanelet2_map_path').get_parameter_value().string_value + self.output_folder_path = self.get_parameter('output_folder_path').get_parameter_value().string_value + self.mgrs_grid = self.get_parameter('mgrs_grid').get_parameter_value().string_value + self.grid_edge_size = self.get_parameter('grid_edge_size').get_parameter_value().integer_value + + Debug.log('Input Lanelet2 Map Path: %s' % self.input_lanelet2_map_path, DebugMessageType.INFO) + Debug.log('Output Folder Path: %s' % self.output_folder_path, DebugMessageType.INFO) + Debug.log('MGRS Grid: %s' % self.mgrs_grid, DebugMessageType.INFO) + Debug.log('Grid Edge Size: %d' % self.grid_edge_size, DebugMessageType.INFO) + + # Complete if missing "version" element in lanelet2_map.osm + xml_tool.complete_missing_version_tag(self.input_lanelet2_map_path) + + # Create config file to extract osm file + config_files = data_preparation.data_preparation(self.mgrs_grid, self.grid_edge_size, self.input_lanelet2_map_path, self.output_folder_path) + # Extract osm file + for config_file_path in config_files: + is_extracted = osmium_tool.extract_osm_file(self.input_lanelet2_map_path, config_file_path, + os.path.join(self.output_folder_path, "lanelet2_map.osm")) + if not is_extracted: + Debug.log("Failed to extract osm file.\n", DebugMessageType.ERROR) + rclpy.shutdown() + + # Complete missing elements in osm file + xml_tool.complete_missing_elements(self.input_lanelet2_map_path, os.path.join(self.output_folder_path, "lanelet2_map.osm")) + + Debug.log('Autoware Lanelet2 Divider Node has been finished.', DebugMessageType.SUCCESS) + exit(0) + + def declare_params(self) -> None: + self.declare_parameter('input_lanelet2_map_path', '') + self.declare_parameter('output_folder_path', '') + self.declare_parameter('mgrs_grid', '') + self.declare_parameter('grid_edge_size', 100) + +def main(args=None): + rclpy.init(args=args) + autoware_lanelet2_divider = AutowareLanlet2Divider() + rclpy.spin(autoware_lanelet2_divider) + autoware_lanelet2_divider.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/map/autoware_lanelet2_divider/autoware_lanelet2_divider/data_preperation/data_preperation.py b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/data_preperation/data_preperation.py new file mode 100644 index 00000000..3b2a02a9 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/data_preperation/data_preperation.py @@ -0,0 +1,239 @@ +from typing import List +from osgeo import gdal +from osgeo import ogr +from tqdm import tqdm +import lanelet2 +from lanelet2.projection import UtmProjector +import sys +import mgrs +import utm +import yaml +import json +import time +import os + +from autoware_lanelet2_divider.debug import Debug, DebugMessageType + + +def create_grid_layer(grid_edge_size, layer_grids, mgrs_grid) -> None: + mgrs_object = mgrs.MGRS() + zone, northp, origin_y, origin_x = mgrs_object.MGRSToUTM(mgrs_grid) + + grid_count = 100000 / grid_edge_size + for i in tqdm(range(int(grid_count)), desc=Debug.get_log("Creating grid layer", DebugMessageType.INFO)): + for j in range(int(grid_count)): + feature_grid = ogr.Feature(layer_grids.GetLayerDefn()) + linear_ring = ogr.Geometry(ogr.wkbLinearRing) + for a in range(5): + pt_x, pt_y = 0.0, 0.0 + if (a == 0) or (a == 4): + pt_x = origin_x + (i * grid_edge_size) + pt_y = origin_y + (j * grid_edge_size) + elif a == 1: + pt_x = origin_x + (i * grid_edge_size) + grid_edge_size + pt_y = origin_y + (j * grid_edge_size) + elif a == 2: + pt_x = origin_x + (i * grid_edge_size) + grid_edge_size + pt_y = origin_y + (j * grid_edge_size) + grid_edge_size + elif a == 3: + pt_x = origin_x + (i * grid_edge_size) + pt_y = origin_y + (j * grid_edge_size) + grid_edge_size + pt_lat, pt_lon = utm.to_latlon(pt_y, pt_x, zone, northp) + linear_ring.AddPoint_2D(pt_lon, pt_lat) + + polygon = ogr.Geometry(ogr.wkbPolygon) + polygon.AddGeometry(linear_ring) + feature_grid.SetGeometry(polygon) + if layer_grids.CreateFeature(feature_grid) != 0: + print("Failed to create feature in shapefile.\n") + sys.exit(1) + + feature_grid.Destroy() + + +def generate_lanelet2_layer(mgrs_grid, lanelet2_map_path, lanelet2_whole_mls, layer_lanelet2_whole): + mgrs_object = mgrs.MGRS() + zone, northp, origin_x, origin_y = mgrs_object.MGRSToUTM(mgrs_grid) + origin_lat, origin_lon = utm.to_latlon(origin_x, origin_y, zone, northp) + + projector = UtmProjector(lanelet2.io.Origin(origin_lat, origin_lon)) + lanelet2_map, load_errors = lanelet2.io.loadRobust(lanelet2_map_path, projector) + + for lanelet_linestring in lanelet2_map.lineStringLayer: + linestring = ogr.Geometry(ogr.wkbLineString) + for node in lanelet_linestring: + node_lat, node_lon = utm.to_latlon(origin_x + node.x, origin_y + node.y, zone, northp) + linestring.AddPoint_2D( node_lon, node_lat) + lanelet2_whole_mls.AddGeometry(linestring) + feature_lanelet2 = ogr.Feature( layer_lanelet2_whole.GetLayerDefn()) + feature_lanelet2.SetGeometry(lanelet2_whole_mls) + if layer_lanelet2_whole.CreateFeature(feature_lanelet2) != 0: + print("Failed to create feature in shapefile.\n") + sys.exit( 1 ) + feature_lanelet2.Destroy() + + +def generate_yaml_dict(layer_filtered_grids, grid_edge_size, mgrs_grid) -> dict: + + mgrs_object = mgrs.MGRS() + zone, northp, origin_x, origin_y = mgrs_object.MGRSToUTM(mgrs_grid) + + metadata_yaml = {} + for filtered_grid in layer_filtered_grids: + geometry_filtered_grid = filtered_grid.GetGeometryRef() + point_lat = 0.0 + point_lon = 0.0 + for linearring in geometry_filtered_grid: + point_lat = linearring.GetPoint(0)[1] + point_lon = linearring.GetPoint(0)[0] + x, y, zone_number, zone_letter = utm.from_latlon(point_lat, point_lon) + + file_id = str(filtered_grid.GetFID()) + ".osm" + yaml_data = { + "x_resolution": float(grid_edge_size), + "y_resolution": float(grid_edge_size), + file_id: [round(float(x - origin_x), 2), round(float(y - origin_y), 2)] + } + metadata_yaml.update(yaml_data) + return metadata_yaml + + +def generate_config_json(layer_filtered_grids, extract_dir) -> str: + extracts = [] + for filtered_grid in layer_filtered_grids: + polygon_inner = [] + geometry_filtered_grid = filtered_grid.GetGeometryRef() + for linearring in geometry_filtered_grid: + for i in range(5): + point_lat = linearring.GetPoint(i)[1] + point_lon = linearring.GetPoint(i)[0] + point_list = [point_lon, point_lat] + polygon_inner.append(point_list) + + polygon_outer = [polygon_inner] + extract_element = { + "description": "optional description", + "output": str(filtered_grid.GetFID()) + ".osm", + "output_format": "osm", + "polygon": polygon_outer + } + extracts.append(extract_element) + + config_json_ = { + "directory": os.path.join(extract_dir, "lanelet2_map.osm"), + "extracts": extracts + } + + return json.dumps(config_json_, indent=2) + + +def data_preparation(mgrs_grid, grid_edge_size, lanelet2_map_path, extract_dir) -> list[str]: + # Create gpkg dataset and layers + # -------------------------------------------------------------------------------- + Debug.log("Creating GDAL driver and GPKG layers.", DebugMessageType.INFO) + driverName = "GPKG" + drv = gdal.GetDriverByName(driverName) + if drv is None: + print("%s driver not available.\n" % driverName) + sys.exit(1) + + ds_grids = drv.Create(os.path.join(extract_dir, "output_layers.gpkg"), 0, 0, 0, gdal.GDT_Unknown) + if ds_grids is None: + print("Creation of output file failed.\n") + sys.exit(1) + + layer_grids = ds_grids.CreateLayer("grids", None, ogr.wkbPolygon) + if layer_grids is None: + print("Layer creation failed.\n") + sys.exit(1) + + layer_lanelet2_whole = ds_grids.CreateLayer("lanelet2_whole", None, ogr.wkbMultiLineString) + if layer_lanelet2_whole is None: + print("Layer creation failed layer_lanelet2_whole.\n") + sys.exit(1) + + # Create new layer for filtered grids + # -------------------------------------------------------------------------------- + layer_filtered_grids = ds_grids.CreateLayer("filtered_grids", None, ogr.wkbPolygon) + if layer_filtered_grids is None: + print("Layer creation failed.\n") + sys.exit(1) + + # Create Grid Layer + # -------------------------------------------------------------------------------- + Debug.log("Creating " + str(grid_edge_size) + " meters grid layer along the " + mgrs_grid + ".", + DebugMessageType.INFO) + start = time.time() + create_grid_layer(grid_edge_size, layer_grids, mgrs_grid) + end = time.time() + formatted = "{:.1f}".format(end - start) + Debug.log("Grid layer created. Lasted " + str(formatted) + " seconds.", DebugMessageType.SUCCESS) + + + # Make a multilinestring in order to use in filtering + # -------------------------------------------------------------------------------- + lanelet2_whole_mls = ogr.Geometry(ogr.wkbMultiLineString) + + # Generate the lanelet2_map linestring layer with gpkg for filtering + # -------------------------------------------------------------------------------- + generate_lanelet2_layer(mgrs_grid, lanelet2_map_path, lanelet2_whole_mls, layer_lanelet2_whole) + + # Filter and destroy feature + # -------------------------------------------------------------------------------- + Debug.log("Filtering the grid layer with input lanelet2 map.", DebugMessageType.INFO) + layer_grids.SetSpatialFilter(lanelet2_whole_mls) + + # Set filtered grid layer + # -------------------------------------------------------------------------------- + for grid in layer_grids: + geometry_grid = grid.GetGeometryRef() + + filtered_feature_grid = ogr.Feature(layer_filtered_grids.GetLayerDefn()) + filtered_feature_grid.SetGeometry(geometry_grid) + if layer_filtered_grids.CreateFeature(filtered_feature_grid) != 0: + print("Failed to create feature in shapefile.\n") + sys.exit(1) + + filtered_feature_grid.Destroy() + + # Create yaml data and write to file + # -------------------------------------------------------------------------------- + Debug.log("Generating metadata.yaml for Dynamic Lanelet2 Map Loading.", DebugMessageType.INFO) + + metadata_yaml = generate_yaml_dict(layer_filtered_grids, grid_edge_size, mgrs_grid) + + with open(os.path.join(extract_dir, "lanelet2_map_metadata.yaml"), 'w', ) as f: + yaml.dump(metadata_yaml, f, default_flow_style=None, sort_keys=False) + + # Create config.json for Osmium Extract + # -------------------------------------------------------------------------------- + Debug.log("Generating config.json for Osmium Extract.", DebugMessageType.INFO) + + config_files = [] + config_name_counter = 1 + total_feature_count = layer_filtered_grids.GetFeatureCount() + + if total_feature_count > 500: + fid_list = [] + for i in range(1, total_feature_count+1): + fid_list.append(i) # add fid into fid_list + if ((i % 500) == 0) or (i == total_feature_count): + dup_layer_grids = layer_filtered_grids + dup_layer_grids.SetAttributeFilter("FID IN {}".format(tuple(fid_list))) + + config_json = generate_config_json(dup_layer_grids, extract_dir) + config_json_name = "config" + str(config_name_counter) + ".json" + with open(os.path.join(extract_dir, config_json_name), "w") as write_file: + write_file.write(config_json) + config_files.append(os.path.join(extract_dir, config_json_name)) + config_name_counter += 1 + fid_list.clear() + else: + config_json = generate_config_json(layer_filtered_grids, extract_dir) + config_json_name = "config" + str(config_name_counter) + ".json" + with open(os.path.join(extract_dir, config_json_name), "w") as write_file: + write_file.write(config_json) + config_files.append(os.path.join(extract_dir, config_json_name)) + + # return os.path.join(extract_dir, "config.json") + return config_files diff --git a/map/autoware_lanelet2_divider/autoware_lanelet2_divider/debug.py b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/debug.py new file mode 100644 index 00000000..af8a899d --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/debug.py @@ -0,0 +1,26 @@ +from enum import Enum +from datetime import datetime + + +class DebugMessageType(Enum): + INFO = {"value": "INFO", "color": "\033[94m"} # Blue + ERROR = {"value": "ERROR", "color": "\033[91m"} # Red + SUCCESS = {"value": "SUCCESS", "color": "\033[92m"} # Green + + +class Debug: + @classmethod + def log(cls, message, message_type=DebugMessageType.INFO): + current_time = datetime.now().strftime("%H:%M:%S") + color = message_type.value["color"] + reset_color = "\033[0m" # Reset color to default + formatted_message = f"[{current_time} - {color}{message_type.value['value']}{reset_color}] {message}" + print(formatted_message) + + @classmethod + def get_log(cls, message, message_type=DebugMessageType.INFO): + current_time = datetime.now().strftime("%H:%M:%S") + color = message_type.value["color"] + reset_color = "\033[0m" # Reset color to default + formatted_message = f"[{current_time} - {color}{message_type.value['value']}{reset_color}] {message}" + return formatted_message diff --git a/map/autoware_lanelet2_divider/autoware_lanelet2_divider/osmium_tool/osmium_tool.py b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/osmium_tool/osmium_tool.py new file mode 100644 index 00000000..077d4e03 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/osmium_tool/osmium_tool.py @@ -0,0 +1,48 @@ +import os +import subprocess + +from autoware_lanelet2_divider.debug import Debug, DebugMessageType + + +def extract_osm_file(input_osm_file_path: str, input_config_file_path: str, output_dir: str) -> bool: + command = f"osmium extract -v -c {input_config_file_path} -s complete_ways -S types=any --overwrite {input_osm_file_path}" + result = subprocess.run(command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + + if result.returncode == 0: + Debug.log(f"Extracted osm file: {input_osm_file_path}", DebugMessageType.SUCCESS) + return True + else: + if "Output directory is missing or not accessible" in result.stderr: + Debug.log( + f"Cannot extracted osm file: {input_osm_file_path}, Output directory is missing or not accessible", + DebugMessageType.ERROR) + Debug.log(f"Creating output directory: {output_dir}", DebugMessageType.INFO) + os.mkdir(output_dir) + extract_osm_file(input_osm_file_path, input_config_file_path, output_dir) + return True + elif "Way IDs out of order" in result.stderr: + Debug.log(f"Cannot extracted osm file: {input_osm_file_path}, Way IDs out of order", + DebugMessageType.ERROR) + Debug.log(f"Sorting osm file: {input_osm_file_path}", DebugMessageType.INFO) + sorted_osm_file = sort_osm_file(input_osm_file_path) + extract_osm_file(sorted_osm_file, input_config_file_path, output_dir) + return True + elif "Relation IDs out of order" in result.stderr: + Debug.log(f"Cannot extracted osm file: {input_osm_file_path}, Way IDs out of order", + DebugMessageType.ERROR) + Debug.log(f"Sorting osm file: {input_osm_file_path}", DebugMessageType.INFO) + sorted_osm_file = sort_osm_file(input_osm_file_path) + extract_osm_file(sorted_osm_file, input_config_file_path, output_dir) + return True + else: + Debug.log(f"Cannot extracted osm file: {input_osm_file_path}, {result.stderr}", DebugMessageType.ERROR) + print(result.stderr) + return False + + +def sort_osm_file(input_osm_file_path: str) -> str: + sorted_osm_file_path = input_osm_file_path.replace(".osm", "_sorted.osm") + command = f"osmium sort {input_osm_file_path} -o {sorted_osm_file_path}" + result = subprocess.run(command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + + return sorted_osm_file_path diff --git a/map/autoware_lanelet2_divider/autoware_lanelet2_divider/xml_tool/xml_tool.py b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/xml_tool/xml_tool.py new file mode 100644 index 00000000..ca7542d0 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/xml_tool/xml_tool.py @@ -0,0 +1,106 @@ +import xml.etree.ElementTree as ET +import os +from tqdm import tqdm + +from autoware_lanelet2_divider.debug import Debug, DebugMessageType + + +def select_tags(root, node_list, way_list, relation_list) -> None: + node_list.clear() + way_list.clear() + relation_list.clear() + for child in root: + if child.tag == "node": + node_list.append(child) + elif child.tag == "way": + way_list.append(child) + elif child.tag == "relation": + relation_list.append(child) + else: + pass + + +def complete_missing_elements(input_osm_file_path: str, input_extracted_osm_folder: str) -> bool: + Debug.log(f"Completing missing elements in osm file: {input_osm_file_path}", DebugMessageType.INFO) + + whole_map_xml = ET.parse(input_osm_file_path) + whole_map_root = whole_map_xml.getroot() + + node_list = [] + way_list = [] + relation_list = [] + + select_tags(whole_map_root, node_list, way_list, relation_list) + + osm_files = [f for f in os.listdir(input_extracted_osm_folder) if f.endswith(".osm")] + for osm_file in tqdm(osm_files, desc=Debug.get_log("Completing missing elements in osm file", DebugMessageType.INFO)): + divided_map_xml = ET.parse(input_extracted_osm_folder + "/" + osm_file) + divided_map_root = divided_map_xml.getroot() + + divided_node_list = [] + divided_way_list = [] + divided_relation_list = [] + + select_tags(divided_map_root, divided_node_list, divided_way_list, divided_relation_list) + + for relation in divided_relation_list: + relation_refs = [member for member in relation.iter("member")] + for r in relation_refs: + if r.attrib["type"] == "way": + if r.attrib["ref"] not in [way.attrib["id"] for way in divided_way_list]: + for way in way_list: + if way.attrib["id"] == r.attrib["ref"]: + divided_map_root.append(way) + select_tags(divided_map_root, divided_node_list, divided_way_list, divided_relation_list) + elif r.attrib["type"] == "relation": + if r.attrib["ref"] not in [rela.attrib["id"] for rela in divided_relation_list]: + for rel in relation_list: + if rel.attrib["id"] == r.attrib["ref"]: + divided_map_root.append(rel) + select_tags(divided_map_root, divided_node_list, divided_way_list, divided_relation_list) + + # Iterate on divided map's ways and find missing nodes + for way in divided_way_list: + nd = [nd.attrib["ref"] for nd in way.iter("nd")] + for n in nd: + if n not in [node.attrib["id"] for node in divided_node_list]: + # find the node in the whole map and add it to the divided map + for node in node_list: + if node.attrib["id"] == n: + divided_map_root.append(node) + select_tags(divided_map_root, divided_node_list, divided_way_list, divided_relation_list) + + divided_map_xml.write(input_extracted_osm_folder + "/" + osm_file) + + return True + + +def complete_missing_version_tag(input_osm_file_path: str): + Debug.log(f"Completing missing version tags in osm file: {input_osm_file_path}", DebugMessageType.INFO) + + whole_map_xml = ET.parse(input_osm_file_path) + whole_map_root = whole_map_xml.getroot() + + add_version = False + for root_element in whole_map_root.attrib: + if root_element != "version": + add_version = True + else: + add_version = False + break + + if add_version: + whole_map_root.set("version", "0.6") + + for element in tqdm(whole_map_root, desc=Debug.get_log("Completing missing version tags in osm file", DebugMessageType.INFO)): + add_version = False + for root_element in element.attrib: + if root_element != "version": + add_version = True + else: + add_version = False + break + if add_version: + element.set("version", "1") + + whole_map_xml.write(input_osm_file_path, encoding='utf-8', xml_declaration=True) diff --git a/map/autoware_lanelet2_divider/config/lanelet2_divider.param.yaml b/map/autoware_lanelet2_divider/config/lanelet2_divider.param.yaml new file mode 100644 index 00000000..a5bf8e85 --- /dev/null +++ b/map/autoware_lanelet2_divider/config/lanelet2_divider.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + input_lanelet2_map_path: "/path/to/lanelet2_map.osm" + output_folder_path: "/path/to/output_folder" + mgrs_grid: "35TPF" + grid_edge_size: 1000 \ No newline at end of file diff --git a/map/autoware_lanelet2_divider/docs/img_ytu_extended.png b/map/autoware_lanelet2_divider/docs/img_ytu_extended.png new file mode 100644 index 00000000..8b23aa72 Binary files /dev/null and b/map/autoware_lanelet2_divider/docs/img_ytu_extended.png differ diff --git a/map/autoware_lanelet2_divider/docs/img_ytu_extended_layers.png b/map/autoware_lanelet2_divider/docs/img_ytu_extended_layers.png new file mode 100644 index 00000000..ced39882 Binary files /dev/null and b/map/autoware_lanelet2_divider/docs/img_ytu_extended_layers.png differ diff --git a/map/autoware_lanelet2_divider/docs/img_ytu_extended_layers_full.png b/map/autoware_lanelet2_divider/docs/img_ytu_extended_layers_full.png new file mode 100644 index 00000000..37aff2bc Binary files /dev/null and b/map/autoware_lanelet2_divider/docs/img_ytu_extended_layers_full.png differ diff --git a/map/autoware_lanelet2_divider/docs/img_ytu_layers.png b/map/autoware_lanelet2_divider/docs/img_ytu_layers.png new file mode 100644 index 00000000..01826e2c Binary files /dev/null and b/map/autoware_lanelet2_divider/docs/img_ytu_layers.png differ diff --git a/map/autoware_lanelet2_divider/docs/img_ytu_original.png b/map/autoware_lanelet2_divider/docs/img_ytu_original.png new file mode 100644 index 00000000..5494e9e1 Binary files /dev/null and b/map/autoware_lanelet2_divider/docs/img_ytu_original.png differ diff --git a/map/autoware_lanelet2_divider/package.xml b/map/autoware_lanelet2_divider/package.xml new file mode 100644 index 00000000..f76591f1 --- /dev/null +++ b/map/autoware_lanelet2_divider/package.xml @@ -0,0 +1,19 @@ + + + + autoware_lanelet2_divider + 1.0.0 + A tool to divide lanelet2 map into smaller pieces with respect to MGRS grid. + ataparlar + bzeren + Apache License 2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/map/autoware_lanelet2_divider/requirements.txt b/map/autoware_lanelet2_divider/requirements.txt new file mode 100644 index 00000000..d3d6275c --- /dev/null +++ b/map/autoware_lanelet2_divider/requirements.txt @@ -0,0 +1,8 @@ +pyyaml==6.0.1 +lark==0.11.3 +pytest==7.1.3 +utm==0.7.0 +mgrs==1.4.6 +gdal==3.4 +tqdm==4.66.2 +lanelet2==1.2.1 \ No newline at end of file diff --git a/map/autoware_lanelet2_divider/resource/autoware_lanelet2_divider b/map/autoware_lanelet2_divider/resource/autoware_lanelet2_divider new file mode 100644 index 00000000..e69de29b diff --git a/map/autoware_lanelet2_divider/setup.cfg b/map/autoware_lanelet2_divider/setup.cfg new file mode 100644 index 00000000..998d46e8 --- /dev/null +++ b/map/autoware_lanelet2_divider/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/autoware_lanelet2_divider +[install] +install_scripts=$base/lib/autoware_lanelet2_divider diff --git a/map/autoware_lanelet2_divider/setup.py b/map/autoware_lanelet2_divider/setup.py new file mode 100644 index 00000000..eb10d98e --- /dev/null +++ b/map/autoware_lanelet2_divider/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup + +package_name = 'autoware_lanelet2_divider' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['config/lanelet2_divider.param.yaml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ataparlar', + maintainer='bzeren', + maintainer_email='ataparlar@leodrive.ai', + maintainer_email='baris@leodrive.ai', + description='A tool to divide lanelet2 map into smaller pieces with respect to MGRS grid.', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'lanelet2_divider = autoware_lanelet2_divider.autoware_lanelet2_divider:main', + ], + }, +) diff --git a/map/autoware_lanelet2_divider/test/test_copyright.py b/map/autoware_lanelet2_divider/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/map/autoware_lanelet2_divider/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/map/autoware_lanelet2_divider/test/test_flake8.py b/map/autoware_lanelet2_divider/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/map/autoware_lanelet2_divider/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/map/autoware_lanelet2_divider/test/test_pep257.py b/map/autoware_lanelet2_divider/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/map/autoware_lanelet2_divider/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'