def vicon_connect(self):
client = PyVicon()
client.connect(self.ip, self.port)
if not client.isConnected():
print("Failed to connect to Vicon!")
return 1
else:
print("Vicon connected!")
self.client = client
return client
def connect(self, ip, port=801, defaults=True):
stat = pyvicon.connect(self._c, "{}:{}".format(ip, port))
if stat and defaults:
pyvicon.enableSegmentData(self._c)
pyvicon.enableMarkerData(self._c)
pyvicon.setStreamMode(self._c, self.SM_ClientPull)
return stat
def frame(self):
return pyvicon.frame(self._c)
def translation(self, name):
return pyvicon.globalTranslation(self._c, name)
def rotation(self, name):
return pyvicon.globalRotation(self._c, name)
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 myversion(self):
return pyvicon.version(self._c)
def enableMarkerData(self, b=True):
if b: return pyvicon.enableMarkerData(self._c)
else: return pyvicon.disableMarkerData(self._c)
def enableSegmentData(self, b=True):
if b: return pyvicon.enableSegmentData(self._c)
else: return pyvicon.disableSegmentData(self._c)
def disconnect(self):
return pyvicon.disconnect(self._c)
def main(sysargs):
# open Vicon connection
print("Connecting to Vicon...")
client = PyVicon()
ip_address = '192.168.10.1'
port = 801
client.connect(ip_address, port)
if not client.isConnected():
print("Failed to connect to Vicon! {}:{}".format(ip_address, port))
return 1
client.frame()
subjects = client.subjects()
# main loop
while True:
client.frame()
for s in subjects:
print('data')
client.disconnect()
print("\nComplete.")
return 0
def main(sysargs):
args = EasyArgs(sysargs)
if 'help' in args:
usage()
return 0
# arg sanity checks
if len(args) < 2:
usage()
return 1
if args.no_preview and not args.video_file and not args.image_file:
print "if -no_preview is toggled, you must a video or image output"
usage()
return 1
# default args
height = args.height or 3000 # milimetres
width = args.width or 9000 # milimetres
framerate = args.framerate or 30 # fps
max_height = args.max_height or 400 # pixels
ip_address = args.ip_address or "192.168.10.1"
port = 801
target = args[1]
# working vars
scale = float(max_height) / height
img_h = int(height * scale)
img_w = int(width * scale)
sleep_time = int((1.0 / framerate) * 1000) # in miliseconds
# open video writer (if applicable)
if "video_file" in args:
export = True
codec = args.codec or 'DIVX'
video = cv2.VideoWriter(args.video_file,
cv2.VideoWriter_fourcc(*codec), framerate,
(img_w, img_h))
else:
export = False
# open window
window_name = "Tracer"
cv2.namedWindow(window_name)
# open pyvicon
client = PyVicon()
client.connect(ip_address, port)
client.setStreamMode(PyVicon.SM_ServerPush)
for i in range(0, 20):
client.frame()
subjects = client.subjects()
# settings
font = cv2.FONT_HERSHEY_SIMPLEX
text_size = 0.5
dot_col = (255, 255, 255)
trace_cols = {}
for s in subjects:
b = int(random.random() * 155) + 100
g = int(random.random() * 155) + 100
r = int(random.random() * 155) + 100
trace_cols[s] = (b, g, r)
print "tracking {} objects".format(len(subjects))
print img_w, img_h
# a frame to paste the trace on
emptyframe = np.zeros((img_h, img_w, 3), np.uint8)
baseframe = emptyframe.copy()
while True:
# a frame to draw dots and text, without affecting the trace frame
dotframe = baseframe.copy()
client.frame()
for s in trace_cols:
x, y, _ = client.translation(s)
pt = (int(x * scale), img_h - int(y * scale))
# draw
cv2.circle(baseframe, pt, 1, trace_cols[s], 2)
cv2.circle(dotframe, pt, 3, dot_col, 5)
# show and wait
if not args.no_preview:
cv2.imshow(window_name, dotframe)
key = cv2.waitKey(sleep_time)
if key == Key.esc:
break
elif key == Key.space:
baseframe = emptyframe.copy()
# save to video
if export:
video.write(dotframe)
# console text
sys.stdout.write("{}, {}\r".format(x, y))
sys.stdout.flush()
# write last still image
if "image_file" in args:
cv2.imwrite(args.image_file, dotframe)
# clean up
if export:
video.release()
cv2.destroyAllWindows()
print "\ndone"
return 0
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 markerStatus(self, name):
return pyvicon.markerStatus(self._c, name)
def isConnected(self):
return pyvicon.isConnected(self._c)
def setStreamMode(self, streamMode):
return pyvicon.setStreamMode(self._c, streamMode)
def subjectCount(self):
return pyvicon.subjectCount(self._c)
def hasSegmentData(self):
return pyvicon.hasSegmentData(self._c)
def subjectName(self, index):
return pyvicon.subjectName(self._c, index)
def hasMarkerData(self):
return pyvicon.hasMarkerData(self._c)
def subjects(self):
return pyvicon.subjects(self._c)
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):
args = EasyArgs(sysargs)
if 'help' in args:
usage()
return 0
# arg sanity checks
if len(args) < 2:
usage()
return 1
if args.no_preview and not args.video_file and not args.image_file:
print "if -no_preview is toggled, you must a video or image output"
usage()
return 1
# default args
height = args.height or 3000 # milimetres
width = args.width or 9000 # milimetres
framerate = args.framerate or 30 # fps
max_height = args.max_height or 400 # pixels
ip_address = args.ip_address or "192.168.10.1"
port = 801
target = args[1]
# working vars
scale = float(max_height) / height
img_h = int(height * scale)
img_w = int(width * scale)
sleep_time = int((1.0 / framerate) * 1000) # in miliseconds
# open video writer (if applicable)
if "video_file" in args:
export = True
codec = args.codec or 'DIVX'
video = cv2.VideoWriter(args.video_file,
cv2.VideoWriter_fourcc(*codec),
framerate, (img_w, img_h))
else:
export = False
# open window
window_name = "Tracer"
cv2.namedWindow(window_name)
# open pyvicon
client = PyVicon()
client.connect(ip_address, port)
client.setStreamMode(PyVicon.SM_ServerPush)
for i in range(0,20):
client.frame()
subjects = client.subjects()
# settings
font = cv2.FONT_HERSHEY_SIMPLEX
text_size = 0.5
dot_col = (255,255,255)
trace_cols = {}
for s in subjects:
b = int(random.random() * 155) + 100
g = int(random.random() * 155) + 100
r = int(random.random() * 155) + 100
trace_cols[s] = (b,g,r)
print "tracking {} objects".format(len(subjects))
print img_w, img_h
# a frame to paste the trace on
emptyframe = np.zeros((img_h,img_w,3), np.uint8)
baseframe = emptyframe.copy()
while True:
# a frame to draw dots and text, without affecting the trace frame
dotframe = baseframe.copy()
client.frame()
for s in trace_cols:
x, y, _ = client.translation(s)
pt = (int(x*scale), img_h-int(y*scale))
# draw
cv2.circle(baseframe, pt, 1, trace_cols[s], 2)
cv2.circle(dotframe, pt, 3, dot_col, 5)
# show and wait
if not args.no_preview:
cv2.imshow(window_name, dotframe)
key = cv2.waitKey(sleep_time)
if key == Key.esc:
break
elif key == Key.space:
baseframe = emptyframe.copy()
# save to video
if export:
video.write(dotframe)
# console text
sys.stdout.write("{}, {}\r".format(x, y))
sys.stdout.flush()
# write last still image
if "image_file" in args:
cv2.imwrite(args.image_file, dotframe)
# clean up
if export:
video.release()
cv2.destroyAllWindows()
print "\ndone"
return 0
