Add initial AUTO code expansion

robotlab_safety/pluto_auto.py

+#!/usr/bin/python3
+
+import argparse
+import sys
+import re
+import tempfile
+import os
+
+class PlutoVariables:
+
+    def __init__(self, prefix):
+        self.prefix = prefix
+        self.by_name = dict()
+
+    def add(self, name, pluto, index):
+        self.by_name[name] = (int(pluto), int(index))
+        pass
+
+    def codegen(self, expr):
+        m = re.match('([^=]+)=ALL\((.*)\)', expr)
+        if m:
+            lh = m.group(1)
+            regexp = m.group(2).replace('*', '.*')
+            rh = [ "(%s+/SM_Pluto%d_Present)" % (v, self.by_name[v][0])
+                   for v in self.by_name if re.match(regexp, v)]
+            return("%s=%s" % (lh, '*'.join(rh)))
+        raise Exception('Invalid expression', expr)
+
+    
+class SPS:
+
+    def __init__(self, path):
+        self.data = open(path, encoding='utf-16').read()
+        self.gm = PlutoVariables('GM')
+        self.parse()
+
+    def parse(self):
+        for l in self.data.split('\n'):
+            # Collect variables
+            m = re.match('! GM([0-9]+)[.]([0-9]+)=(\S+)', l)
+            if m:
+                self.gm.add(m.group(3), m.group(1), m.group(2))
+                pass
+            pass
+        pass
+
+    def expand(self):
+        result = []
+        for l in self.data.split('\n'):
+            result.append(l)
+            m = re.match(';\s*AUTO:\s*(.*)', l)
+            if m:
+                code = self.gm.codegen(m.group(1))
+                if code != result[-2]:
+                    result[-2] = code
+                    pass
+                pass
+            pass
+        return '\n'.join(result)
+        
+
+if __name__ == '__main__':
+    optParser = argparse.ArgumentParser()
+    optParser.add_argument("sps", nargs='+',
+                           help="Pluto master programs")
+    optParser.add_argument("--write",
+                           action="store_true", default=False, 
+                           help="Rewrite source file if needed")
+    options = optParser.parse_args(sys.argv[1:])
+    for f in options.sps:
+        sps = SPS(f)
+        orig = sps.data
+        expanded = sps.expand()
+        if orig != expanded:
+            if options.write:
+                f1 = tempfile.NamedTemporaryFile(dir=os.path.dirname(f),
+                                                 delete=False)
+                print(f1.name)
+                f1.write(expanded.replace('\n', '\r\n').encode('utf-16'))
+                f1.close()
+                os.rename(f1.name, f)
+                pass
+            else:
+                for w, (l1, l2) in enumerate(zip(orig.split('\n'),
+                    expanded.split('\n'))):
+                    if (l1!=l2):
+                        print("%d '%s'" % (w, l1))
+                        print("%d '%s'" % (w, l2))