def Xtest_parse_points(self): r = ROSMsgParser(config={}, bus=None) with open('point_data.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_points(packet) packet = r.get_packet() index += 1 if index > 10: break
def Xtest_parse_image(self): r = ROSMsgParser(config={}, bus=None) with open('image_raw2.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_jpeg_image(packet, 'dump_%03d.jpg' % index) 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