rosmsg.py - FIXUP parse point clouds

robotika · Jan 5, 2019 · 341f10b · 341f10b
1 parent f0b1f5f
commit 341f10b
Showing 1 changed file with 9 additions and 3 deletions.
diff --git a/osgar/drivers/rosmsg.py b/osgar/drivers/rosmsg.py
@@ -211,13 +211,19 @@ def parse_points(data):
     is_bigendian, point_step, row_step = struct.unpack_from('<?II', data, pos)
     pos += 9
 #    print(is_bigendian, point_step, row_step)
-#    for i in range(10):
-#        print(struct.unpack_from('<ffff', data, pos + i * 32))
+    arr_size = struct.unpack_from('<I', data, pos)[0]
+    pos += 4
+    assert arr_size == height * width * 32, arr_size
+    nan = float('nan')
+#    for i in range(height*width):
+#        pt = struct.unpack_from('<ffff', data, pos + i * 32)
+#        assert str(pt) == str((nan, nan, nan, 0.0)), (i, pt)
+
     pos += row_step * height
     is_dense = struct.unpack_from('<?', data, pos)[0]
     pos += 1
 #    print(is_dense)
-    assert pos + 4 == len(data), (pos, len(data))  # why 4?! CRC?? alignment?
+    assert pos == len(data), (pos, len(data))
 
 
 class ROSMsgParser(Thread):