Add origin parser one test

scantools/scanners/navvis/origin_parser.py

@@ -0,0 +1,51 @@
+import json
+from pathlib import Path
+
+
+def convert_navvis_origin_to_csv(navvis_origin : dict):
+    csv_str = "# CRS, qw, qx, qy, qz, tx, ty, tz\n"
+
+    #CRS stands for Coordinate Reference System (CRS)
+    #Example: EPSG:25834 https://epsg.io/25834
+
+    if 'CRS' in navvis_origin:
+        crs = navvis_origin['CRS']
+    else:
+        crs = "unknown"
+
+    position = navvis_origin['Pose']['position']
+    orientation = navvis_origin['Pose']['orientation']
+
+    csv_str += (f"{crs},"
+                f"{orientation['w']},"
+                f"{orientation['x']},"
+                f"{orientation['y']},"
+                f"{orientation['z']},"
+                f"{position['x']},"
+                f"{position['y']},"
+                f"{position['z']}\n")
+
+    return csv_str
+
+
+def convert_navvis_origin_file_to_csv(file_path : Path):
+    """
+    Read NavVis SLAM Anchor Origin File Format Version 1.0 and write as csv file.
+    The navvis origin file is a json file stored in the 'anchors' folder.
+    :param file_path: Path to the file
+    :return: NavVis anchor origin
+    :rtype: Dict
+    """
+
+    if not file_path.exists():
+        return
+
+    with file_path.open() as f:
+        navvis_origin = json.load(f)
+        return convert_navvis_origin_to_csv(navvis_origin)
+
+
+
+
+
+

scantools/tests/test_origin_parser.py

@@ -0,0 +1,47 @@
+""" Tests for origin_parse.py """
+import pytest
+
+from ..scanners.navvis import origin_parser
+
+@pytest.mark.parametrize("input_data, expected_output_csv", [({
+    "CRS": "EPSG:25834",
+    "Pose": {
+        "orientation": {
+            "w": 0.8,
+            "x": 0,
+            "y": 0,
+            "z": -0.5
+        },
+        "position": {
+            "x": 6.3,
+            "y": 2.4,
+            "z": 99.95
+        }
+    }},
+     "# CRS, qw, qx, qy, qz, \
+        tx, ty, tz\n\
+        EPSG:25834,0.8,0,0,-0.5,\
+        6.3,2.4,99.95\n"),
+    ({
+        "Pose": {  
+            "orientation": {
+                "w": 0.5,
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            }
+        }
+    }, "# CRS, qw, qx, qy, qz, tx, ty, tz\nunknown,0.5,0,0,0,0,0,0\n"),
+])
+
+def test_parse_navvis_origin(input_data, expected_output_csv):
+    print("?",expected_output_csv)
+    csv = origin_parser.convert_navvis_origin_to_csv(input_data)
+    print("?",csv)
+    assert expected_output_csv.replace(" ","") == csv.replace(" ","")
+