#pose definition for flimnap
#
# example:
# struct __attribute__((__packed__)) pose_t {
#     float x; /* m */
#     float y; /* m */
#     float pitch; /* rad */
#     float yaw; /* rad */
#     float roll; /* rad */
#     float steer; /* rad */
#     float rear_wheel; /* rad */
#     float v; /* m/s */
#     uint8_t timestamp;  /* Converted from system ticks to milliseconds
#                          * and contains only the least significant bits */
# }; /* 33 bytes */

import os
import re
import struct


def parse_format(source_file):
    with open(source_file, 'r') as f:
        start_pose_definition = False
        pose_format = '<'
        pose_format_text = ''
        for line in f:
            if not line:
                raise EOFError('Pose not fully defined in file {0}'.format(
                    source_file))

            if not start_pose_definition:
                if 'struct __attribute__((__packed__)) pose_t {' in line:
                    start_pose_definition = True
                    pose_format_text += line
            else:
                match = re.search('}; /\* ([0-9]+) bytes \*/', line)
                if match: # end of pose definition
                    pose_size = int(match.group(1))
                    pose_format_text += line
                    struct_size = struct.Struct(pose_format).size
                    assert struct_size == pose_size, \
                            '{0}, {1}'.format(pose_size, struct_size)
                    return (pose_format, pose_format_text)
                else:
                    words = line.split()
                    if words[1][-1] != ';':
                        # line is commented
                        assert words[0].startswith(('//', '/*', '*/', '*')), \
                                'Unexpected format for pose definition: ' + line
                        continue

                    t = words[0]
                    if t == 'float':
                        pose_format += 'f'
                    elif t == 'uint8_t':
                        pose_format += 'B'
                    else:
                        msg = 'Conversion for type {0} not handled'.format(t)
                        raise ValueError(msg)
                    pose_format_text += line


if __name__ == '__main__':
    local_dir = os.path.dirname(os.path.realpath(__file__))
    project_dir = os.path.join(local_dir, os.path.pardir, os.path.pardir, 'projects')
    flimnap_file = os.path.join(project_dir, 'flimnap', 'main.cc')
    print(parse_format(flimnap_file))

@@ -504,16 +504,34 @@
  struct%0A
+import numpy as np
 %0A%0Adef pa
@@ -631,36 +631,38 @@
 = False%0A        
-pose
+struct
 _format = '%3C'%0A  
@@ -659,16 +659,38 @@
 t = '%3C'%0A
+        np_dtype = %5B%5D%0A
         
@@ -1363,20 +1363,22 @@
 .Struct(
-pose
+struct
 _format)
@@ -1508,16 +1508,86 @@
 t_size)%0A
+                    assert np.dtype(np_dtype).itemsize == struct_size%0A
         
@@ -1610,20 +1610,22 @@
 rn (
-pose
+struct
 _format,
  pos
@@ -1620,16 +1620,36 @@
 _format,
+ np.dtype(np_dtype),
  pose_fo
@@ -2090,36 +2090,38 @@
                 
-pose
+struct
 _format += 'f'%0A 
@@ -2119,16 +2119,79 @@
  += 'f'%0A
+                        np_dtype.append((words%5B1%5D%5B:-1%5D, 'f4'))%0A
         
@@ -2243,28 +2243,30 @@
             
-pose
+struct
 _format += '
@@ -2268,16 +2268,79 @@
  += 'B'%0A
+                        np_dtype.append((words%5B1%5D%5B:-1%5D, 'u1'))%0A
         
@@ -2776,14 +2776,18 @@
     
-print(
+a, b, c = 
 pars
@@ -2804,14 +2804,52 @@
 limnap_file)
+%0A    print(a)%0A    print(b)%0A    print(c
 )%0A

Return numpy dtype in pose.parse_format