def get_most_recent_single_fly_data(self): superpacket_bufs = self.get_list_of_bufs() if len(superpacket_bufs)==0: # no new data return buf = superpacket_bufs[-1] # most recent superpacket packets = data_packets.decode_super_packet( buf ) packet = packets[-1] # most recent packet tmp = data_packets.decode_data_packet(packet) #(corrected_framenumber, timestamp, obj_ids, state_vecs, meanPs) = tmp return tmp
def talker(connect_to_mainbrain=True, server=None): ###################### variables that should be function inputs ########### loginfo = 0 ###################### initialize ######################################### # flydra publishers pub_flydra = rospy.Publisher('flydra_data', flydra_packet) rospy.init_node('flydra_repeater') # set up connection to mainbrain if connect_to_mainbrain: # make connection to flydra mainbrain my_host = '' # get fully qualified hostname my_port = 8320 # arbitrary number # create UDP socket object, grab the port sockobj = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) print 'binding',( my_host, my_port) sockobj.bind(( my_host, my_port)) print 'bound to: ', my_host, my_port # connect to mainbrain mainbrain_hostname = server mainbrain_port = flydra.common_variables.mainbrain_port mainbrain_name = 'main_brain' print 'connecting to mainbrain...' print 'using port: ', mainbrain_port print 'using name: ', mainbrain_name print 'connecting to server: ', server remote_URI = "PYROLOC://%s:%d/%s" % (mainbrain_hostname, mainbrain_port, mainbrain_name) Pyro.core.initClient(banner=0) mainbrain = Pyro.core.getProxyForURI(remote_URI) mainbrain._setOneway(['log_message']) my_host_fqdn = socket.getfqdn(my_host) mainbrain.register_downstream_kalman_host(my_host_fqdn,my_port) print 'connection established with ', mainbrain # create a dummy mainbrain if not connect_to_mainbrain: dummy_mainbrain = DummyFlydra.DummyMainbrain() print 'connected to dummy_mainbrain' ###################### while running ######################################### while not rospy.is_shutdown(): # use flydra if connect_to_mainbrain: print 'waiting for packet' buf, addr = sockobj.recvfrom(4096) print 'packet recieved' packets = data_packets.decode_super_packet( buf ) for packet in packets: tmp = data_packets.decode_data_packet(packet) # obj_ids: order from one that's been around the longest first # state_vecs = [x,y,z,xvel,yvel,zvel] # meanPs = error for each object corrected_framenumber, timestamp_i, timestamp_f, obj_ids, state_vecs, meanPs = tmp latency = timestamp_f - timestamp_i print latency # use dummy flydra if not connect_to_mainbrain: latency, obj_ids, state_vecs = dummy_mainbrain.get_flies() # print list of obj_ids just to make sure things are happening if loginfo: rospy.loginfo(obj_ids) # package and publish flydra data flies_tmp = [] for i in range(len(obj_ids)): fly = flydra_fly(obj_ids[i], state_vecs[i,:]) # package individual fly info into the flydra_fly msg packet flies_tmp.append(fly) flies = flydra_packet(flies_tmp, latency) # package all the flies and global flydra data into the flydra_packet msg packet pub_flydra.publish(flies)
def run(self): while 1: #print 'running' # get data when available, put into buffer # process data # stick data into the gui #print 'listening for packet on',self.s if not self.dummy: buf, addr = self.s.recvfrom(4096) else: time.sleep(0.05) # pause 50 msec #print 'got packet:',buf #self.q.put( buf ) if self.fly_pos is not None: if self.dummy: theta = time.time() % (2*pi) x = 0.3*np.cos( theta ) y = 0.05*np.sin( theta ) z = 0.05*np.sin( theta/2.3 ) voitest = 3 else: # actually use flydra #print('using flydra!') packets = data_packets.decode_super_packet( buf ) for packet in packets: tmp = data_packets.decode_data_packet(packet) # obj_ids: order from one that's been around the longest first # state_vecs = [x,y,z,xvel,yvel,zvel] # meanPs = error for each object corrected_framenumber, timestamp_i, timestamp_f, obj_ids, state_vecs, meanPs = tmp if self.preferred_objid is None: self.choose_pref_objid(obj_ids, state_vecs, meanPs) # get the preferred object id, use that try: pref_objid_index = obj_ids.index(self.preferred_objid) except ValueError: #print "That fly doesn't exist anymore... finding a new fly" self.choose_pref_objid(obj_ids, state_vecs, meanPs) #print("Found a new fly with object ID: ", self.preferred_objid) #print("Fly list: ", obj_ids) if self.preferred_objid is None: continue pref_objid_index = obj_ids.index(self.preferred_objid) latency = timestamp_f - timestamp_i #if latency >= 0.1: # raise ValueError, 'Flydra latency > 100ms, something is probably wrong!' x,y,z,xvel,yvel,zvel= state_vecs[pref_objid_index] #print(x,y,z) self.fly_pos.pos.put([x,y,z,xvel,yvel,zvel,latency])