diff --git a/map/autoware_lanelet2_divider/README.md b/map/autoware_lanelet2_divider/README.md new file mode 100644 index 00000000..de2bed8d --- /dev/null +++ b/map/autoware_lanelet2_divider/README.md @@ -0,0 +1,159 @@ +# autoware_lanelet2_divider + +This is a lanelet2 tile generator tool for Autoware dynamic lanelet2 map loading feature. + +Working principal of this tool is sequentially: + +- 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_lanelet2_divider](#autoware_lanelet2_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_lanelet2_divider Python Dependencies](#autoware_lanelet2_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 vertex 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 vertex 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 + + + +```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_lanelet2_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: + + +

+ 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: + + +

+ 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..2af594a5 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/__init__.py @@ -0,0 +1,7 @@ +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" 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..00a2dee9 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/autoware_lanelet2_divider.py @@ -0,0 +1,92 @@ +import os +import shutil + +import autoware_lanelet2_divider.data_preparation.data_preparation as data_preparation +from autoware_lanelet2_divider.debug import Debug +from autoware_lanelet2_divider.debug import DebugMessageType +import autoware_lanelet2_divider.osmium_tool.osmium_tool as osmium_tool +import autoware_lanelet2_divider.xml_tool.xml_tool as xml_tool +import rclpy +from rclpy.node import Node + + +class AutowareLanelet2Divider(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) + + # Create copy of osm file + shutil.copy( + self.input_lanelet2_map_path, self.input_lanelet2_map_path.replace(".osm", "_temp.osm") + ) + self.input_lanelet2_map_path = self.input_lanelet2_map_path.replace(".osm", "_temp.osm") + + # 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") + ) + + # Remove temp osm file + os.remove(self.input_lanelet2_map_path) + + 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 = AutowareLanelet2Divider() + 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_preparation/data_preparation.py b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/data_preparation/data_preparation.py new file mode 100644 index 00000000..652a0924 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/data_preparation/data_preparation.py @@ -0,0 +1,310 @@ +import json +import os +import sys +import time + +from autoware_lanelet2_divider.debug import Debug +from autoware_lanelet2_divider.debug import DebugMessageType +import lanelet2 +from lanelet2.projection import UtmProjector +import mgrs +from osgeo import gdal # cspell: ignore osgeo +from osgeo import ogr # cspell: ignore osgeo +from tqdm import tqdm +import utm +import yaml + + +def create_grid_layer(grid_edge_size, layer_grids, mgrs_grid) -> None: + """ + Create a grid layer of polygons in the specified GDAL layer. + + Parameters: + grid_edge_size (float): The size of each grid cell. + layer_grids (ogr.Layer): The layer in which to create the grid polygons. + mgrs_grid (str): The MGRS grid string used for location projection. + + Returns: + None + """ + mgrs_object = mgrs.MGRS() + zone, hemisphere, 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()) # cspell: ignore Defn + 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, hemisphere) + 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: + Debug.log("Failed to create feature in shapefile.", DebugMessageType.ERROR) + sys.exit(1) + + feature_grid.Destroy() + + +def generate_lanelet2_layer(mgrs_grid, lanelet2_map_path, lanelet2_whole_mls, layer_lanelet2_whole): + """ + Generate a Lanelet2 layer from the given Lanelet2 map path and adds it to the specified GDAL layer. + + Parameters: + mgrs_grid (str): The MGRS grid string used for location projection. + lanelet2_map_path (str): The file path to the Lanelet2 map. + lanelet2_whole_mls (ogr.Geometry): The MultiLineString geometry to which lanelet geometries will be added. + layer_lanelet2_whole (ogr.Layer): The layer to which the Lanelet2 features will be added. + + Returns: + None + """ + mgrs_object = mgrs.MGRS() + zone, hemisphere, origin_x, origin_y = mgrs_object.MGRSToUTM(mgrs_grid) + origin_lat, origin_lon = utm.to_latlon(origin_x, origin_y, zone, hemisphere) + + 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, hemisphere + ) + linestring.AddPoint_2D(node_lon, node_lat) + lanelet2_whole_mls.AddGeometry(linestring) + feature_lanelet2 = ogr.Feature(layer_lanelet2_whole.GetLayerDefn()) # cspell: ignore Defn + feature_lanelet2.SetGeometry(lanelet2_whole_mls) + if layer_lanelet2_whole.CreateFeature(feature_lanelet2) != 0: + Debug.log("Failed to create feature in shapefile.", DebugMessageType.ERROR) + sys.exit(1) + feature_lanelet2.Destroy() + + +def generate_yaml_dict(layer_filtered_grids, grid_edge_size, mgrs_grid) -> dict: + """ + Generate a YAML-compatible dictionary from the filtered grid layer. + + Parameters: + layer_filtered_grids (ogr.Feature): The layer containing filtered grid features. + grid_edge_size (float): The size of each grid cell. + mgrs_grid (str): The MGRS grid string used for location projection. + + Returns: + dict: A dictionary containing grid metadata for YAML output. + """ + mgrs_object = mgrs.MGRS() + zone, hemisphere, 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: + """ + Generate a configuration JSON string for Osmium Extract from the filtered grid layer. + + Parameters: + layer_filtered_grids (ogr.Feature): The layer containing filtered grid features. + extract_dir (str): The directory where the output files will be saved. + + Returns: + str: A JSON string containing the configuration for Osmium Extract. + """ + 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: str, grid_edge_size: int, lanelet2_map_path: str, extract_dir: str +) -> list[str]: + """ + Prepare the data by creating grid layers, generating Lanelet2 layers, and producing metadata files. + + Parameters: + mgrs_grid (str): The MGRS grid string used for location projection. + grid_edge_size (int): The size of each grid cell. + lanelet2_map_path (str): The file path to the Lanelet2 map. + extract_dir (str): The directory where the output files will be saved. + + Returns: + list[str]: A list of paths to the generated configuration JSON files. + """ + # Create gpkg dataset and layers + Debug.log("Create output directory if not exist.", DebugMessageType.INFO) + os.makedirs(extract_dir, exist_ok=True) + + Debug.log("Creating GDAL driver and GPKG layers.", DebugMessageType.INFO) + driverName = "GPKG" + drv = gdal.GetDriverByName(driverName) + if drv is None: + Debug.log("%s driver not available.\n" % driverName, DebugMessageType.ERROR) + 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: + Debug.log("Creation of output file failed.", DebugMessageType.ERROR) + sys.exit(1) + + layer_grids = ds_grids.CreateLayer("grids", None, ogr.wkbPolygon) + if layer_grids is None: + Debug.log("Layer creation failed.", DebugMessageType.ERROR) + sys.exit(1) + + layer_lanelet2_whole = ds_grids.CreateLayer("lanelet2_whole", None, ogr.wkbMultiLineString) + if layer_lanelet2_whole is None: + Debug.log("Layer creation failed layer_lanelet2_whole.", DebugMessageType.ERROR) + 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: + Debug.log("Layer creation failed.", DebugMessageType.ERROR) + 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() + ) # cspell: ignore Defn + filtered_feature_grid.SetGeometry(geometry_grid) + if layer_filtered_grids.CreateFeature(filtered_feature_grid) != 0: + Debug.log("Failed to create feature in shapefile.", DebugMessageType.ERROR) + 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() + maximum_feature_count = 500 + + if total_feature_count > maximum_feature_count: + fid_list = [] + for i in range(1, total_feature_count + 1): + fid_list.append(i) # add fid into fid_list + if ((i % maximum_feature_count) == 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..d9520045 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/debug.py @@ -0,0 +1,70 @@ +from datetime import datetime +from enum import Enum + + +class DebugMessageType(Enum): + """ + Enum representing different types of debug messages. + + Attributes: + INFO (dict): Message type for informational messages. + ERROR (dict): Message type for error messages. + SUCCESS (dict): Message type for success messages. + """ + + INFO = {"value": "INFO", "color": "\033[94m"} # Blue + ERROR = {"value": "ERROR", "color": "\033[91m"} # Red + SUCCESS = {"value": "SUCCESS", "color": "\033[92m"} # Green + + +class Debug: + """ + A class for logging debug messages with different types. + + Methods: + log(message, message_type=DebugMessageType.INFO): + Logs a message to the console with a timestamp and message type. + + get_log(message, message_type=DebugMessageType.INFO): + Returns a formatted log message string without printing it. + """ + + @classmethod + def log(cls, message, message_type=DebugMessageType.INFO): + """ + Log a message to the console with a timestamp and message type. + + Parameters: + message (str): The message to log. + message_type (DebugMessageType): The type of the message (default: INFO). + + Returns: + None + """ + 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): + """ + Return a formatted log message string without printing it. + + Parameters: + message (str): The message to format. + message_type (DebugMessageType): The type of the message (default: INFO). + + Returns: + str: The formatted log message string. + """ + 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..aba19379 --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/osmium_tool/osmium_tool.py @@ -0,0 +1,102 @@ +import os +import subprocess + +from autoware_lanelet2_divider.debug import Debug +from autoware_lanelet2_divider.debug import DebugMessageType + +# Error handler dictionary for osmium tool +# Key: Error message +# Value: List of functions to handle the error +# - First function: Function to log the error +# - Second function: Function to log the action +# - Third function: Function to execute the action +ERROR_HANDLERS = { + "Output directory is missing or not accessible": [ + lambda input_osm_file_path, input_config_file_path, output_dir: Debug.log( + f"Cannot extracted osm file: {input_osm_file_path}, Output directory is missing or not accessible", + DebugMessageType.ERROR, + ), + lambda input_osm_file_path, input_config_file_path, output_dir: f"Creating output directory: {output_dir}", + lambda input_osm_file_path, input_config_file_path, output_dir: os.makedirs( + output_dir, exist_ok=True + ), + ], + "Way IDs out of order / Relation IDs out of order": [ + lambda input_osm_file_path, input_config_file_path, output_dir: Debug.log( + f"Cannot extracted osm file: {input_osm_file_path}, Way IDs out of order", + DebugMessageType.ERROR, + ), + lambda input_osm_file_path, input_config_file_path, output_dir: Debug.log( + f"Sorting osm file: {input_osm_file_path}", DebugMessageType.INFO + ), + lambda input_osm_file_path, input_config_file_path, output_dir: sort_osm_file( + input_osm_file_path + ), + ], +} + + +def extract_osm_file( + input_osm_file_path: str, + input_config_file_path: str, + output_dir: str, + args: str = "-v -s complete_ways -S types=any", +) -> bool: + """ + Extract a specified .osm file using the osmium tool, with given arguments and configurations. + + Parameters: + input_osm_file_path (str): Path to the input .osm file. + input_config_file_path (str): Path to the configuration file for the osmium tool. + output_dir (str): Path to the directory where the output will be stored. + args (str): Additional arguments for osmium extraction command. Defaults to "-v -s complete_ways -S types=any". + + Returns: + bool: True if extraction is successful, False otherwise. + """ + command = f"osmium extract -c {input_config_file_path} --overwrite {input_osm_file_path} {args}" + 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 + + for error_message, (error_log, info_log, action) in ERROR_HANDLERS.items(): + if any(error in result.stderr for error in error_message.split(" / ")): + error_log(input_osm_file_path, input_config_file_path, output_dir) # Log the error + info_log(input_osm_file_path, input_config_file_path, output_dir) # Log the action + action_output = action( + input_osm_file_path, input_config_file_path, output_dir + ) # Execute the action + return extract_osm_file( + action_output if action_output is not None else input_osm_file_path, + input_config_file_path, + output_dir, + ) + + Debug.log( + f"Cannot extracted osm file: {input_osm_file_path}, {result.stderr}", + DebugMessageType.ERROR, + ) + return False + + +def sort_osm_file(input_osm_file_path: str) -> str: + """ + Sort a specified .osm file using the osmium tool to handle out-of-order Way or Relation IDs. + + It stores the sorted .osm file in the same directory as the input file with "_sorted.osm" appended to the filename. + + Parameters: + input_osm_file_path (str): Path to the input .osm file that needs sorting. + + Returns: + str: Path to the sorted .osm file. + """ + 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}" + 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..9b8fb6ae --- /dev/null +++ b/map/autoware_lanelet2_divider/autoware_lanelet2_divider/xml_tool/xml_tool.py @@ -0,0 +1,158 @@ +import os +import xml.etree.ElementTree as ET + +from autoware_lanelet2_divider.debug import Debug +from autoware_lanelet2_divider.debug import DebugMessageType +from tqdm import tqdm + + +def select_tags(root, node_list, way_list, relation_list) -> None: + """ + Populate lists for 'node', 'way', and 'relation' XML elements found in the given root element. + + Parameters: + root (ElementTree): The root XML element to parse. + node_list (list): List to store 'node' elements. + way_list (list): List to store 'way' elements. + relation_list (list): List to store 'relation' elements. + + Returns: + 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) + + +def complete_missing_elements(input_osm_file_path: str, input_extracted_osm_folder: str) -> bool: + """ + Complete missing elements (nodes, ways, relations) in the divided OSM files by checking against the whole map. + + Parameters: + input_osm_file_path (str): Path to the whole map OSM file. + input_extracted_osm_folder (str): Directory containing divided OSM files. + + Returns: + bool: True if the process completes successfully. + """ + 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: + for r in relation.iter("member"): + 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 + ]: # cspell: ignore rela + 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: + for n in [nd.attrib["ref"] for nd in way.iter("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): + """ + Add missing version attributes to the root and its child elements in an OSM file, if needed. + + Parameters: + input_osm_file_path (str): Path to the OSM file to update with version tags. + + Returns: + None + """ + 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 = any(root_element != "version" for root_element in whole_map_root.attrib) + + 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 + 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..7dd64159 --- /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 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..27ce2ecb --- /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 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..fcbbe093 --- /dev/null +++ b/map/autoware_lanelet2_divider/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages +from setuptools import 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", "bzeren"], + maintainer_email=["ataparlar@leodrive.ai", "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..95f03810 --- /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..49c1644f --- /dev/null +++ b/map/autoware_lanelet2_divider/test/test_flake8.py @@ -0,0 +1,23 @@ +# 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..a2c3deb8 --- /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"