Merge branch 'main' into feat/separate-publish-and-read-net-status

Author: TetsuKawa

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",
+        ]
+    )

planning/static_centerline_generator/src/static_centerline_generator_node.cpp

@@ -47,6 +47,7 @@
 #include <lanelet2_projection/UTM.h>
 
 #include <chrono>
+#include <filesystem>
 #include <fstream>
 #include <limits>
 #include <memory>
@@ -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