Skip to content

Commit 984b5c9

Browse files
authored
Merge pull request #48 from toppers/varray
可変長配列対応!
2 parents f15c667 + 9c8b212 commit 984b5c9

17 files changed

+250
-40
lines changed

bindings/python/hako_binary/binary_io.py

+41
Original file line numberDiff line numberDiff line change
@@ -172,3 +172,44 @@ def writeBinary(binary_data, off, bin):
172172
def readBinary(binary_data, off, size):
173173
return binary_data[off:off+size]
174174

175+
class PduMetaData:
176+
PDU_META_DATA_SIZE = 24
177+
PDU_META_DATA_MAGICNO = 0x12345678
178+
PDU_META_DATA_VERSION = 1
179+
def __init__(self):
180+
self.magicno = 0
181+
self.version = 0
182+
self.base_off = 0
183+
self.heap_off = 0
184+
self.total_size = 0
185+
def set_empty(self):
186+
self.magicno = PduMetaData.PDU_META_DATA_MAGICNO
187+
self.version = PduMetaData.PDU_META_DATA_VERSION
188+
self.base_off = PduMetaData.PDU_META_DATA_SIZE
189+
def to_bytes(self):
190+
data = bytearray()
191+
data.extend(self.magicno.to_bytes(4, byteorder='little'))
192+
data.extend(self.version.to_bytes(4, byteorder='little'))
193+
data.extend(self.base_off.to_bytes(4, byteorder='little'))
194+
data.extend(self.heap_off.to_bytes(4, byteorder='little'))
195+
data.extend(self.total_size.to_bytes(4, byteorder='little'))
196+
return data
197+
198+
199+
class PduMetaDataParser:
200+
def __init__(self):
201+
self.meta = PduMetaData()
202+
203+
def load_pdu_meta(self, binary_data):
204+
if len(binary_data) < PduMetaData.PDU_META_DATA_SIZE:
205+
return None
206+
magicno = binTovalue("uint32", readBinary(binary_data, 0, 4))
207+
version = binTovalue("uint32", readBinary(binary_data, 4, 4))
208+
if magicno != PduMetaData.PDU_META_DATA_MAGICNO or version != PduMetaData.PDU_META_DATA_VERSION:
209+
return None
210+
self.meta.magicno = magicno
211+
self.meta.version = version
212+
self.meta.base_off = binTovalue("uint32", readBinary(binary_data, 8, 4))
213+
self.meta.heap_off = binTovalue("uint32", readBinary(binary_data, 12, 4))
214+
self.meta.total_size = binTovalue("uint32", readBinary(binary_data, 16, 4))
215+
return self.meta

bindings/python/hako_binary/binary_reader.py

+33-8
Original file line numberDiff line numberDiff line change
@@ -6,17 +6,24 @@
66

77
from hako_binary import binary_io
88
from hako_binary import offset_parser
9-
from hako_binary import offset_map
109

1110
def decode_base64(data):
1211
return base64.b64decode(data)
1312

1413
def binary_read(offmap, typename, binary_data):
1514
json_data = {}
16-
binary_read_recursive(offmap, binary_data, json_data, 0, typename)
15+
meta_parser = binary_io.PduMetaDataParser()
16+
meta = meta_parser.load_pdu_meta(binary_data)
17+
if meta is None:
18+
meta = binary_io.PduMetaData()
19+
meta.set_empty()
20+
#print("binary_data: size", len(binary_data))
21+
#print("meta.to_bytes(): size", len(meta.to_bytes()))
22+
binary_io.writeBinary(binary_data, 0, meta.to_bytes())
23+
binary_read_recursive(meta, offmap, binary_data, json_data, binary_io.PduMetaData.PDU_META_DATA_SIZE, typename)
1724
return json_data
1825

19-
def binary_read_recursive(offmap, binary_data, json_data, base_off, typename):
26+
def binary_read_recursive(meta: binary_io.PduMetaData, offmap, binary_data, json_data, base_off, typename):
2027
#lines = offmap[typename]
2128
lines = offmap.get(typename)
2229
for line in lines:
@@ -29,24 +36,42 @@ def binary_read_recursive(offmap, binary_data, json_data, base_off, typename):
2936
bin = binary_io.readBinary(binary_data, off, size)
3037
value = binary_io.binTovalue(type, bin)
3138
json_data[name] = value
32-
else:
33-
array_size = offset_parser.array_size(line)
39+
elif (offset_parser.is_array(line)):
3440
array_value = binary_io.readBinary(binary_data, off, size)
3541
json_data[name + '__raw' ] = array_value
3642
json_data[name] = binary_io.binToArrayValues(type, array_value)
43+
else: #varray
44+
array_size = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off, 4))
45+
offset_from_heap = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off + 4, 4))
46+
one_elm_size = size
47+
array_value = binary_io.readBinary(binary_data, meta.heap_off + offset_from_heap, one_elm_size * array_size)
48+
json_data[name + '__raw' ] = array_value
49+
json_data[name] = binary_io.binToArrayValues(type, array_value)
3750
else:
3851
if (offset_parser.is_single(line)):
3952
tmp_json_data = {}
40-
binary_read_recursive(offmap, binary_data, tmp_json_data, off, type)
53+
binary_read_recursive(meta, offmap, binary_data, tmp_json_data, off, type)
4154
json_data[name] = tmp_json_data
42-
else:
55+
elif (offset_parser.is_array(line)):
4356
i = 0
4457
array_size = offset_parser.array_size(line)
4558
one_elm_size = int(size / array_size)
4659
array_value = []
4760
while i < array_size:
4861
tmp_json_data = {}
49-
binary_read_recursive(offmap, binary_data, tmp_json_data, off + (i * one_elm_size), type)
62+
binary_read_recursive(meta, offmap, binary_data, tmp_json_data, off + (i * one_elm_size), type)
63+
array_value.append(tmp_json_data)
64+
i = i + 1
65+
json_data[name] = array_value
66+
else: #varray
67+
array_size = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off, 4))
68+
offset_from_heap = binary_io.binTovalue("int32", binary_io.readBinary(binary_data, off + 4, 4))
69+
one_elm_size = size
70+
i = 0
71+
array_value = []
72+
while i < array_size:
73+
tmp_json_data = {}
74+
binary_read_recursive(meta, offmap, binary_data, tmp_json_data, meta.heap_off + offset_from_heap + (i * one_elm_size), type)
5075
array_value.append(tmp_json_data)
5176
i = i + 1
5277
json_data[name] = array_value

bindings/python/hako_binary/binary_writer.py

+90-25
Original file line numberDiff line numberDiff line change
@@ -5,43 +5,108 @@
55

66
from hako_binary import binary_io
77
from hako_binary import offset_parser
8-
from hako_binary import offset_map
8+
9+
class DynamicAllocator:
10+
def __init__(self):
11+
self.data = bytearray()
12+
self.offset_map = {}
13+
14+
def add(self, bytes_data, expected_offset=None, key=None):
15+
if expected_offset is not None:
16+
current_size = len(self.data)
17+
if current_size < expected_offset:
18+
padding = bytearray(expected_offset - current_size)
19+
self.data.extend(padding)
20+
21+
offset = len(self.data)
22+
self.data.extend(bytes_data)
23+
24+
if key:
25+
self.offset_map[key] = offset
26+
27+
return offset
28+
29+
def to_array(self):
30+
return self.data
31+
32+
def size(self):
33+
return len(self.data)
34+
35+
def get_offset(self, key):
36+
return self.offset_map.get(key, None)
37+
38+
class BinaryWriterContainer:
39+
def __init__(self):
40+
self.heap_allocator = DynamicAllocator()
41+
self.meta = binary_io.PduMetaData()
42+
self.meta.set_empty()
943

1044
def binary_write(offmap, binary_data, json_data, typename):
11-
binary_write_recursive(offmap, binary_data, json_data, 0, typename)
45+
base_allocator = DynamicAllocator()
46+
bw_container = BinaryWriterContainer()
47+
binary_write_recursive(bw_container, offmap, base_allocator, json_data, typename)
48+
49+
# メタデータの設定
50+
total_size = base_allocator.size() + bw_container.heap_allocator.size() + binary_io.PduMetaData.PDU_META_DATA_SIZE
51+
bw_container.meta.total_size = total_size
52+
bw_container.meta.heap_off = binary_io.PduMetaData.PDU_META_DATA_SIZE + base_allocator.size()
53+
54+
# binary_data のサイズを total_size に調整
55+
if len(binary_data) < total_size:
56+
binary_data.extend(bytearray(total_size - len(binary_data)))
57+
elif len(binary_data) > total_size:
58+
del binary_data[total_size:]
59+
60+
# メタデータをバッファにコピー
61+
binary_io.writeBinary(binary_data, 0, bw_container.meta.to_bytes())
1262

13-
def binary_write_recursive(offmap, binary_data, json_data, base_off, typename):
14-
#lines = offmap[typename]
63+
# 基本データをバッファにコピー
64+
binary_io.writeBinary(binary_data, bw_container.meta.base_off, base_allocator.to_array())
65+
66+
# ヒープデータをバッファにコピー
67+
binary_io.writeBinary(binary_data, bw_container.meta.heap_off, bw_container.heap_allocator.to_array())
68+
69+
def get_binary(type, bin, elm_size):
70+
if type == "string":
71+
buffer = bytearray(elm_size)
72+
buffer[:len(bin)] = bin
73+
return buffer
74+
else:
75+
return bin
76+
77+
def binary_write_recursive(bw_container: BinaryWriterContainer, offmap, allocator, json_data, typename):
1578
lines = offmap.get(typename)
1679
for key in json_data:
1780
line = offset_parser.select_by_name(lines, key)
1881
if line is None:
1982
continue
20-
off = offset_parser.member_off(line) + base_off
2183
type = offset_parser.member_type(line)
22-
if (offset_parser.is_primitive(line)):
23-
if (offset_parser.is_single(line)):
84+
off = offset_parser.member_off(line)
85+
if offset_parser.is_primitive(line):
86+
if offset_parser.is_single(line):
2487
bin = binary_io.typeTobin(type, json_data[key])
25-
binary_io.writeBinary(binary_data, off, bin)
26-
else:
27-
i = 0
88+
bin = get_binary(type, bin, offset_parser.member_size(line))
89+
# print(f"{type} {key} = {json_data[key]} : bin: {bin} size: {offset_parser.member_size(line)} bin_size: {len(bin)}")
90+
allocator.add(bin, expected_offset=off)
91+
elif offset_parser.is_array(line):
2892
elm_size = offset_parser.member_size(line)
2993
array_size = offset_parser.array_size(line)
3094
one_elm_size = int(elm_size / array_size)
31-
for elm in json_data[key]:
95+
for i, elm in enumerate(json_data[key]):
96+
bin = binary_io.typeTobin(type, elm)
97+
bin = get_binary(type, bin, one_elm_size)
98+
allocator.add(bin, expected_offset=(off + i * one_elm_size))
99+
else: # varray
100+
for i, elm in enumerate(json_data[key]):
32101
bin = binary_io.typeTobin(type, elm)
33-
binary_io.writeBinary(binary_data, off + (i * one_elm_size), bin)
34-
i = i + 1
102+
bin = get_binary(type, bin, offset_parser.member_size(line))
103+
bw_container.heap_allocator.add(bin, expected_offset=(off + i * offset_parser.member_size(line)))
35104
else:
36-
if (offset_parser.is_single(line)):
37-
binary_write_recursive(offmap, binary_data, json_data[key], off, type)
38-
else:
39-
i = 0
40-
elm_size = offset_parser.member_size(line)
41-
array_size = offset_parser.array_size(line)
42-
one_elm_size = int(elm_size / array_size)
43-
for elm in json_data[key]:
44-
binary_write_recursive(offmap, binary_data, elm, off + (i * one_elm_size), type)
45-
i = i + 1
46-
47-
105+
if offset_parser.is_single(line):
106+
binary_write_recursive(bw_container, offmap, allocator, json_data[key], type)
107+
elif offset_parser.is_array(line):
108+
for i, elm in enumerate(json_data[key]):
109+
binary_write_recursive(bw_container, offmap, allocator, elm, type)
110+
else: # varray
111+
for i, elm in enumerate(json_data[key]):
112+
binary_write_recursive(bw_container, offmap, bw_container.heap_allocator, elm, type)

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,4 +2,4 @@ single:primitive:name:string:0:128
22
single:primitive:version:uint32:128:4
33
single:primitive:asset_time:int64:136:8
44
single:primitive:ext_off:uint32:144:4
5-
single:primitive:ext_size:uint32:148:4
5+
single:primitive:ext_size:uint32:148:4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
single:primitive:time_usec:uint64:0:8
2+
array:primitive:controls:float32:8:64:16
3+
single:primitive:mode:uint8:72:1
4+
single:primitive:flags:uint64:80:8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
single:primitive:time_usec:uint64:0:8
2+
single:primitive:lat:int32:8:4
3+
single:primitive:lon:int32:12:4
4+
single:primitive:alt:int32:16:4
5+
single:primitive:eph:uint16:20:2
6+
single:primitive:epv:uint16:22:2
7+
single:primitive:vel:uint16:24:2
8+
single:primitive:vn:int16:26:2
9+
single:primitive:ve:int16:28:2
10+
single:primitive:vd:int16:30:2
11+
single:primitive:cog:uint16:32:2
12+
single:primitive:satellites_visible:uint8:34:1
13+
single:primitive:id:uint8:35:1
14+
single:primitive:yaw:uint8:36:1
15+
single:primitive:fix_type:uint8:37:1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
single:primitive:time_usec:uint64:0:8
2+
single:primitive:xacc:float32:8:4
3+
single:primitive:yacc:float32:12:4
4+
single:primitive:zacc:float32:16:4
5+
single:primitive:xgyro:float32:20:4
6+
single:primitive:ygyro:float32:24:4
7+
single:primitive:zgyro:float32:28:4
8+
single:primitive:xmag:float32:32:4
9+
single:primitive:ymag:float32:36:4
10+
single:primitive:zmag:float32:40:4
11+
single:primitive:abs_pressure:float32:44:4
12+
single:primitive:diff_pressure:float32:48:4
13+
single:primitive:pressure_alt:float32:52:4
14+
single:primitive:temperature:float32:56:4
15+
single:primitive:fields_updated:uint32:60:4
16+
single:primitive:id:uint8:64:1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
single:primitive:time_usec:uint64:0:8
2+
array:primitive:attitude_quaternion:float32:8:16:4
3+
single:primitive:rollspeed:float32:24:4
4+
single:primitive:pitchspeed:float32:28:4
5+
single:primitive:yawspeed:float32:32:4
6+
single:primitive:lat:int32:36:4
7+
single:primitive:lon:int32:40:4
8+
single:primitive:alt:int32:44:4
9+
single:primitive:vx:int16:48:2
10+
single:primitive:vy:int16:50:2
11+
single:primitive:vz:int16:52:2
12+
single:primitive:ind_airspeed:uint16:54:2
13+
single:primitive:true_airspeed:uint16:56:2
14+
single:primitive:xacc:int16:58:2
15+
single:primitive:yacc:int16:60:2
16+
single:primitive:zacc:int16:62:2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
single:primitive:aaa:int32:0:4
2+
array:primitive:fixed_str:string:4:256:2
3+
varray:primitive:varray_str:string:260:128:8
4+
array:struct:fixed_array:SimpleVarray:268:120:5
5+
varray:struct:data:SimpleVarray:388:24:8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
varray:primitive:data:int8:0:1:8
2+
array:primitive:fixed_array:int8:8:10:10
3+
single:primitive:p_mem1:int32:20:4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
single:primitive:forward_r:int16:0:2
2+
single:primitive:forward_l:int16:2:2
3+
single:primitive:left:int16:4:2
4+
single:primitive:right:int16:6:2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
single:primitive:detect_f:bool:0:4
2+
single:primitive:detect_r:bool:4:4
3+
single:primitive:detect_l:bool:8:4
4+
single:primitive:detect_b:bool:12:4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
single:struct:acc:geometry_msgs/Vector3:0:24
2+
single:struct:gyro:geometry_msgs/Vector3:24:24
3+
single:struct:mag:geometry_msgs/Vector3:48:24

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

+2-2
Original file line numberDiff line numberDiff line change
@@ -6,5 +6,5 @@ single:primitive:time_increment:float32:148:4
66
single:primitive:scan_time:float32:152:4
77
single:primitive:range_min:float32:156:4
88
single:primitive:range_max:float32:160:4
9-
array:primitive:ranges:float32:164:1440:360
10-
array:primitive:intensities:float32:1604:1440:360
9+
varray:primitive:ranges:float32:164:4:8
10+
varray:primitive:intensities:float32:172:4:8

bindings/python/hako_binary/offset_parser.py

+8
Original file line numberDiff line numberDiff line change
@@ -28,9 +28,17 @@ def is_primitive(data):
2828
def is_single(data):
2929
return data.split(":")[0] == "single"
3030

31+
def is_array(data):
32+
return data.split(":")[0] == "array"
33+
34+
def is_varray(data):
35+
return data.split(":")[0] == "varray"
36+
3137
def array_size(data):
3238
if (is_single(data)):
3339
return 0
40+
if (is_varray(data)):
41+
return -1
3442
return int(data.split(":")[6])
3543

3644
def member_off(data):

bindings/python/hako_pdu.py

+1
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
from hako_binary import offset_map
77
from hako_binary import binary_writer
88
from hako_binary import binary_reader
9+
from hako_binary import binary_io
910

1011
class PduBinaryConvertor:
1112
def __init__(self, offset_path, pdudef_path):

0 commit comments

Comments
 (0)