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])
