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