def on_daq_callback(self, data):
mdf = rc.MDF_PLOT_POSITION()
self.serial_no += 1
mdf.tool_id = 0
mdf.missing = 0
self.variable += 1
mdf.xyz[:] = np.array([self.variable] * 3)
mdf.ori[:] = np.array(
[self.variable] * 4
) # will work but need!!! reading modules to know the format of buffer
#mdf.buffer[data.size:] = -1
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
print self.variable
sys.stdout.write('|')
sys.stdout.flush()
# now check for exit message
in_msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
hdr = msg.GetHeader()
msg_type = hdr.msg_type
dest_mod_id = hdr.dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print "Received MT_EXIT, disconnecting..."
self.daq_task.StopTask()
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
self.stop()
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'RandomGen')
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = self.shuffle_q(np.array(in_mdf.ori[:]))
# np.testing.assert_array_equal(positions[:,0], orientations[:,0], err_msg='Samples are not aligned')
if self.calibrated:
if in_mdf.tool_id == (self.marker + 1):
# calculating output
self.Qk = qa.norm(
orientations
) # need to find a way to discriminate the tools files in the messages???
Qr = qa.mult(self.Qk, qa.inv(self.Qi)).flatten()
Tk = positions
hotspot_position = (qa.rotate(Qr, self.Xi) + Tk).flatten()
hotspot_vector_head = qa.rotate(Qr, plate_vector)
if np.any(np.isnan(hotspot_position)) == True:
print "x",
# print ' *****nan present, check coil is within frame!*****'
# creating output message
out_mdf = rc.MDF_HOTSPOT_POSITION()
out_mdf.xyz[:] = hotspot_position
out_mdf.ori[:3] = hotspot_vector_head # Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_HOTSPOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("o")
else:
if np.any(np.isnan(positions)) == True:
raise Exception, "nan present"
if np.any(np.isnan(orientations)) == True:
raise Exception, "nan present"
if (
(self.store_plate >= self.store_plate_pos.shape[0])
& (self.store_plate >= self.store_plate_ori.shape[0])
& (self.store_coil >= self.store_coil_pos.shape[0])
& (self.store_coil >= self.store_coil_ori.shape[0])
):
self.calibrating = False
self.make_calibration_vector()
elif in_mdf.tool_id == (self.marker + 1):
self.store_coil_pos[self.store_coil, :] = positions
self.store_coil_ori[self.store_coil, :] = orientations
self.store_coil += 1
elif in_mdf.tool_id == (self.plate + 1):
self.store_plate_pos[self.store_plate, :] = positions
self.store_plate_ori[self.store_plate, :] = orientations
self.store_plate += 1
def send_metronome(self, state):
mdf = rc.MDF_MNOME_STATE()
self.serial_no += 1
mdf.sample_header.SerialNo = self.serial_no
mdf.sample_header.Flags = 0
mdf.sample_header.DeltaTime = (1. / 5)
mdf.state = state
msg = CMessage(rc.MT_MNOME_STATE)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
print "Sent message %d" % state
def send_sample_generated(self):
sg = rc.MDF_SAMPLE_GENERATED()
self.serial_no += 1
sg.sample_header.SerialNo = self.serial_no
sg.sample_header.Flags = 0
sg.sample_header.DeltaTime = (1. / self.freq)
sg.source_timestamp = self.default_timer() #time.time()
sg_msg = CMessage(rc.MT_SAMPLE_GENERATED)
copy_to_msg(sg, sg_msg)
self.mod.SendMessage(sg_msg)
sys.stdout.write('|')
sys.stdout.flush()
def send_output(self, sample_header):
mdf = rc.MDF_COMPOSITE_MOVEMENT_COMMAND()
mdf.tag = "composite"
vel = np.zeros_like(mdf.vel)
vel[:] = self.get_combined_command()
if self.idle_gateable == 1:
vel[:8] *= self.gate
mdf.vel[:] = vel
mdf.sample_header = sample_header
msg = CMessage(rc.MT_COMPOSITE_MOVEMENT_COMMAND)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
def send_sample_generated(self):
sg = rc.MDF_SAMPLE_GENERATED()
self.serial_no += 1
sg.sample_header.SerialNo = self.serial_no
sg.sample_header.Flags = 0
sg.sample_header.DeltaTime = (1. / self.freq)
sg.source_timestamp = self.default_timer() #time.time()
sg_msg = CMessage(rc.MT_SAMPLE_GENERATED)
copy_to_msg(sg, sg_msg)
self.mod.SendMessage(sg_msg)
sys.stdout.write('|')
sys.stdout.flush()
def send_output(self, sample_header):
mdf = rc.MDF_COMPOSITE_MOVEMENT_COMMAND()
mdf.tag = 'composite'
vel = np.zeros_like(mdf.vel)
vel[:] = self.get_combined_command()
if self.idle_gateable == 1:
vel[:8] *= self.gate
mdf.vel[:] = vel
mdf.sample_header = sample_header
msg = CMessage(rc.MT_COMPOSITE_MOVEMENT_COMMAND)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if self.calibrated:
if self.set_collecting:
if (msg_type == rc.MT_HOTSPOT_POSITION) & (self.got_coil == False):
# handling input message
in_mdf = rc.MDF_HOTSPOT_POSITION()
copy_from_msg(in_mdf, in_msg)
self.current_vtail = np.array(in_mdf.xyz[:]) #Hotspot position
self.current_vhead = np.array(in_mdf.ori[:3]) #Vector head of coil, used to find ori
self.got_coil = True
elif (msg_type == rc.MT_POLARIS_POSITION) & (self.got_head == False):
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = self.shuffle_q(np.array(in_mdf.ori[:]))
if in_mdf.tool_id == (self.glasses + 1):
# calculating output
self.head, Qr = self.tp.get_pos(orientations, positions)
print(self.head)
self.got_head = True
elif (self.got_head == True) & (self.got_coil == True):
plot_position = self.current_vtail - self.head + self.plot_vertex_vec
in_mdf = rc.MDF_POLARIS_POSITION()
out_mdf = rc.MDF_PLOT_POSITION()
copy_from_msg(in_mdf, in_msg)
out_mdf.xyz[:] = plot_position
out_mdf.ori[:] = np.append(self.current_vhead, 0)# Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")
self.count += 1
save_array = np.insert(np.concatenate(((self.current_vtail - self.head), self.current_vhead)), 0, self.count)
self.current_map_data = np.vstack((self.current_map_data, save_array))
self.got_coil = False
self.got_head = False
self.set_collecting = False
if self.count > 100:
location = raw_input("Finished! Where should it save?")
np.savetxt(str(location) +'.txt',self.current_map_data[1:],
delimiter=',', newline='/n')
mlab.savefig(str(location) + '.png', figure=self.fig)
mlab.close(self.fig)
self.count = 0
self.run()
def respond_to_ping(mod, msg, module_name):
dest_mod_id = msg.GetHeader().dest_mod_id
p = rc.MDF_PING()
copy_from_msg(p, msg)
# print "PING received for '{0}'".format(p.module_name)
if (p.module_name.lower() == module_name.lower()) or (p.module_name == "*") or (dest_mod_id == mod.GetModuleID()):
mdf = rc.MDF_PING_ACK()
mdf.module_name = module_name
msg_out = CMessage(rc.MT_PING_ACK)
copy_to_msg(mdf, msg_out)
mod.SendMessage(msg_out)
def respond_to_ping(self, msg, module_name):
#print "PING received for '{0}'".format(p.module_name)
dest_mod_id = msg.GetHeader().dest_mod_id
p = rc.MDF_PING()
copy_from_msg(p, msg)
if (p.module_name.lower() == module_name.lower()) or (p.module_name == "*") or \
(dest_mod_id == self.mod.GetModuleID()):
mdf = rc.MDF_PING_ACK()
mdf.module_name = module_name + ":" + self.host_name # + ":" + self.host_os
msg_out = CMessage(rc.MT_PING_ACK)
copy_to_msg(mdf, msg_out)
self.mod.SendMessage(msg_out)
def send_btn_release(self, btn, controller_id):
print "controller_id %d sending button release %s" % (controller_id, btn)
btn_map = {'l1' : rc.PS3_B_L1,
'l2' : rc.PS3_B_L2,
'r1' : rc.PS3_B_R1,
'x' : rc.PS3_B_X,
'sq' : rc.PS3_B_SQUARE,
'crc': rc.PS3_B_CIRCLE,
'trg': rc.PS3_B_TRIANGLE}
mdf_out = rc.MDF_PS3_BUTTON_RELEASE()
mdf_out.whichButton = btn_map[btn]
mdf_out.controllerId = controller_id
msg_out = CMessage(rc.MT_PS3_BUTTON_RELEASE)
copy_to_msg(mdf_out, msg_out)
self.mod.SendMessage(msg_out)
def send_btn_press(self, btn, controller_id):
print "controller_id %d sending button press %s" % (controller_id, btn)
btn_map = {'l1' : rc.PS3_B_L1,
'l2' : rc.PS3_B_L2,
'r1' : rc.PS3_B_R1,
'x' : rc.PS3_B_X,
'sq' : rc.PS3_B_SQUARE,
'crc': rc.PS3_B_CIRCLE,
'trg': rc.PS3_B_TRIANGLE}
mdf_out = rc.MDF_PS3_BUTTON_PRESS()
mdf_out.whichButton = btn_map[btn]
mdf_out.controllerId = controller_id
# make outgoing message data
msg_out = CMessage(rc.MT_PS3_BUTTON_PRESS)
copy_to_msg(mdf_out, msg_out)
self.mod.SendMessage(msg_out)
def run(self):
self.delta_time_calc = time.time() #time.time()
while True:
if (time.time() - self.delta_time_calc) % 2 == 0:
self.tail[0] = self.tail[0] + 1
self.head[1] = self.head[1] + 1
out_mdf = rc.MDF_PLOT_POSITION()
self.serial_no += 1
out_mdf.sample_header.SerialNo = self.serial_no
out_mdf.sample_header.Flags = 0
out_mdf.sample_header.DeltaTime = (1. / 5)
out_mdf.xyz[:] = self.tail
out_mdf.ori[:] = np.append(self.head, 0)# Qk - coil active orientation
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")
def process_msg(self, in_msg):
header = in_msg.GetHeader()
if header.msg_type == rc.MT_FT_DATA:
mdf = rc.MDF_FT_DATA()
copy_from_msg(mdf, in_msg)
rate(self.rate)
self.ball.pos = vector(mdf.F[0:3])
self.shadow_cursor.pos = vector(
[mdf.F[0], -self.length / 2, mdf.F[2]])
self.unit_target = np.array(self.target_vector) / np.linalg.norm(
self.target_vector)
self.target_position = np.array(
self.unit_target) * self.max_factor * self.force_scale
self.target.pos = self.target_position
self.shadow_target.pos = [
self.target_position[0], -self.length / 2,
self.target_position[2]
]
distance = [a - b for a, b in zip(self.ball.pos, self.target.pos)]
if (distance[0]**2 + distance[1]**2 + distance[2]**2)**(
1 / 2.) >= self.threshold and self.RTFT_display:
self.ball.color = self.ball_color
self.state = 0
elif (distance[0]**2 + distance[1]**2 + distance[2]**2)**(
1 / 2.) < self.threshold and self.RTFT_display:
if self.state == 0: # if previous sample was outside radius, and now we're inside...
self.start_hold = time.time()
self.state = 1
self.ball.color = color.orange
else:
if time.time() > (self.start_hold + self.hold_time):
self.ball.color = color.green
self.target.visible = False
self.shadow_target.visible = False
self.state = 2
out_mdf = rc.MDF_FT_COMPLETE()
out_mdf.FT_COMPLETE = self.state
out_mdf.sample_header = mdf.sample_header
msg = CMessage(rc.MT_FT_COMPLETE)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
else:
self.state = 1
self.ball.color = color.orange
else:
self.state = -1
if self.state == 2 and self.solo: #if no executive file
self.target.pos = [
float(x) for x in [
np.random.rand(1, 1) * self.max_factor *
self.force_scale,
np.random.rand(1, 1) * self.max_factor *
self.force_scale,
np.random.rand(1, 1) * self.max_factor *
self.force_scale
]
]
self.shadow_target.pos = [
self.target.pos[0], -self.length / 2, self.target.pos[2]
]
sys.stdout.write(
"%7.4f, %5d, %16.2f\n" %
(mdf.F[2], self.state,
(self.start_hold + self.hold_time) - time.time()))
#msg_str = "%7.4f " * 6 + "\n"
#sys.stdout.write(msg_str % (mdf.F[0], mdf.F[1], mdf.F[2],
# mdf.T[0], mdf.T[1], mdf.T[2]))
sys.stdout.flush()
elif header.msg_type == rc.MT_RTFT_CONFIG:
mdf = rc.MDF_RTFT_CONFIG()
copy_from_msg(mdf, in_msg)
self.max_factor = mdf.max_factor
self.RTFT_display = mdf.RTFT_display
self.target_vector = mdf.target_vector[:]
self.ball.visible = mdf.cursor_visible
self.target.visible = mdf.target_visible
self.shadow_target.visible = mdf.shadow_target_visible
self.shadow_cursor.visible = mdf.shadow_cursor_visible
self.ball_color = [1, 0, 0]
self.solo = False
def run(self):
while True:
in_msg = CMessage()
rcv = self.mod.ReadMessage(in_msg, 0.1)
if rcv == 1:
msg_type = in_msg.GetHeader().msg_type
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, in_msg, 'Metronome')
elif msg_type == rc.MT_MNOME_STATE:
print 'got message'
in_mdf = rc.MDF_MNOME_STATE()
copy_from_msg(in_mdf, in_msg)
if in_mdf.state == 0:
print 'got stop'
self.pause_state = True
self.count = 0
elif in_mdf.state == 1:
print 'got start'
self.pause_state = False
self.count = 0
elif in_mdf.state == 2:
print 'got pause'
self.pause_state = True
self.count = 0
elif msg_type == self.in_msg_num:
if self.pause_state:
pass
else:
self.count += 1
if self.pretrigger_time > 0:
if self.count == self.metronome_count:
in_mdf = eval('rc.MDF_%s()' %
(self.in_msg_type.upper()))
copy_from_msg(in_mdf, in_msg)
out_mdf = rc.MDF_TMS_TRIGGER()
out_mdf.sample_header = in_mdf.sample_header
out_msg = CMessage(rc.MT_TMS_TRIGGER)
copy_to_msg(out_mdf, out_msg)
self.mod.SendMessage(out_msg)
self.count = 0 - int(
np.random.uniform(0, 1.5, 1)[0] *
self.in_msg_freq)
if self.count == self.trigger_out_count:
sound_thread = threading.Thread(
target=self.play_sound)
sound_thread.start()
else:
if self.count == self.trigger_out_count:
in_mdf = eval('rc.MDF_%s()' %
(self.in_msg_type.upper()))
copy_from_msg(in_mdf, in_msg)
out_mdf = rc.MDF_TMS_TRIGGER()
out_mdf.sample_header = in_mdf.sample_header
out_msg = CMessage(rc.MT_TMS_TRIGGER)
copy_to_msg(out_mdf, out_msg)
self.mod.SendMessage(out_msg)
if self.count == self.metronome_count:
self.count = 0 - int(
np.random.uniform(0, 1.5, 1)[0] *
self.in_msg_freq)
sound_thread = threading.Thread(
target=self.play_sound)
sound_thread.start()
MID_REPLY = 10
# Note: Reply must be started first
if __name__ == "__main__":
mod = PyDragonfly.Dragonfly_Module(MID_REPLY, 0)
mod.ConnectToMMM()
mod.Subscribe(mdefs.MT_REQUEST_TEST_DATA)
print("Reply running...\n")
run = True
while run:
in_msg = PyDragonfly.CMessage()
print("Waiting for message")
rcv = mod.ReadMessage(in_msg, 1.0)
if rcv == 1:
print("Received message", in_msg.GetHeader().msg_type)
out_msg = PyDragonfly.CMessage(mdefs.MT_TEST_DATA)
data = mdefs.MDF_TEST_DATA()
if in_msg.GetHeader().msg_type == mdefs.MT_REQUEST_TEST_DATA:
data.a = -20
data.b = 47
data.x = 123.456
copy_to_msg(data, out_msg)
mod.SendMessage(out_msg)
print("Sent message", out_msg.GetHeader().msg_type)
elif in_msg.GetHeader().msg_type in (MT_EXIT, MT_KILL):
run = False
import PyDragonfly
from PyDragonfly import copy_to_msg
import message_defs as md
import sys
MID_PRODUCER = 10
if __name__ == "__main__":
mod = PyDragonfly.Dragonfly_Module(MID_PRODUCER, 0)
mod.ConnectToMMM("localhost:7111")
print "Producer running...\n"
a = 0
run = True
while run:
out_msg = PyDragonfly.CMessage(md.MT_TEST_DATA)
data = md.MDF_TEST_DATA()
data.a = a
data.b = -3
data.x = 1.234
copy_to_msg(data, out_msg)
mod.SendMessage(out_msg)
print "Sent message ", out_msg.GetHeader().msg_type
a += 1
time.sleep(1)
def find_hotspot_to_cz(
self, HS_GCS, glasses_orientation, glasses_position,
coil_vector): #Saves 0, 0, 0 array to file when object called
check_HS_GCS = self.check(HS_GCS)
check_glasses_orientation = self.check(glasses_orientation)
check_glasses_position = self.check(glasses_position)
check_coil_vector = self.check(coil_vector)
if np.any(
np.array([
check_HS_GCS, check_glasses_orientation,
check_glasses_position, check_coil_vector
])):
print np.array([
check_HS_GCS, check_glasses_orientation,
check_glasses_position, check_coil_vector
])
HS_LCS = (np.zeros(3) * np.NaN)[None]
print HS_LCS
vector_LCS = (np.zeros(3) * np.NaN)[None]
with open(self.location, 'a') as location:
np.savetxt(location,
np.array(HS_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
location.close()
with open(self.vector, 'a') as data:
np.savetxt(data,
np.array(vector_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
data.close()
sys.stdout.write("No position\n")
else:
cz_pos, cz_rot = self.glasses_to_cz.get_pos(
glasses_orientation, glasses_position)
LT_pos, LT_rot = self.glasses_to_LT.get_pos(
glasses_orientation, glasses_position)
RT_pos, RT_rot = self.glasses_to_RT.get_pos(
glasses_orientation, glasses_position)
Nasion_pos, Nasion_rot = self.glasses_to_Nasion.get_pos(
glasses_orientation, glasses_position)
calibration_matrix = self.make_head_axis(LT_pos, RT_pos,
Nasion_pos)
HS_LCS = qa.rotate(calibration_matrix, (HS_GCS - cz_pos))
vector_LCS = qa.rotate(calibration_matrix, coil_vector)
#HS_LCS = qa.rotate(rot, qa.rotate(self.calibration_matrix, (HS_GCS - cz_pos)))
#vector_LCS = qa.rotate(rot, qa.rotate(self.calibration_matrix, (coil_vector)))
with open(self.location, 'a') as location:
np.savetxt(location,
np.array(HS_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
location.close()
with open(self.vector, 'a') as data:
np.savetxt(data,
np.array(vector_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
data.close()
self.stimulus_no += 1
print self.stimulus_no
in_mdf = rc.MDF_POLARIS_POSITION()
out_mdf = rc.MDF_PLOT_POSITION()
out_mdf.xyz[:] = HS_LCS[0]
out_mdf.ori[:] = np.append(vector_LCS[0],
0) # Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")
