def Xtest_parse_odom(self):
r = ROSMsgParser(config={}, bus=None)
with open('odom_data.bin', 'rb') as f:
# with open('x3-odom.bin', 'rb') as f:
r._buf += f.read()
index = 0
packet = r.get_packet() # first packet is structure file
while packet is not None:
if index > 0:
parse_odom(packet)
packet = r.get_packet()
index += 1
if index > 10:
break
def Xtest_parse_laser(self):
r = ROSMsgParser(config={}, bus=None)
with open('laser_data2.bin', 'rb') as f:
r._buf += f.read()
index = 0
packet = r.get_packet() # first packet is structure file
while packet is not None:
if index > 0:
scan = parse_laser(packet)
print(scan)
self.assertEqual(len(scan), 720)
self.assertEqual(type(scan[0]), int)
packet = r.get_packet()
index += 1
if index > 10:
break
def Xtest_parse_imu(self):
r = ROSMsgParser(config={}, bus=None)
# with open('imu_data.txt', 'rb') as f:
with open('imu-x3.bin', 'rb') as f:
r._buf += f.read()
index = 0
packet = r.get_packet() # first packet is structure file
while packet is not None:
if index > 0:
ori = parse_imu(packet)
# q0, q1, q2, q3 = ori # quaternion
# x = math.atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2))
# y = math.asin(2*(q0*q2-q3*q1))
# z = math.atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3))
# print([math.degrees(a) for a in [x, y, z]])
packet = r.get_packet()
index += 1
def test_publish_desired_speed(self):
data = b'\x08\x00\x00\x00\x02\x00\x00\x00\x00\x06\x81\x14' # clock as 8 bytes
bus = MagicMock()
r = ROSMsgParser(config={}, bus=bus)
r.slot_raw(timestamp=None, data=data)
bus.publish.assert_called_with('sim_time_sec', 2)
clock_data = b'\x08\x00\x00\x00\x03\x00\x00\x00\x00\x00\x00\x00' # clock 3s
r.slot_raw(timestamp=None, data=clock_data)
bus.publish.assert_called_with(
'sim_time_sec', 3) # initially desired speed is None -> no cmd
r.slot_desired_speed(timestamp=None, data=[0, 0])
r.slot_raw(timestamp=None, data=clock_data)
# ... asserting that the last call has been made in a particular way
bus.publish.assert_called_with('cmd', b'cmd_vel 0.000000 0.000000')
# after update from 3D desired speed only extended cmd_vel_3d should be used
r.slot_desired_speed_3d(timestamp=None, data=[[1, 2, 3], [4, 5, 6]])
r.slot_raw(timestamp=None, data=clock_data)
bus.publish.assert_called_with(
'cmd',
b'cmd_vel_3d 1.000000 2.000000 3.000000 4.000000 5.000000 6.000000'
)