File size: 1,741 Bytes
1f3a93e | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 | import msgpack
import msgpack_numpy as msgpack_numpy
import numpy as np
from gear_sonic.utils.teleop.input_readers import (
build_body_pose_sample,
decode_msgpack_byte_multi_array,
)
def test_decode_msgpack_byte_multi_array_from_byte_chunks():
payload = {
"timestamp": 123456789,
"joint_positions": [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]],
"joint_orientations": [[0.0, 0.0, 0.0, 1.0], [0.5, 0.5, 0.5, 0.5]],
}
packed = msgpack.packb(payload, default=msgpack_numpy.encode, use_bin_type=True)
byte_chunks = [bytes([value]) for value in packed]
decoded = decode_msgpack_byte_multi_array(
byte_chunks,
msgpack_module=msgpack,
msgpack_numpy_module=msgpack_numpy,
)
assert decoded["timestamp"] == payload["timestamp"]
assert decoded["joint_positions"] == payload["joint_positions"]
assert decoded["joint_orientations"] == payload["joint_orientations"]
def test_build_body_pose_sample_uses_existing_teleop_shape():
payload = {
"timestamp": 1_000_000_100,
"joint_positions": [[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]],
"joint_orientations": [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 1.0, 0.0]],
}
sample, stamp_ns, fps_ema = build_body_pose_sample(
payload,
prev_stamp_ns=1_000_000_000,
fps_ema=0.0,
)
assert sample is not None
assert stamp_ns == payload["timestamp"]
assert sample["body_poses_np"].shape == (24, 7)
np.testing.assert_allclose(sample["body_poses_np"][0], np.array([0.1, 0.2, 0.3, 0.0, 0.0, 0.0, 1.0]))
np.testing.assert_allclose(sample["body_poses_np"][1], np.array([0.4, 0.5, 0.6, 0.0, 0.0, 1.0, 0.0]))
assert sample["dt"] == 1e-7
assert fps_ema == 1.0 / 1e-7
|