From 91d91538857b4ba8a4676cf53ceba9c527c4d5eb Mon Sep 17 00:00:00 2001
From: James R Heselden <primordia@live.com>
Date: Tue, 12 Sep 2023 08:50:54 +0100
Subject: [PATCH] openrmf convertor initiated

---
 .../convertors/openrmf_to_tmap.py             | 54 +++++++++++++++++++
 .../convertors/templating/tmap.py             |  4 +-
 2 files changed, 57 insertions(+), 1 deletion(-)
 create mode 100644 environment_common/convertors/openrmf_to_tmap.py

diff --git a/environment_common/convertors/openrmf_to_tmap.py b/environment_common/convertors/openrmf_to_tmap.py
new file mode 100644
index 0000000..bd184dc
--- /dev/null
+++ b/environment_common/convertors/openrmf_to_tmap.py
@@ -0,0 +1,54 @@
+import sys, os
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
+
+import yaml
+from pprint import pprint
+
+from environment_common.convertors.templating.tmap import TMapTemplates
+
+def run(args=None):
+
+    place_id = args['location_name']
+
+    rmf_path = os.path.join(args['src'], 'config', 'topological', 'rmf.building.yaml')
+    with open(rmf_path) as f:
+        data = f.read()
+        rmf = yaml.safe_load(data)
+
+
+    # Construct TMap
+    tmap = TMapTemplates.vert_start
+    tmap += TMapTemplates.vert_ring('vert1m', 1)
+
+    tmap += TMapTemplates.opening.format(**{'gen_time':0, 'location':place_id})
+
+    # Include Nodes and edges
+    node = {'location':place_id, 'vert': 'vert1m', 'restrictions':'robot', 'connections':None}
+    edge = {'action':'move_base', 'action_type':'move_base_msgs/MoveBaseGoal', 'restrictions':'robot'}
+
+    for k, level in rmf['levels'].items():
+        for i, n in enumerate(level['vertices']):
+            x, y = n[0], n[1]
+            name = f"{k}_{i}"
+            node.update({'name':name, 'x':x, 'y':y})
+            tmap += TMapTemplates.node.format(**node)
+            tmap += TMapTemplates.edges_empty
+
+    # Save TMap File
+    tmap_path = os.path.join(args['src'], 'config', 'topological', 'network_autogen.tmap2.yaml')
+    with open(tmap_path, 'w') as f:
+        f.write(tmap)
+
+
+
+    return rmf
+
+def main(args=None):
+    e = 'environment_template'
+    src = '/'.join(get_package_prefix(e).split('/')[:-2]) + f'/src/{e}'
+    location_name = 'riseholme_polytunnel'
+    args = {'src': src, 'location_name':location_name}
+    return run(args)
+
+if __name__ == '__main__':
+    rmf = main()
diff --git a/environment_common/convertors/templating/tmap.py b/environment_common/convertors/templating/tmap.py
index fda5988..ea861e7 100644
--- a/environment_common/convertors/templating/tmap.py
+++ b/environment_common/convertors/templating/tmap.py
@@ -115,7 +115,9 @@ class TMapTemplates:
     vert_start = """
 verts:
   verts:"""
-    vert_ring = """
+
+    def vert_ring(id, sz):
+        return f"""
   - verts: &{id}
     - x: {-0.130*sz}
       y: {0.213*sz}