def main(sysargs):
# set arguments
args = EasyArgs(sysargs)
cfg = EasyConfig(args.config, group="capture")
time = args.time or cfg.default_time
output_folder = args.output or cfg.default_output
outpath = os.path.join(output_folder, datetime.now().strftime(cfg.date_format))
num_frames = int(time * cfg.framerate) + (cfg.flash_delay * 2)
flash_at = [cfg.flash_delay, num_frames - cfg.flash_delay]
sleeper = Sleeper(1.0 / cfg.framerate)
# data directory sanity check
if not os.path.exists(output_folder):
os.makedirs(output_folder)
# start the serial listener
if cfg.run_serial:
try:
serial = Serial(cfg.serial_device, 9600)
serial.setRTS(0) # set at zero
except OSError:
print "Couldn't open serial device", cfg.serial_device
return 1
else:
print "Not listening to serial"
# open Vicon connection
print "Connecting to Vicon..."
client = PyVicon()
client.connect(cfg.ip_address, cfg.port)
if not client.isConnected():
print "Failed to connect to Vicon! {}:{}".format(
cfg.ip_address, cfg.port)
return 1
csvfiles = []
csvwriters = {}
# determine training or capture mode
if args.training:
# test target existance
client.frame()
if not cfg.trainer_target in client.subjects():
print "Cannot find:", cfg.trainer_target
return 1
f = open(args.training, 'wb')
csvfiles.append(f)
csvwriters[cfg.trainer_target] = csv.writer(f, delimiter=cfg.output_delimiter, quoting=csv.QUOTE_MINIMAL)
subjects = [cfg.trainer_target]
else:
client.frame()
subjects = client.subjects()
# open CSV files
for sub in subjects:
path = "{0}_{1}.csv".format(outpath, sub)
f = open(path, 'wb')
w = csv.writer(f, delimiter=cfg.output_delimiter, quoting=csv.QUOTE_MINIMAL)
csvfiles.append(f)
csvwriters[sub] = w
# print status
print ""
print "Using config:", cfg._path
print "Running for", time, "seconds ({} frames)".format(num_frames)
print "Flash delay at:", cfg.flash_delay, " ({} seconds)".format(int(cfg.flash_delay / cfg.framerate))
print "Capturing at", cfg.framerate, "frames per second"
if args.training:
print "Recording training target:", cfg.trainer_target, "into:", args.training
else:
print "Saving data into:", output_folder
print "Recording these subjects:\n", ", ".join(subjects)
print ""
# main loop
for c in range(0, num_frames):
sleeper.stamp()
# run flash
flash = "."
if cfg.run_serial:
if c in flash_at:
serial.setRTS(1)
flash = "F"
sys.stdout.write("\r - - - - - - - Flash!\r")
else:
serial.setRTS(0)
client.frame()
for s in subjects:
csvwriters[s].writerow(
[sleeper.getStamp(), flash] +
list(client.translation(s)) +
list(client.rotation(s)) +
list(client.markerStatus(s)))
# sleep until next timestamp
sys.stdout.write("{}/{}\r".format(c, num_frames))
sleeper.sleep("\r - - - - - - - - - Late!\r")
sys.stdout.flush()
# clean up
for f in csvfiles: f.close()
client.disconnect()
print "\nComplete."
return 0
def main(sysargs):
import sys, os
from python_vicon import PyVicon
import time
print("Connecting to Vicon...")
client = PyVicon()
client.connect("192.168.10.1", 801)
if not client.isConnected():
print("Failed to connect to Vicon!")
return 1
print("Sending Mocap data")
csvfiles = []
csvwriters = {}
time_usec = 0 #time.clock() * 1000 * 1000
trans_scale = 1000
print("Sending MAV Vision Data")
itime_usec = 100000
i = 0.01
dt = 0.1
while True:
i = i + 0.01
time.sleep(0.01) #0.05, working
client.frame()
subjects = client.subjects()
for s in subjects:
if (s == 'IRIS_1'):
trans = client.translation(s)
if (trans[0] == 0.0 and trans[1] == 0.0 and trans[2] == 0.0):
print('dead packet')
continue
rot = client.rotation(s)
#rot = [0,0,0]
#rot[0]=0
#rot[1]=0
#rot[2]=0
q_raw = client.quaternion(s)
######message = mav.mav.local_position_ned_send(time_boot_ms=0, x=trans[0]/trans_scale, y=trans[1]/trans_scale, z=trans[2]/trans_scale, vx=0, vy=0, vz=0)
# ENU (from mocap) | NED new coordinates(in Mavros)
# x <-----------------------> y = Xned
# y <-----------------------> x = Yned
# z <-----------------------> -z = Zned
x_ENU = trans[0] / trans_scale
y_ENU = trans[1] / trans_scale
z_ENU = trans[2] / trans_scale
x_NED = y_ENU
y_NED = x_ENU
z_NED = -z_ENU
#if (time_usec % 10000 == 0):
# print("%d\t%f\t%f\t%f" % (
# time_usec, x_NED,y_NED,z_NED))
#ignored by ekf2
#message = mav.mav.global_vision_position_estimate_send(usec=time_usec,
# x=x_NED,
# y=y_NED,
# z=z_NED,
# roll=rot[0],
# pitch=rot[1],
# yaw=rot[2])
message = mav.mav.vision_position_estimate_send(usec=time_usec,
x=x_NED,
y=y_NED,
z=z_NED,
roll=rot[0],
pitch=rot[1],
yaw=rot[2])
#message = mav.mav.vision_speed_estimate_send(usec=time_usec, x=0, y=0, z=0)
#ignored by ekf2
#message = mav.mav.vision_position_estimate_send(usec=time_usec,
# x=trans[0] / trans_scale,
# y=trans[1] / trans_scale,
# z=trans[2] / trans_scale, roll=rot[0],
# pitch=rot[1], yaw=rot[2])
#message = mav.mav.att_pos_mocap_send(time_usec, q_raw, x=x_NED, y=y_NED, z=z_NED)
time_usec += 1000
def main(sysargs):
# set arguments
args = EasyArgs(sysargs)
cfg = EasyConfig(args.config, group="capture")
time = args.time or cfg.default_time
output_folder = args.output or cfg.default_output
outpath = os.path.join(output_folder,
datetime.now().strftime(cfg.date_format))
num_frames = int(time * cfg.framerate) + (cfg.flash_delay * 2)
flash_at = [cfg.flash_delay, num_frames - cfg.flash_delay]
sleeper = Sleeper(1.0 / cfg.framerate)
# data directory sanity check
if not os.path.exists(output_folder):
os.makedirs(output_folder)
# start the serial listener
if cfg.run_serial:
try:
serial = Serial(cfg.serial_device, 9600)
serial.setRTS(0) # set at zero
except OSError:
print "Couldn't open serial device", cfg.serial_device
return 1
else:
print "Not listening to serial"
# open Vicon connection
print "Connecting to Vicon..."
client = PyVicon()
client.connect(cfg.ip_address, cfg.port)
if not client.isConnected():
print "Failed to connect to Vicon! {}:{}".format(
cfg.ip_address, cfg.port)
return 1
csvfiles = []
csvwriters = {}
# determine training or capture mode
if args.training:
# test target existance
client.frame()
if not cfg.trainer_target in client.subjects():
print "Cannot find:", cfg.trainer_target
return 1
f = open(args.training, 'wb')
csvfiles.append(f)
csvwriters[cfg.trainer_target] = csv.writer(
f, delimiter=cfg.output_delimiter, quoting=csv.QUOTE_MINIMAL)
subjects = [cfg.trainer_target]
else:
client.frame()
subjects = client.subjects()
# open CSV files
for sub in subjects:
path = "{0}_{1}.csv".format(outpath, sub)
f = open(path, 'wb')
w = csv.writer(f,
delimiter=cfg.output_delimiter,
quoting=csv.QUOTE_MINIMAL)
csvfiles.append(f)
csvwriters[sub] = w
# print status
print ""
print "Using config:", cfg._path
print "Running for", time, "seconds ({} frames)".format(num_frames)
print "Flash delay at:", cfg.flash_delay, " ({} seconds)".format(
int(cfg.flash_delay / cfg.framerate))
print "Capturing at", cfg.framerate, "frames per second"
if args.training:
print "Recording training target:", cfg.trainer_target, "into:", args.training
else:
print "Saving data into:", output_folder
print "Recording these subjects:\n", ", ".join(subjects)
print ""
# main loop
for c in range(0, num_frames):
sleeper.stamp()
# run flash
flash = "."
if cfg.run_serial:
if c in flash_at:
serial.setRTS(1)
flash = "F"
sys.stdout.write("\r - - - - - - - Flash!\r")
else:
serial.setRTS(0)
client.frame()
for s in subjects:
csvwriters[s].writerow([sleeper.getStamp(), flash] +
list(client.translation(s)) +
list(client.rotation(s)) +
list(client.markerStatus(s)))
# sleep until next timestamp
sys.stdout.write("{}/{}\r".format(c, num_frames))
sleeper.sleep("\r - - - - - - - - - Late!\r")
sys.stdout.flush()
# clean up
for f in csvfiles:
f.close()
client.disconnect()
print "\nComplete."
return 0
