From c40da9f1d312d88b13bf4a88124ac688df6d1c90 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 16 Apr 2024 20:55:46 +0900 Subject: [PATCH] feat(static_centerline_generator): add script to show diff of lanelet2 maps (#6804) * feat(static_centerline_generator): add script to show diff of lanelet2 maps Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix typo Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../scripts/show_lanelet2_map_diff.py | 139 ++++++++++++++++++ .../src/static_centerline_generator_node.cpp | 15 ++ 2 files changed, 154 insertions(+) create mode 100755 planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py diff --git a/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py new file mode 100755 index 0000000000000..912226511f1d9 --- /dev/null +++ b/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py @@ -0,0 +1,139 @@ +#!/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +from decimal import Decimal +import os +import subprocess +import xml.etree.ElementTree as ET + + +def sort_attributes(root): + for shallow_element in root: + # sort nodes + attrib = shallow_element.attrib + if len(attrib) > 1: + attributes = sorted(attrib.items()) + attrib.clear() + attrib.update(attributes) + if shallow_element.tag == "relation": + pass + + # sort the element in the tag + for deep_element in shallow_element: + attrib = deep_element.attrib + if len(attrib) > 1: + # adjust attribute order, e.g. by sorting + attributes = sorted(attrib.items()) + attrib.clear() + attrib.update(attributes) + + # sort tags + sorted_shallow_element = sorted(shallow_element, key=lambda x: x.items()) + if len(shallow_element) != 0: + # NOTE: This "tail" is related to the indent of the next tag + first_tail = shallow_element[0].tail + last_tail = shallow_element[-1].tail + for idx, val_shallow_element in enumerate(sorted_shallow_element): + shallow_element[idx] = val_shallow_element + if idx == len(shallow_element) - 1: + shallow_element[idx].tail = last_tail + else: + shallow_element[idx].tail = first_tail + + +def remove_diff_to_ignore(osm_root): + decimal_precision = 11 # for lat/lon values + + # remove attributes of the osm tag + osm_root.attrib.clear() + + # remove the MetaInfo tag generated by Vector Map Builder + if osm_root[0].tag == "MetaInfo": + osm_root.remove(osm_root[0]) + + # remove unnecessary attributes for diff + for osm_child in osm_root: + if osm_child.tag == "osm": + osm_child.attrib.pop("osm") + if "visible" in osm_child.attrib and osm_child.attrib["visible"]: + osm_child.attrib.pop("visible") + if "version" in osm_child.attrib and osm_child.attrib["version"]: + osm_child.attrib.pop("version") + if "action" in osm_child.attrib and osm_child.attrib["action"] == "modify": + osm_child.attrib.pop("action") + if "lat" in osm_child.attrib: + osm_child.attrib["lat"] = str( + Decimal(float(osm_child.attrib["lat"])).quantize(Decimal(f"1e-{decimal_precision}")) + ) + if "lon" in osm_child.attrib: + osm_child.attrib["lon"] = str( + Decimal(float(osm_child.attrib["lon"])).quantize(Decimal(f"1e-{decimal_precision}")) + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-s", "--sort-attributes", action="store_true", help="Sort attributes of LL2 maps" + ) + parser.add_argument( + "-i", + "--ignore-minor-attributes", + action="store_true", + help="Ignore minor attributes of LL2 maps which does not change the map's behavior", + ) + args = parser.parse_args() + + original_osm_file_name = "/tmp/static_centerline_generator/input/lanelet2_map.osm" + modified_osm_file_name = "/tmp/static_centerline_generator/output/lanelet2_map.osm" + + # load LL2 maps + original_osm_tree = ET.parse(original_osm_file_name) + original_osm_root = original_osm_tree.getroot() + modified_osm_tree = ET.parse(modified_osm_file_name) + modified_osm_root = modified_osm_tree.getroot() + + # sort attributes + if args.sort_attributes: + sort_attributes(modified_osm_root) + sort_attributes(original_osm_root) + + # ignore minor attributes + if args.ignore_minor_attributes: + remove_diff_to_ignore(original_osm_root) + remove_diff_to_ignore(modified_osm_root) + + # write LL2 maps + output_dir_path = "/tmp/static_centerline_generator/show_lanelet2_map_diff/" + os.makedirs(output_dir_path + "original/", exist_ok=True) + os.makedirs(output_dir_path + "modified/", exist_ok=True) + + original_osm_tree.write(output_dir_path + "original/lanelet2_map.osm") + modified_osm_tree.write(output_dir_path + "modified/lanelet2_map.osm") + + # show diff + print("[INFO] Show diff of following LL2 maps") + print(f" {output_dir_path + 'original/lanelet2_map.osm'}") + print(f" {output_dir_path + 'modified/lanelet2_map.osm'}") + subprocess.run( + [ + "diff", + output_dir_path + "original/lanelet2_map.osm", + output_dir_path + "modified/lanelet2_map.osm", + "--color", + ] + ) diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index ec7d6278e346d..750b2e68b16b0 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -336,6 +337,13 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { + // copy the input LL2 map to the temporary file for debugging + const std::string debug_input_file_dir{"/tmp/static_centerline_generator/input/"}; + std::filesystem::create_directories(debug_input_file_dir); + std::filesystem::copy( + lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm", + std::filesystem::copy_options::overwrite_existing); + // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map @@ -635,5 +643,12 @@ void StaticCenterlineGeneratorNode::save_map( // save map with modified center line lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_); RCLCPP_INFO(get_logger(), "Saved map."); + + // copy the output LL2 map to the temporary file for debugging + const std::string debug_output_file_dir{"/tmp/static_centerline_generator/output/"}; + std::filesystem::create_directories(debug_output_file_dir); + std::filesystem::copy( + lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", + std::filesystem::copy_options::overwrite_existing); } } // namespace static_centerline_generator