Merge pull request #48 from toppers/varray

可変長配列対応！

bindings/python/hako_binary/binary_io.py

@@ -172,3 +172,44 @@ def writeBinary(binary_data, off, bin):
 def readBinary(binary_data, off, size):
     return binary_data[off:off+size]
 
+class PduMetaData:
+    PDU_META_DATA_SIZE = 24
+    PDU_META_DATA_MAGICNO = 0x12345678
+    PDU_META_DATA_VERSION = 1
+    def __init__(self):
+        self.magicno = 0
+        self.version = 0
+        self.base_off = 0
+        self.heap_off = 0
+        self.total_size = 0
+    def set_empty(self):
+        self.magicno = PduMetaData.PDU_META_DATA_MAGICNO
+        self.version = PduMetaData.PDU_META_DATA_VERSION
+        self.base_off = PduMetaData.PDU_META_DATA_SIZE
+    def to_bytes(self):
+        data = bytearray()
+        data.extend(self.magicno.to_bytes(4, byteorder='little'))
+        data.extend(self.version.to_bytes(4, byteorder='little'))
+        data.extend(self.base_off.to_bytes(4, byteorder='little'))
+        data.extend(self.heap_off.to_bytes(4, byteorder='little'))
+        data.extend(self.total_size.to_bytes(4, byteorder='little'))
+        return data
+
+
+class PduMetaDataParser:
+    def __init__(self):
+        self.meta = PduMetaData()
+
+    def load_pdu_meta(self, binary_data):
+        if len(binary_data) < PduMetaData.PDU_META_DATA_SIZE:
+            return None
+        magicno = binTovalue("uint32", readBinary(binary_data, 0, 4))
+        version = binTovalue("uint32", readBinary(binary_data, 4, 4))
+        if magicno != PduMetaData.PDU_META_DATA_MAGICNO or version != PduMetaData.PDU_META_DATA_VERSION:
+            return None
+        self.meta.magicno    = magicno
+        self.meta.version    = version
+        self.meta.base_off   = binTovalue("uint32", readBinary(binary_data, 8, 4))
+        self.meta.heap_off   = binTovalue("uint32", readBinary(binary_data, 12, 4))
+        self.meta.total_size = binTovalue("uint32", readBinary(binary_data, 16, 4))
+        return self.meta

bindings/python/hako_binary/binary_reader.py

@@ -6,17 +6,24 @@
 
 from hako_binary import binary_io
 from hako_binary import offset_parser
-from hako_binary import offset_map
 
 def decode_base64(data):
     return base64.b64decode(data)
 
 def binary_read(offmap, typename, binary_data):
     json_data = {}
-    binary_read_recursive(offmap, binary_data, json_data, 0, typename)
+    meta_parser = binary_io.PduMetaDataParser()
+    meta = meta_parser.load_pdu_meta(binary_data)
+    if meta is None:
+        meta = binary_io.PduMetaData()
+        meta.set_empty()
+        #print("binary_data: size", len(binary_data))
+        #print("meta.to_bytes(): size", len(meta.to_bytes()))
+        binary_io.writeBinary(binary_data, 0, meta.to_bytes())
+    binary_read_recursive(meta, offmap, binary_data, json_data, binary_io.PduMetaData.PDU_META_DATA_SIZE, typename)
     return json_data
 
-def binary_read_recursive(offmap, binary_data, json_data, base_off, typename):
+def binary_read_recursive(meta: binary_io.PduMetaData, offmap, binary_data, json_data, base_off, typename):
     #lines = offmap[typename]
     lines = offmap.get(typename)
     for line in lines:
@@ -29,24 +36,42 @@ def binary_read_recursive(offmap, binary_data, json_data, base_off, typename):
                 bin = binary_io.readBinary(binary_data, off, size)
                 value = binary_io.binTovalue(type, bin)
                 json_data[name] = value
-            else:
-                array_size = offset_parser.array_size(line)
+            elif (offset_parser.is_array(line)):
                 array_value = binary_io.readBinary(binary_data, off, size)
                 json_data[name + '__raw' ] = array_value
                 json_data[name] = binary_io.binToArrayValues(type, array_value)
+            else: #varray
+                array_size = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off, 4))
+                offset_from_heap = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off + 4, 4))
+                one_elm_size = size
+                array_value = binary_io.readBinary(binary_data, meta.heap_off + offset_from_heap, one_elm_size * array_size)
+                json_data[name + '__raw' ] = array_value
+                json_data[name] = binary_io.binToArrayValues(type, array_value)
         else:
             if (offset_parser.is_single(line)):
                 tmp_json_data = {}
-                binary_read_recursive(offmap, binary_data, tmp_json_data, off, type)
+                binary_read_recursive(meta, offmap, binary_data, tmp_json_data, off, type)
                 json_data[name] = tmp_json_data
-            else:
+            elif (offset_parser.is_array(line)):
                 i = 0
                 array_size = offset_parser.array_size(line)
                 one_elm_size = int(size / array_size)
                 array_value = []
                 while i < array_size:
                     tmp_json_data = {}
-                    binary_read_recursive(offmap, binary_data, tmp_json_data, off + (i * one_elm_size), type)
+                    binary_read_recursive(meta, offmap, binary_data, tmp_json_data, off + (i * one_elm_size), type)
+                    array_value.append(tmp_json_data)
+                    i = i + 1
+                json_data[name] = array_value
+            else: #varray
+                array_size = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off, 4))
+                offset_from_heap = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off + 4, 4))
+                one_elm_size = size
+                i = 0
+                array_value = []
+                while i < array_size:
+                    tmp_json_data = {}
+                    binary_read_recursive(meta, offmap, binary_data, tmp_json_data, meta.heap_off + offset_from_heap + (i * one_elm_size), type)
                     array_value.append(tmp_json_data)
                     i = i + 1
                 json_data[name] = array_value

bindings/python/hako_binary/binary_writer.py

@@ -5,43 +5,108 @@
 
 from hako_binary import binary_io
 from hako_binary import offset_parser
-from hako_binary import offset_map
+
+class DynamicAllocator:
+    def __init__(self):
+        self.data = bytearray()
+        self.offset_map = {}
+
+    def add(self, bytes_data, expected_offset=None, key=None):
+        if expected_offset is not None:
+            current_size = len(self.data)
+            if current_size < expected_offset:
+                padding = bytearray(expected_offset - current_size)
+                self.data.extend(padding)
+
+        offset = len(self.data)
+        self.data.extend(bytes_data)
+
+        if key:
+            self.offset_map[key] = offset
+
+        return offset
+
+    def to_array(self):
+        return self.data
+
+    def size(self):
+        return len(self.data)
+
+    def get_offset(self, key):
+        return self.offset_map.get(key, None)
+
+class BinaryWriterContainer:
+    def __init__(self):
+        self.heap_allocator = DynamicAllocator()
+        self.meta = binary_io.PduMetaData()
+        self.meta.set_empty()
 
 def binary_write(offmap, binary_data, json_data, typename):
-    binary_write_recursive(offmap, binary_data, json_data, 0, typename)
+    base_allocator = DynamicAllocator()
+    bw_container = BinaryWriterContainer()
+    binary_write_recursive(bw_container, offmap, base_allocator, json_data, typename)
+
+    # メタデータの設定
+    total_size = base_allocator.size() + bw_container.heap_allocator.size() + binary_io.PduMetaData.PDU_META_DATA_SIZE
+    bw_container.meta.total_size = total_size
+    bw_container.meta.heap_off = binary_io.PduMetaData.PDU_META_DATA_SIZE + base_allocator.size()
+
+    # binary_data のサイズを total_size に調整
+    if len(binary_data) < total_size:
+        binary_data.extend(bytearray(total_size - len(binary_data)))
+    elif len(binary_data) > total_size:
+        del binary_data[total_size:]
+
+    # メタデータをバッファにコピー
+    binary_io.writeBinary(binary_data, 0, bw_container.meta.to_bytes())
 
-def binary_write_recursive(offmap, binary_data, json_data, base_off, typename):
-    #lines = offmap[typename]
+    # 基本データをバッファにコピー
+    binary_io.writeBinary(binary_data, bw_container.meta.base_off, base_allocator.to_array())
+
+    # ヒープデータをバッファにコピー
+    binary_io.writeBinary(binary_data, bw_container.meta.heap_off, bw_container.heap_allocator.to_array())
+
+def get_binary(type, bin, elm_size):
+    if type == "string":
+        buffer = bytearray(elm_size)
+        buffer[:len(bin)] = bin
+        return buffer
+    else:
+        return bin
+
+def binary_write_recursive(bw_container: BinaryWriterContainer, offmap, allocator, json_data, typename):
     lines = offmap.get(typename)
     for key in json_data:
         line = offset_parser.select_by_name(lines, key)
         if line is None:
             continue
-        off = offset_parser.member_off(line) + base_off
         type = offset_parser.member_type(line)
-        if (offset_parser.is_primitive(line)):
-            if (offset_parser.is_single(line)):
+        off = offset_parser.member_off(line)
+        if offset_parser.is_primitive(line):
+            if offset_parser.is_single(line):
                 bin = binary_io.typeTobin(type, json_data[key])
-                binary_io.writeBinary(binary_data, off, bin)
-            else:
-                i = 0
+                bin = get_binary(type, bin, offset_parser.member_size(line))
+                # print(f"{type} {key} = {json_data[key]} : bin: {bin} size: {offset_parser.member_size(line)} bin_size: {len(bin)}")
+                allocator.add(bin, expected_offset=off)
+            elif offset_parser.is_array(line):
                 elm_size = offset_parser.member_size(line)
                 array_size = offset_parser.array_size(line)
                 one_elm_size = int(elm_size / array_size)
-                for elm in json_data[key]:
+                for i, elm in enumerate(json_data[key]):
+                    bin = binary_io.typeTobin(type, elm)
+                    bin = get_binary(type, bin, one_elm_size)
+                    allocator.add(bin, expected_offset=(off + i * one_elm_size))
+            else:  # varray
+                for i, elm in enumerate(json_data[key]):
                     bin = binary_io.typeTobin(type, elm)
-                    binary_io.writeBinary(binary_data, off + (i * one_elm_size), bin)
-                    i = i + 1
+                    bin = get_binary(type, bin, offset_parser.member_size(line))
+                    bw_container.heap_allocator.add(bin, expected_offset=(off + i * offset_parser.member_size(line)))
         else:
-            if (offset_parser.is_single(line)):
-                binary_write_recursive(offmap, binary_data, json_data[key], off, type)
-            else:
-                i = 0
-                elm_size = offset_parser.member_size(line)
-                array_size = offset_parser.array_size(line)
-                one_elm_size = int(elm_size / array_size)
-                for elm in json_data[key]:
-                    binary_write_recursive(offmap, binary_data, elm, off + (i * one_elm_size), type)
-                    i = i + 1
-
-
+            if offset_parser.is_single(line):
+                binary_write_recursive(bw_container, offmap, allocator, json_data[key], type)
+            elif offset_parser.is_array(line):
+                for i, elm in enumerate(json_data[key]):
+                    binary_write_recursive(bw_container, offmap, allocator, elm, type)
+            else:  # varray
+                for i, elm in enumerate(json_data[key]):
+                    binary_write_recursive(bw_container, offmap, bw_container.heap_allocator, elm, type)

bindings/python/hako_binary/offset/ev3_msgs/Ev3PduActuatorHeader.offset

@@ -2,4 +2,4 @@ single:primitive:name:string:0:128
 single:primitive:version:uint32:128:4
 single:primitive:asset_time:int64:136:8
 single:primitive:ext_off:uint32:144:4
-single:primitive:ext_size:uint32:148:4
+single:primitive:ext_size:uint32:148:4

bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilActuatorControls.offset

@@ -0,0 +1,4 @@
+single:primitive:time_usec:uint64:0:8
+array:primitive:controls:float32:8:64:16
+single:primitive:mode:uint8:72:1
+single:primitive:flags:uint64:80:8

bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilGps.offset

@@ -0,0 +1,15 @@
+single:primitive:time_usec:uint64:0:8
+single:primitive:lat:int32:8:4
+single:primitive:lon:int32:12:4
+single:primitive:alt:int32:16:4
+single:primitive:eph:uint16:20:2
+single:primitive:epv:uint16:22:2
+single:primitive:vel:uint16:24:2
+single:primitive:vn:int16:26:2
+single:primitive:ve:int16:28:2
+single:primitive:vd:int16:30:2
+single:primitive:cog:uint16:32:2
+single:primitive:satellites_visible:uint8:34:1
+single:primitive:id:uint8:35:1
+single:primitive:yaw:uint8:36:1
+single:primitive:fix_type:uint8:37:1

bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilSensor.offset

@@ -0,0 +1,16 @@
+single:primitive:time_usec:uint64:0:8
+single:primitive:xacc:float32:8:4
+single:primitive:yacc:float32:12:4
+single:primitive:zacc:float32:16:4
+single:primitive:xgyro:float32:20:4
+single:primitive:ygyro:float32:24:4
+single:primitive:zgyro:float32:28:4
+single:primitive:xmag:float32:32:4
+single:primitive:ymag:float32:36:4
+single:primitive:zmag:float32:40:4
+single:primitive:abs_pressure:float32:44:4
+single:primitive:diff_pressure:float32:48:4
+single:primitive:pressure_alt:float32:52:4
+single:primitive:temperature:float32:56:4
+single:primitive:fields_updated:uint32:60:4
+single:primitive:id:uint8:64:1

bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilStateQuaternion.offset

@@ -0,0 +1,16 @@
+single:primitive:time_usec:uint64:0:8
+array:primitive:attitude_quaternion:float32:8:16:4
+single:primitive:rollspeed:float32:24:4
+single:primitive:pitchspeed:float32:28:4
+single:primitive:yawspeed:float32:32:4
+single:primitive:lat:int32:36:4
+single:primitive:lon:int32:40:4
+single:primitive:alt:int32:44:4
+single:primitive:vx:int16:48:2
+single:primitive:vy:int16:50:2
+single:primitive:vz:int16:52:2
+single:primitive:ind_airspeed:uint16:54:2
+single:primitive:true_airspeed:uint16:56:2
+single:primitive:xacc:int16:58:2
+single:primitive:yacc:int16:60:2
+single:primitive:zacc:int16:62:2

bindings/python/hako_binary/offset/hako_msgs/SimpleStructVarray.offset

@@ -0,0 +1,5 @@
+single:primitive:aaa:int32:0:4
+array:primitive:fixed_str:string:4:256:2
+varray:primitive:varray_str:string:260:128:8
+array:struct:fixed_array:SimpleVarray:268:120:5
+varray:struct:data:SimpleVarray:388:24:8

bindings/python/hako_binary/offset/hako_msgs/SimpleVarray.offset

@@ -0,0 +1,3 @@
+varray:primitive:data:int8:0:1:8
+array:primitive:fixed_array:int8:8:10:10
+single:primitive:p_mem1:int32:20:4

bindings/python/hako_binary/offset/pico_msgs/LightSensor.offset

@@ -0,0 +1,4 @@
+single:primitive:forward_r:int16:0:2
+single:primitive:forward_l:int16:2:2
+single:primitive:left:int16:4:2
+single:primitive:right:int16:6:2

bindings/python/hako_binary/offset/pico_msgs/PicoSensor.offset

@@ -0,0 +1,4 @@
+single:primitive:detect_f:bool:0:4
+single:primitive:detect_r:bool:4:4
+single:primitive:detect_l:bool:8:4
+single:primitive:detect_b:bool:12:4

bindings/python/hako_binary/offset/satelite_msgs/SateliteImu.offset

@@ -0,0 +1,3 @@
+single:struct:acc:geometry_msgs/Vector3:0:24
+single:struct:gyro:geometry_msgs/Vector3:24:24
+single:struct:mag:geometry_msgs/Vector3:48:24

bindings/python/hako_binary/offset/sensor_msgs/LaserScan.offset

@@ -6,5 +6,5 @@ single:primitive:time_increment:float32:148:4
 single:primitive:scan_time:float32:152:4
 single:primitive:range_min:float32:156:4
 single:primitive:range_max:float32:160:4
-array:primitive:ranges:float32:164:1440:360
-array:primitive:intensities:float32:1604:1440:360
+varray:primitive:ranges:float32:164:4:8
+varray:primitive:intensities:float32:172:4:8

bindings/python/hako_binary/offset_parser.py

@@ -28,9 +28,17 @@ def is_primitive(data):
 def is_single(data):
     return data.split(":")[0] == "single"
 
+def is_array(data):
+    return data.split(":")[0] == "array"
+
+def is_varray(data):
+    return data.split(":")[0] == "varray"
+
 def array_size(data):
     if (is_single(data)):
         return 0
+    if (is_varray(data)):
+        return -1
     return int(data.split(":")[6])
 
 def member_off(data):

bindings/python/hako_pdu.py

@@ -6,6 +6,7 @@
 from hako_binary import offset_map
 from hako_binary import binary_writer
 from hako_binary import binary_reader
+from hako_binary import binary_io
 
 class PduBinaryConvertor:
     def __init__(self, offset_path, pdudef_path):