def receive_laser(UDP_PORT, timeout_limit):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # UDP
    sock.bind((UDP_IP, UDP_PORT))
    sock.settimeout(timeout_limit)
    sockets_list.append(sock)
    while True:
        try:
            data = sock.recv(65536)
            laser_data = LaserScan()
            laser_data.deserialize(data)
            np.save('laser', laser_data)
            # print('Laser Data Received\n')
        except timeout:
            print('Error: Laser Data Timeout')
            sys.exit(1)
def get_recorded_dataset(directories):
    if isinstance(directories, str):
        directories = [directories]
    dataset = []
    suffixes = {'sound': '.wav',
                'meta': '.txt',
                'mic_pose': '.msg',
                'speaker_pose': '.msg',
                'self_pose': '.msg',
                'other_pose': '.msg',
                'self_scan': '.msg',
                'other_scan': '.msg'}
    for directory in directories:
        d_full = os.path.abspath(directory)
        _dataset = {}
        for f in os.listdir(d_full):
            f_full = os.path.join(d_full, f)
            for prefix, suffix in suffixes.items():
                if f.startswith(prefix+'_') and f.endswith(suffix):
                    id = f[len(prefix)+1:-len(suffix)]
                    if id in _dataset:
                        entry = _dataset[id]
                    else:
                        entry = {}
                        _dataset[id] = entry
                    if prefix == 'sound':
                        entry['recorded_sound_file'] = f_full
                    elif prefix == 'meta':
                        with open(f_full, 'r') as _f:
                            entry.update(yaml.load(_f.read()))
                    elif prefix.endswith('pose'):
                        ps = PoseStamped()
                        with open(f_full, 'rb') as _f:
                            ps.deserialize(_f.read())
                        entry[prefix] = ps
                    elif prefix.endswith('scan'):
                        ls = LaserScan()
                        with open(f_full, 'rb') as _f:
                            ls.deserialize(_f.read())
                        entry[prefix] = ls
                        
        print 'Found {} entries in directory "{}"'.format(len(_dataset), d_full)
        for k, v in _dataset.iteritems():
            v['id'] = os.path.basename(directory)+'/'+k
            dataset.append(v)
    return dataset
Beispiel #3
0
    points[1,:] += posemsg.position.y
    points[2,:] += posemsg.position.z
    return points

data_dir = os.path.abspath(sys.argv[1])
data_num = sys.argv[2]
rate, sound = wavfile.read(os.path.join(data_dir, 'sound_'+data_num+'.wav'))
sound = sound/32768.
channels = sound.shape[1]

mapmsg = OccupancyGrid()
with open(os.path.join(data_dir, 'map.msg'), 'rb') as f:
    mapmsg.deserialize(f.read())
lscanmsg = LaserScan()
with open(os.path.join(data_dir, 'self_scan_'+data_num+'.msg'), 'rb') as f:
    lscanmsg.deserialize(f.read())
rscanmsg = LaserScan()
with open(os.path.join(data_dir, 'other_scan_'+data_num+'.msg'), 'rb') as f:
    rscanmsg.deserialize(f.read())
lposemsg = PoseStamped()
with open(os.path.join(data_dir, 'self_pose_'+data_num+'.msg'), 'rb') as f:
    lposemsg.deserialize(f.read())
rposemsg = PoseStamped()
with open(os.path.join(data_dir, 'other_pose_'+data_num+'.msg'), 'rb') as f:
    rposemsg.deserialize(f.read())
with open(os.path.join(data_dir, 'meta_'+data_num+'.txt'), 'rb') as f:
    metadata = yaml.load(f.read())

_rate, _sound = wavfile.read(metadata['sound_file'])
if len(_sound.shape) == 1:
    _sound = np.expand_dims(_sound, 1)