def _reader_thread(self):
while(1):
self._timer.wait()
markers = OWL.owlGetMarkers()
if markers.size() > 0:
#If there's data, add a frame to the buffer
new_frame = np.empty((self._num_points, 3, 1))
new_frame.fill(np.nan)
#Add the markers
for i in range(markers.size()):
m = markers[i]
if m.cond > 0:
new_frame[m.id,0,0] = m.x
new_frame[m.id,1,0] = m.y
new_frame[m.id,2,0] = m.z
# print("%d: %f %f %f" % (m.id, m.x, m.y, m.z))
timestamp = np.array(time.time())
self._read_buffer.put((new_frame, timestamp))
self._frame_count += 1
if OWL.owlGetError() != OWL.OWL_NO_ERROR:
print('A mocap read error occurred')
if self._shutdown_flag:
return
def __init__(self,
server_name,
freq=OWL.OWL_MAX_FREQUENCY,
scale=(0.001, 0.001, 0.001),
offset=(0, 0, 0),
postprocess=False,
slave=False):
super(Phasespace, self).__init__()
flags = 0
if postprocess:
flags |= OWL.OWL_POSTPROCESS
if slave:
flags |= OWL.OWL_SLAVE
if OWL.owlInit(server_name, flags) < 0:
raise OwlError('phasespace')
OWL.owlSetFloat(OWL.OWL_FREQUENCY, freq)
OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE)
logging.info('running phasespace at %.1f Hz', freq)
self.scale = scale
self.offset = offset
self.trackers = []
self.frame_rate = freq
self._updated = viz.tick()
self._lock = threading.Lock()
self._thread = None
self._running = False
def __init__(self,
server_name,
freq=OWL.OWL_MAX_FREQUENCY,
scale=(0.001, 0.001, 0.001),
offset=(0, 0, 0),
postprocess=False,
slave=False):
super(Phasespace, self).__init__()
flags = 0
if postprocess:
flags |= OWL.OWL_POSTPROCESS
if slave:
flags |= OWL.OWL_SLAVE
if OWL.owlInit(server_name, flags) < 0:
raise OwlError('phasespace')
OWL.owlSetFloat(OWL.OWL_FREQUENCY, freq)
OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE)
logging.info('running phasespace at %.1f Hz', freq)
self.scale = scale
self.offset = offset
self.trackers = []
self.frame_rate = freq
self._updated = viz.tick()
self._lock = threading.Lock()
self._thread = None
self._running = False
def _reader_thread(self):
while (1):
self._timer.wait()
markers = OWL.owlGetMarkers()
if markers.size() > 0:
#If there's data, add a frame to the buffer
new_frame = np.empty((self._num_points, 3, 1))
new_frame.fill(np.nan)
#Add the markers
for i in range(markers.size()):
m = markers[i]
if m.cond > 0:
new_frame[m.id, 0, 0] = m.x
new_frame[m.id, 1, 0] = m.y
new_frame[m.id, 2, 0] = m.z
# print("%d: %f %f %f" % (m.id, m.x, m.y, m.z))
timestamp = np.zeros((1, ))
timestamp[0] = time.time()
self._output_frame(new_frame, timestamp)
if OWL.owlGetError() != OWL.OWL_NO_ERROR:
print('A mocap read error occurred')
if self._shutdown_flag:
return
def update(self):
'''Update our knowledge of the current data from phasespace.'''
if self._lock.acquire(False):
return
now = viz.tick()
#if now - math.floor(now) < 0.1 / self.frame_rate:
# logging.info('%dus elapsed since last phasespace update',
# 1000000 * (now - self._updated))
self._updated = now
rigids = OWL.owlGetRigids()
markers = OWL.owlGetMarkers()
err = OWL.owlGetError()
if err != OWL.OWL_NO_ERROR:
hex = '0x%x' % err
logging.debug('OWL error %s (%s) getting marker data',
ERROR_MAP.get(err, hex), hex)
return
getting_at = viz.tick() - now
sx, sy, sz = self.scale
ox, oy, oz = self.offset
def transform(x, y, z):
return sz * z + oz, sy * y + oy, sx * x + ox
def swizzle(w, a, b, c):
return c, b, a, -w
for marker in markers:
t, o = marker.id >> 12, marker.id & 0xfff
x, y, z = marker.x, marker.y, marker.z
self.trackers[t].update_markers(
o, Marker(pos=transform(x, y, z), cond=marker.cond),
Marker(pos=(x, y, z), cond=marker.cond))
marker_at = viz.tick() - now
for rigid in rigids:
self.trackers[rigid.id].update_pose(
Pose(pos=transform(*rigid.pose[0:3]),
quat=swizzle(*rigid.pose[3:7]),
cond=rigid.cond))
rigid_at = viz.tick() - now
#logging.info('phasespace update: %dus -> %dus -> %dus',
# 1000000 * getting_at,
# 1000000 * marker_at,
# 1000000 * rigid_at)
self._lock.release()
def update(self):
'''Update our knowledge of the current data from phasespace.'''
if self._lock.acquire(False):
return
now = viz.tick()
#if now - math.floor(now) < 0.1 / self.frame_rate:
# logging.info('%dus elapsed since last phasespace update',
# 1000000 * (now - self._updated))
self._updated = now
rigids = OWL.owlGetRigids()
markers = OWL.owlGetMarkers()
err = OWL.owlGetError()
if err != OWL.OWL_NO_ERROR:
hex = '0x%x' % err
logging.debug(
'OWL error %s (%s) getting marker data',
ERROR_MAP.get(err, hex), hex)
return
getting_at = viz.tick() - now
sx, sy, sz = self.scale
ox, oy, oz = self.offset
def transform(x, y, z):
return sz * z + oz, sy * y + oy, sx * x + ox
def swizzle(w, a, b, c):
return c, b, a, -w
for marker in markers:
t, o = marker.id >> 12, marker.id & 0xfff
x, y, z = marker.x, marker.y, marker.z
self.trackers[t].update_markers(o,
Marker(pos=transform(x, y, z), cond=marker.cond),
Marker(pos=(x, y, z), cond=marker.cond))
marker_at = viz.tick() - now
for rigid in rigids:
self.trackers[rigid.id].update_pose(Pose(
pos=transform(*rigid.pose[0:3]),
quat=swizzle(*rigid.pose[3:7]),
cond=rigid.cond))
rigid_at = viz.tick() - now
#logging.info('phasespace update: %dus -> %dus -> %dus',
# 1000000 * getting_at,
# 1000000 * marker_at,
# 1000000 * rigid_at)
self._lock.release()
def track_points(self, markers):
'''Track a set of markers using phasespace.
Parameters
----------
markers : int or sequence of int
If this is an integer, specifies the number of markers we ought to
track. If this is a sequence of integers, it specifies the IDs of
the markers to track.
Returns
-------
PointTracker :
A PointTracker instance to use for tracking the markers in question.
'''
if isinstance(markers, int):
markers = range(markers)
marker_ids = sorted(markers)
# set up tracker using owl libraries.
index = len(self.trackers)
OWL.owlTrackeri(index, OWL.OWL_CREATE, OWL.OWL_POINT_TRACKER)
for i, marker_id in enumerate(marker_ids):
OWL.owlMarkeri(OWL.MARKER(index, i), OWL.OWL_SET_LED, marker_id)
OWL.owlTracker(index, OWL.OWL_ENABLE)
if OWL.owlGetStatus() == 0:
raise OwlError('point tracker (index {})'.format(index))
tracker = PointTracker(index, marker_ids)
self.trackers.append(tracker)
return tracker
async def rank(ctx,player,map,hero,season,stage):
if not isinstance(ctx.channel,discord.DMChannel):
return
else:
[p,m,h,results]=OWL.player_hero_stats(player,map,hero,season,stage)
print(p,m,h,'\n')
stats=results.groupby('stat_name')['stat_amount'].sum()
print(stats.round(0))
def get_sensor_coordinates_phasespace():
sensor_locations = []
while(1):
markers = []
n = owl.owlGetMarkers(markers, 50)
err = owl.owlGetError()
if (err != owl.OWL_NO_ERROR):
break
if(n==0): continue
if(n == 4):
for i in range(n):
#print "%d) %.2f %.2f %.2f" % (i, markers[i].x, markers[i].y, markers[i].z)
sensor_locations.append([markers[i].x, markers[i].y, markers[i].z])
break
return sensor_locations
def track_points(self, markers):
'''Track a set of markers using phasespace.
Parameters
----------
markers : int or sequence of int
If this is an integer, specifies the number of markers we ought to
track. If this is a sequence of integers, it specifies the IDs of
the markers to track.
Returns
-------
PointTracker :
A PointTracker instance to use for tracking the markers in question.
'''
if isinstance(markers, int):
markers = range(markers)
marker_ids = sorted(markers)
# set up tracker using owl libraries.
index = len(self.trackers)
OWL.owlTrackeri(index, OWL.OWL_CREATE, OWL.OWL_POINT_TRACKER)
for i, marker_id in enumerate(marker_ids):
OWL.owlMarkeri(OWL.MARKER(index, i), OWL.OWL_SET_LED, marker_id)
OWL.owlTracker(index, OWL.OWL_ENABLE)
if OWL.owlGetStatus() == 0:
raise OwlError('point tracker (index {})'.format(index))
tracker = PointTracker(index, marker_ids)
self.trackers.append(tracker)
return tracker
def reset(self):
'''Reset this rigid body based on the current locations of its markers.
'''
logging.info('resetting rigid body %s', self.marker_ids)
# assemble marker positions
positions = []
com = []
with self._lock:
for m in self.marker_ids:
marker = self._raw_markers[self.marker_ids.index(m)]
if marker is None or not 0 < marker.cond < 100:
logging.error('missing marker %d for reset', m)
return
positions.append(marker.pos)
if m in self.center_marker_ids:
com.append(marker.pos)
# compute center of mass
cx = sum(x for x, _, _ in com) / len(com)
cy = sum(y for _, y, _ in com) / len(com)
cz = sum(z for _, _, z in com) / len(com)
logging.info('body center: (%s, %s, %s)', cx, cy, cz)
# reset phasespace marker positions
OWL.owlTracker(self._index, OWL.OWL_DISABLE)
for i, (x, y, z) in enumerate(positions):
OWL.owlMarkerfv(OWL.MARKER(self._index, i), OWL.OWL_SET_POSITION,
(x - cx, y - cy, z - cz))
OWL.owlTracker(self._index, OWL.OWL_ENABLE)
def update(self):
'''Update our knowledge of the current data from phasespace.'''
now = viz.tick()
# logging.info('%dus elapsed since last phasespace update',
# 1000000 * (now - self._updated))
self._updated = now
rigids = OWL.owlGetRigids()
markers = OWL.owlGetMarkers()
err = OWL.owlGetError()
if err != OWL.OWL_NO_ERROR:
hex = '0x%x' % err
logging.debug('OWL error %s (%s) getting marker data',
ERROR_MAP.get(err, hex), hex)
return
sx, sy, sz = self.scale
ox, oy, oz = self.origin
def psPoseToVizardPose(x, y,
z): # converts Phasespace pos to vizard cond
return -sz * z + oz, sy * y + oy, -sx * x + ox
# converts Phasespace quat to vizard quat
def psQuatToVizardQuat(w, a, b, c):
return -c, b, -a, -w
for marker in markers:
if (marker.cond > 0
and marker.cond < self.owlParamMarkerCondThresh):
t, o = marker.id >> 12, marker.id & 0xfff
x, y, z = marker.x, marker.y, marker.z
self.trackers[t].update_markers(
o, Marker(pos=psPoseToVizardPose(x, y, z),
cond=marker.cond),
Marker(pos=(x, y, z), cond=marker.cond))
for rigid in rigids:
if (rigid.cond > 0 and rigid.cond < self.owlParamMarkerCondThresh):
self.trackers[rigid.id].update_pose(
Pose(pos=psPoseToVizardPose(*rigid.pose[0:3]),
quat=psQuatToVizardQuat(*rigid.pose[3:7]),
cond=rigid.cond))
async def on_ready(self):
print('OWL_stats Loaded')
@commands.command(aliases=['stats','per10'])
async def statsper10.(self,ctx,player,*hero,*map):
"""Prints stats per 10 for a player"""
"""Accepts additional options (hero("ALL"),map)"""
if hero.lower()=='all':
hero='All Heroes'
if not hero:
hero='All Heroes'
await ctx.send(OWL.hero_stats_per_10(player,hero,map,2020,1))
def reset(self):
'''Reset this rigid body based on the current locations of its markers.
'''
#logging.info('resetting rigid body %s', self.marker_ids)
print ('Tryingn to reset rigid body %s', self.filename)
localPositions = self._getLocalPositions()
if (localPositions == -1):
return
OWL.owlTracker(self._index, OWL.OWL_DISABLE)
for i, (x, y, z) in enumerate(localPositions):
print 'Resetting marker: ' + str(i)
OWL.owlMarkerfv(OWL.MARKER(self._index, i),
OWL.OWL_SET_POSITION,
[x,y,z])
OWL.owlTracker(self._index, OWL.OWL_ENABLE)
print ('Done. resetting rigid body %s', self.filename)
def reset(self):
'''Reset this rigid body based on the current locations of its markers.
'''
logging.info('resetting rigid body %s', self.marker_ids)
# assemble marker positions
positions = []
com = []
with self._lock:
for m in self.marker_ids:
marker = self._raw_markers[self.marker_ids.index(m)]
if marker is None or not 0 < marker.cond < 100:
logging.error('missing marker %d for reset', m)
return
positions.append(marker.pos)
if m in self.center_marker_ids:
com.append(marker.pos)
# compute center of mass
cx = sum(x for x, _, _ in com) / len(com)
cy = sum(y for _, y, _ in com) / len(com)
cz = sum(z for _, _, z in com) / len(com)
logging.info('body center: (%s, %s, %s)', cx, cy, cz)
# reset phasespace marker positions
OWL.owlTracker(self._index, OWL.OWL_DISABLE)
for i, (x, y, z) in enumerate(positions):
OWL.owlMarkerfv(OWL.MARKER(self._index, i),
OWL.OWL_SET_POSITION,
(x - cx, y - cy, z - cz))
OWL.owlTracker(self._index, OWL.OWL_ENABLE)
def update(self):
'''Update our knowledge of the current data from phasespace.'''
now = viz.tick()
#logging.info('%dus elapsed since last phasespace update',
# 1000000 * (now - self._updated))
self._updated = now
rigids = OWL.owlGetRigids()
markers = OWL.owlGetMarkers()
err = OWL.owlGetError()
if err != OWL.OWL_NO_ERROR:
hex = '0x%x' % err
logging.debug(
'OWL error %s (%s) getting marker data',
ERROR_MAP.get(err, hex), hex)
return
sx, sy, sz = self.scale
ox, oy, oz = self.origin
def psPoseToVizardPose(x, y, z): # converts Phasespace pos to vizard cond
return -sz * z + oz, sy * y + oy, -sx * x + ox
def psQuatToVizardQuat(w, a, b, c): # converts Phasespace quat to vizard quat
return -c, b, -a, -w
for marker in markers:
if( marker.cond > 0 and marker.cond < self.owlParamMarkerCondThresh ):
t, o = marker.id >> 12, marker.id & 0xfff
x, y, z = marker.x, marker.y, marker.z
self.trackers[t].update_markers(o,
Marker(pos=psPoseToVizardPose(x, y, z), cond=marker.cond),
Marker(pos=(x, y, z), cond=marker.cond))
for rigid in rigids:
if( rigid.cond > 0 and rigid.cond < self.owlParamMarkerCondThresh ):
self.trackers[rigid.id].update_pose(Pose(
pos=psPoseToVizardPose(*rigid.pose[0:3]),
quat=psQuatToVizardQuat(*rigid.pose[3:7]),
cond=rigid.cond))
def reset(self):
'''Reset this rigid body based on the current locations of its markers.
'''
#logging.info('resetting rigid body %s', self.marker_ids)
print('Tryingn to reset rigid body %s', self.filename)
localPositions = self._getLocalPositions()
if (localPositions == -1):
return
OWL.owlTracker(self._index, OWL.OWL_DISABLE)
for i, (x, y, z) in enumerate(localPositions):
print('Resetting marker: ' + str(i))
OWL.owlMarkerfv(OWL.MARKER(self._index, i), OWL.OWL_SET_POSITION,
[x, y, z])
OWL.owlTracker(self._index, OWL.OWL_ENABLE)
print('Done. resetting rigid body %s', self.filename)
def reset(self):
'''Reset this rigid body based on the current locations of its markers.
'''
logging.info('resetting rigid body %s', self.marker_ids)
localPositionsPS_mIdx_xyz = self._getLocalPositions(psFormat=True)
# localPositionsPS_mIdx_xyz = []
# for viz_XYZ in localPositionsViz_mIdx_xyz:
# ps_XYZ = (-1000*viz_XYZ[2],1000*viz_XYZ[1],-1000*viz_XYZ[0])
# localPositionsPS_mIdx_xyz.append(ps_XYZ)
if (localPositionsPS_mIdx_xyz == -1):
return
OWL.owlTracker(self._index, OWL.OWL_DISABLE)
for i, (x, y, z) in enumerate(localPositionsPS_mIdx_xyz):
OWL.owlMarkerfv(OWL.MARKER(self._index, i), OWL.OWL_SET_POSITION,
[x, y, z])
OWL.owlTracker(self._index, OWL.OWL_ENABLE)
def update(self):
'''Update our knowledge of the current data from phasespace.'''
now = viz.tick()
#logging.info('%dus elapsed since last phasespace update',
# 1000000 * (now - self._updated))
self._updated = now
frameNum = OWL.owlGetIntegerv(OWL.OWL_FRAME_NUMBER)[0]
rigids = OWL.owlGetRigids()
markers = OWL.owlGetMarkers()
err = OWL.owlGetError()
if err != OWL.OWL_NO_ERROR:
hex = '0x%x' % err
logging.debug('OWL error %s (%s) getting marker data',
ERROR_MAP.get(err, hex), hex)
return
sx, sy, sz = self.scale
ox, oy, oz = self.origin
def psPositionToVizardPosition(
x, y, z): # converts Phasespace pos to vizard cond
#return sz * z + oz, sy * y + oy, sx * x + ox
return -sz * z + oz, sy * y + oy, -sx * x + ox
def psQuatToVizardQuat(w, a, b,
c): # converts Phasespace quat to vizard quat
return -c, b, -a, -w
#trackerPos_tIdx_XYZ = [0] * (len(markers) + len(rigids))
for markerNum, marker in enumerate(markers):
if (marker.cond > 0
and marker.cond < self.owlParamMarkerCondThresh):
t, o = marker.id >> 12, marker.id & 0xfff
x, y, z = marker.x, marker.y, marker.z
vizPos = psPositionToVizardPosition(x, y, z)
self.trackers[t].update_markers(
o, Marker(pos=vizPos, cond=marker.cond,
frame=marker.frame),
Marker(pos=(x, y, z), cond=marker.cond,
frame=marker.frame))
# print 'ID: ' + str(marker.id)
# print 't, o: ' + str(t) + ' ' + str(o)
# print 'markerNum: ' + str(markerNum)
# Append to buffer
#if( self.markerBuffers[markerNum].getSize() == 0 or
#self.markerBuffers[markerNum].getLastFrame() != marker.frame ):
#self.markerBuffers[markerNum].append( ( time.time(), Marker(pos=vizPos, cond=marker.cond, frame = marker.frame)) )
for rigid in rigids:
if (rigid.cond > 0 and rigid.cond < self.owlParamMarkerCondThresh):
vizPos = psPositionToVizardPosition(*rigid.pose[0:3])
#print vizPos
#vizPos = psPositionToVizardPosition(*rigid.pose[0:2])
newPose = Pose(vizPos,
quat=psQuatToVizardQuat(*rigid.pose[3:7]),
cond=rigid.cond,
frame=rigid.frame)
oldPoseFrame = self.trackers[rigid.id]._pose.frame
self.trackers[rigid.id].update_pose(newPose)
if (self.loggingData is True and oldPoseFrame < rigid.frame):
# Write out new data
self.logData(self.trackers[rigid.id])
def __del__(self):
'''Clean up our connection to the phasespace server.'''
OWL.owlDone()
def __init__(self, config=None):
super(phasespaceInterface, self).__init__()
#self.trackPosBuff_sIdx_mIdx_XYZ = collections.deque(maxlen=200) # a deque of tuples (time,[marker tracker list])
self.config = config
### set up a logger and add handlers
self.loggingData = False
self.outFilePath = []
self.logger = False
self.fhandler = False
###
self.startTime = time.time()
if config == None:
print('***Debug mode***')
self.phaseSpaceFilePath = '../Resources/'
self.origin = [0, 0, 0]
self.scale = [0.001, 0.001, 0.001]
self.serverAddress = '192.168.1.230'
self.rigidFileNames_ridx = ['shutterGlass.rb']
self.rigidAvgMarkerList_rIdx_mId = [[3, 4]]
self.rigidOffsetMM_ridx_WorldXYZ = [[0, 0, 0], [0, 0, 0]]
self.owlParamMarkerCount = 10
self.owlParamFrequ = 480 #OWL.OWL_MAX_FREQUENCY
self.owlParamInterp = 0
self.owlParamMarkerCondThresh = 50
self.owlParamPostProcess = 0
self.owlParamModeNum = 3
print '**** Using default MODE #' + str(
self.owlParamModeNum) + ' ****'
else:
self.phaseSpaceFilePath = './Resources/'
self.origin = self.config['phasespace']['origin']
self.scale = self.config['phasespace']['scale']
self.serverAddress = self.config['phasespace']['phaseSpaceIP']
self.rigidFileNames_ridx = self.config['phasespace'][
'rigidBodyList']
self.rigidOffsetMM_ridx_WorldXYZ = eval(
self.config['phasespace']['rigidOffsetMM_ridx_WorldXYZ'])
self.rigidAvgMarkerList_rIdx_mId = eval(
self.config['phasespace']['rigidAvgMarkerList_rIdx_mId'])
self.owlParamModeNum = self.config['phasespace']['owlParamModeNum']
self.owlParamMarkerCount = self.config['phasespace'][
'owlParamMarkerCount']
self.owlParamFrequ = self.config['phasespace']['owlParamFrequ']
self.owlParamInterp = self.config['phasespace']['owlParamInterp']
self.owlParamMarkerCondThresh = self.config['phasespace'][
'owlParamMarkerCondThresh']
self.owlParamRigidCondThresh = self.config['phasespace'][
'owlParamRigidCondThresh']
self.owlParamPostProcess = self.config['phasespace'][
'owlParamPostProcess']
# set default frequency
if (self.owlParamFrequ == 0):
self.owlParamFrequ = OWL.OWL_MAX_FREQUENCY
flags = 'OWL.OWL_MODE' + str(self.owlParamModeNum)
if (self.owlParamPostProcess):
flags = flags + '|OWL.OWL_POSTPROCESS'
initCode = OWL.owlInit(self.serverAddress, eval(flags))
#initCode = owlInit(self.serverAddress,0)
if (initCode < 0):
raise OwlError('phasespace')
print "Mocap: Could not connect to OWL Server"
exit()
else:
print '**** OWL Initialized with flags: ' + flags + ' ****'
OWL.owlSetFloat(OWL.OWL_FREQUENCY, self.owlParamFrequ)
OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE)
OWL.owlSetInteger(OWL.OWL_INTERPOLATION, self.owlParamInterp)
OWL.owlSetInteger(OWL.OWL_TIMESTAMP, OWL.OWL_ENABLE)
self.trackers = []
#self.rigidBuffers = []
######################################################################
######################################################################
# Create rigid objects
# for all rigid bodies passed into the init function...
for rigidIdx in range(len(self.rigidFileNames_ridx)):
#self.rigidBuffers.append( psBuffer() )
rigidOffsetMM_WorldXYZ = [0, 0, 0]
rigidAvgMarkerList_mIdx = [0]
if (len(self.rigidOffsetMM_ridx_WorldXYZ) < rigidIdx):
print 'Rigid offset not set! Using offset of [0,0,0]'
else:
rigidOffsetMM_WorldXYZ = self.rigidOffsetMM_ridx_WorldXYZ[
rigidIdx]
if (len(self.rigidAvgMarkerList_rIdx_mId) < rigidIdx):
print 'Average markers not provided! Using default (marker 0)'
else:
rigidAvgMarkerList_mIdx = self.rigidAvgMarkerList_rIdx_mId[
rigidIdx]
self.track_rigid(self.rigidFileNames_ridx[rigidIdx],
rigidAvgMarkerList_mIdx, rigidOffsetMM_WorldXYZ)
# def __init__(self, index, marker_ids, center_marker_ids, localOffest_xyz):
markersFromRigids = self.get_markers()
if (self.owlParamMarkerCount > len(markersFromRigids)):
markersToTrack = list(
set(range(self.owlParamMarkerCount)) -
set(markersFromRigids.keys()))
self.track_points(markersToTrack)
# ##############
# # Create buffers
#
# self.markerBuffers = []
#
# for mIdx in range(self.owlParamMarkerCount):
# self.markerBuffers.append(psBuffer())
#
# self.rigidBuffers = []
#
# for rigidIdx in range(len(self.rigidFileNames_ridx)):
#
# self.rigidBuffers.append( psBuffer() )
#
###########################################################################
self._updated = viz.tick()
self._thread = None
self._running = False
def __init__(self, msg):
self.msg = 'setting up {}'.format(msg)
self.err = OWL.owlGetError()
def track_rigid(self, markers_orFilename=None, center_markers=None, localOffest_xyz = [0,0,0]):
'''Add a rigid-body tracker to the phasespace workload.
Parameters
----------
markers_orFilename : varied
This parameter describes the markers that make up the rigid body. It
can take several forms:
- An integer, n. This will result in tracking markers 0 through n-1.
- A sequence of integers, (n1, n2, ...). This will use markers n1,
n2, ... to track the rigid body.
- A dictionary, {n1: (x, y, z), n2: ...}. This will use markers n1,
n2, ... to track the rigid body, using relative marker offsets
specified by the values in the dictionary.
- A string. The rigid body configuration will be loaded from the
file named in the string.
If markers_orFilename is an integer or sequence of integers, then random marker
offsets will be assigned initially; call the .reset() method on the
rigid tracker to reassign the marker offsets.
center_markers : sequence of int
This parameter specifies which marker ids to use for computing the
center of the rigid body. If this is None, all markers will be
used.
Returns
-------
RigidTracker :
A RigidTracker instance to use for tracking this rigid body.
'''
def random_offsets():
return [random.random() - 0.5 for _ in range(3)]
marker_map = {}
# If markers_orFilename is a dictionary
if isinstance(markers_orFilename, dict):
marker_map = markers_orFilename
# If markers_orFilename is an int
if isinstance(markers_orFilename, int):
for i in range(markers_orFilename):
marker_map[i] = random_offsets()
# If markers_orFilename is a tuple
if isinstance(markers_orFilename, (tuple, list, set)):
for i in markers_orFilename:
marker_map[i] = random_offsets()
# If markers_orFilename is a string
if isinstance(markers_orFilename, str):
fileLocation = self.phaseSpaceFilePath + markers_orFilename
print "Initializing rigid body: " + fileLocation
# load rigid body configuration from a file.
with open(fileLocation) as handle:
for line in handle:
id, x, y, z = line.replace(',', '').strip().split()
marker_map[int(id)] = float(x), float(y), float(z)
marker_map = sorted(marker_map.iteritems())
marker_ids = tuple(i for i, _ in marker_map)
# set up tracker using owl libraries.
# Marker locations have been read in from file
index = len(self.trackers)
OWL.owlTrackeri(index, OWL.OWL_CREATE, OWL.OWL_RIGID_TRACKER)
for i, (marker_id, pos) in enumerate(marker_map):
OWL.owlMarkeri(OWL.MARKER(index, i), OWL.OWL_SET_LED, marker_id)
OWL.owlMarkerfv(OWL.MARKER(index, i), OWL.OWL_SET_POSITION, pos)
OWL.owlTracker(index, OWL.OWL_ENABLE)
if OWL.owlGetStatus() == 0:
raise OwlError('rigid tracker (index {})'.format(index))
tracker = RigidTracker(index,marker_ids, center_markers or marker_ids, localOffest_xyz)
if isinstance(markers_orFilename, str):
tracker.filepath = self.phaseSpaceFilePath
tracker.filename = markers_orFilename
self.trackers.append(tracker)
return tracker
def __init__(self, ip_address, num_points, framerate=None, buffer_length=2):
self._num_points = num_points
self._shutdown_flag = False
self._start_time = 0
self._frame_count = 0
#Run the read loop at 2000Hz regardless of actual framerate to control
#jitter
self._timer = RateTimer(2000)
#Connect to the server
OWL.owlInit(ip_address, 0)
OWL.owlSetInteger(OWL.OWL_FRAME_BUFFER_SIZE, 0)
tracker = 0
OWL.owlTrackeri(tracker, OWL.OWL_CREATE, OWL.OWL_POINT_TRACKER)
# add the points we want to track to the tracker
for i in range(num_points):
OWL.owlMarkeri(OWL.MARKER(tracker, i), OWL.OWL_SET_LED, i)
#Activate tracker
OWL.owlTracker(0, OWL.OWL_ENABLE)
#Set frequency
if framerate is None:
OWL.owlSetFloat(OWL.OWL_FREQUENCY, OWL.OWL_MAX_FREQUENCY)
# self._timer = RateTimer(1500)
else:
OWL.owlSetFloat(OWL.OWL_FREQUENCY, framerate)
# self._timer = RateTimer(framerate*3)
#Start streaming
OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE)
#Check for errors
if OWL.owlGetError() != OWL.OWL_NO_ERROR:
raise RuntimeError('An error occurred while connecting to the mocap server')
#Initialize a circular read buffer
self._read_buffer = _RingBuffer(buffer_length)
#Start the reader thread
self._reader = Thread(target=self._reader_thread)
self._reader.daemon = True
self._start_time = time.time()
self._reader.start()
def __del__(self):
'''Clean up our connection to the phasespace server.'''
OWL.owlDone()
def close(self):
self._shutdown_flag = True
self._reader.join()
OWL.owlDone()
def __init__(self, msg):
self.msg = 'setting up {}'.format(msg)
self.err = OWL.owlGetError()
async def OWLmap(ctx,map,team,season):
if not isinstance(ctx.channel,discord.DMChannel):
return
else:
await ctx.send(OWL.map_stats(map,team,season=0))
print(OWL.map_stats(map,team,season))
def __init__(self, ip_address, num_points, framerate=None):
super(PhasespaceMocapSource, self).__init__()
self._num_points = num_points
self._shutdown_flag = False
# Run the read loop at 2000Hz regardless of actual framerate to control
# jitter
self._timer = RateTimer(2000)
# Connect to the server
OWL.owlInit(ip_address, 0)
OWL.owlSetInteger(OWL.OWL_FRAME_BUFFER_SIZE, 0)
tracker = 0
OWL.owlTrackeri(tracker, OWL.OWL_CREATE, OWL.OWL_POINT_TRACKER)
# add the points we want to track to the tracker
for i in range(num_points):
OWL.owlMarkeri(OWL.MARKER(tracker, i), OWL.OWL_SET_LED, i)
# Activate tracker
OWL.owlTracker(0, OWL.OWL_ENABLE)
# Set frequency
if framerate is None:
OWL.owlSetFloat(OWL.OWL_FREQUENCY, OWL.OWL_MAX_FREQUENCY)
# self._timer = RateTimer(1500)
else:
OWL.owlSetFloat(OWL.OWL_FREQUENCY, framerate)
# self._timer = RateTimer(framerate*3)
# Start streaming
OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE)
# Check for errors
if OWL.owlGetError() != OWL.OWL_NO_ERROR:
print('%s', (OWL.owlGetError()))
raise RuntimeError(
'An error occurred while connecting to the mocap server')
# Start the reader thread
self._reader = Thread(target=self._reader_thread)
self._reader.daemon = True
self._start_time = time.time()
self._reader.start()
def __init__(self, config=None):
super(phasespaceInterface, self).__init__()
self.rbTrackers_rbIdx = [] # A list full of rigidBodyTrackers
# Note that these are vectors of Phasespace marker objects
self.markerTrackers_mIdx = []
self.config = config
if config == None:
print('***Debug mode***')
self.phaseSpaceFilePath = 'Resources/'
self.origin = [0, 0, 0]
self.scale = [0.001, 0.001, 0.001]
self.serverAddress = '192.168.1.230'
self.rigidFileNames_ridx = ['hmd-nvisMount.rb', 'paddle-hand.rb']
self.rigidAvgMarkerList_rIdx_mId = [[1, 2], [3, 5]]
self.rigidOffset_ridx_XYZ = [[0, 0, 0], [0, 0, 0]]
self.owlParamMarkerCount = 20
self.owlParamFrequ = OWL.OWL_MAX_FREQUENCY
self.owlParamInterp = 0
self.owlParamMarkerCondThresh = 50
self.owlParamPostProcess = 0
self.owlParamModeNum = 1
print('**** Using default MODE #' + str(self.owlParamModeNum) +
' ****')
else:
self.phaseSpaceFilePath = 'Resources/'
self.origin = self.config['phasespace']['origin']
self.scale = self.config['phasespace']['scale']
self.serverAddress = self.config['phasespace']['phaseSpaceIP']
self.rigidFileNames_ridx = self.config['phasespace'][
'rigidBodyList']
self.rigidOffset_ridx_XYZ = eval(
self.config['phasespace']['rigidOffset_ridx_XYZ'])
self.rigidAvgMarkerList_rIdx_mId = eval(
self.config['phasespace']['rigidAvgMarkerList_rIdx_mId'])
self.owlParamModeNum = self.config['phasespace']['owlParamModeNum']
self.owlParamMarkerCount = self.config['phasespace'][
'owlParamMarkerCount']
self.owlParamFrequ = self.config['phasespace']['owlParamFrequ']
self.owlParamInterp = self.config['phasespace']['owlParamInterp']
self.owlParamMarkerCondThresh = self.config['phasespace'][
'owlParamMarkerCondThresh']
self.owlParamRigidCondThresh = self.config['phasespace'][
'owlParamRigidCondThresh']
self.owlParamPostProcess = self.config['phasespace'][
'owlParamPostProcess']
# set default frequency
if (self.owlParamFrequ == 0):
self.owlParamFrequ = OWL.OWL_MAX_FREQUENCY
flags = 'OWL.OWL_MODE' + str(self.owlParamModeNum)
if (self.owlParamPostProcess):
flags = flags + '|OWL.OWL_POSTPROCESS'
initCode = OWL.owlInit(self.serverAddress, eval(flags))
#initCode = owlInit(self.serverAddress,0)
if (initCode < 0):
raise OwlError('phasespace')
print("Mocap: Could not connect to OWL Server")
exit()
else:
print('**** OWL Initialized with flags: ' + flags + ' ****')
OWL.owlSetFloat(OWL.OWL_FREQUENCY, self.owlParamFrequ)
OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE)
OWL.owlSetInteger(OWL.OWL_INTERPOLATION, self.owlParamInterp)
self.trackers = []
######################################################################
######################################################################
# Create rigid objects
#head = mocap.track_rigid('Resources/hmd-nvisMount.rb', center_markers=(1,2))
# for all rigid bodies passed into the init function...
for rigidIdx in range(len(self.rigidFileNames_ridx)):
# initialize rigid and store in self.rbTrackers_rbIdx
rigidOffsetMM_WorldXYZ = [0, 0, 0]
rigidAvgMarkerList_mIdx = [0]
if (len(self.rigidOffset_ridx_XYZ) < rigidIdx):
print('Rigid offset not set! Using offset of [0,0,0]')
else:
rigidOffsetMM_WorldXYZ = self.rigidOffset_ridx_XYZ[rigidIdx]
if (len(self.rigidAvgMarkerList_rIdx_mId) < rigidIdx):
print('Average markers not provided! Using default (marker 0)')
else:
rigidAvgMarkerList_mIdx = self.rigidAvgMarkerList_rIdx_mId[
rigidIdx]
#rigidOffsetMM_WorldXYZ = self.rigidOffset_ridx_XYZ[rigidIdx]
#rigidNameAndPathStr = self.phaseSpaceFilePath + self.rigidFileNames_ridx[rigidIdx]
# self.rbTrackers_rbIdx.append(
# self.track_rigid(rigidNameAndPathStr , rigidAvgMarkerList_mId ) )
# # rigidOffsetMM_WorldXYZ
self.rbTrackers_rbIdx.append(
self.track_rigid(self.rigidFileNames_ridx[rigidIdx],
rigidAvgMarkerList_mIdx,
rigidOffsetMM_WorldXYZ))
# def track_rigid(self, filename, markers=None,
# center_markers=None):
#######################################################################
self._updated = viz.tick()
self._thread = None
self._running = False
def track_rigid(self,
markers_orFilename=None,
center_markers=None,
localOffest_xyz=[0, 0, 0]):
'''Add a rigid-body tracker to the phasespace workload.
Parameters
----------
markers_orFilename : varied
This parameter describes the markers that make up the rigid body. It
can take several forms:
- An integer, n. This will result in tracking markers 0 through n-1.
- A sequence of integers, (n1, n2, ...). This will use markers n1,
n2, ... to track the rigid body.
- A dictionary, {n1: (x, y, z), n2: ...}. This will use markers n1,
n2, ... to track the rigid body, using relative marker offsets
specified by the values in the dictionary.
- A string. The rigid body configuration will be loaded from the
file named in the string.
If markers_orFilename is an integer or sequence of integers, then random marker
offsets will be assigned initially; call the .reset() method on the
rigid tracker to reassign the marker offsets.
center_markers : sequence of int
This parameter specifies which marker ids to use for computing the
center of the rigid body. If this is None, all markers will be
used.
Returns
-------
RigidTracker :
A RigidTracker instance to use for tracking this rigid body.
'''
def random_offsets():
return [random.random() - 0.5 for _ in range(3)]
marker_map = {}
# If markers_orFilename is a dictionary
if isinstance(markers_orFilename, dict):
marker_map = markers_orFilename
# If markers_orFilename is an int
if isinstance(markers_orFilename, int):
for i in range(markers_orFilename):
marker_map[i] = random_offsets()
# If markers_orFilename is a tuple
if isinstance(markers_orFilename, (tuple, list, set)):
for i in markers_orFilename:
marker_map[i] = random_offsets()
# If markers_orFilename is a string
if isinstance(markers_orFilename, str):
fileLocation = self.phaseSpaceFilePath + markers_orFilename
print "Initializing rigid body: " + fileLocation
import os
#import os.path
#f = open(os.path.dirname(__file__) + '/../data.yml')
# load rigid body configuration from a file.
with open(fileLocation) as handle:
for line in handle:
id, x, y, z = line.replace(',', '').strip().split()
marker_map[int(id)] = float(x), float(y), float(z)
marker_map = sorted(marker_map.iteritems())
marker_ids = tuple(i for i, _ in marker_map)
# set up tracker using owl libraries.
# Marker locations have been read in from file
index = len(self.trackers)
OWL.owlTrackeri(index, OWL.OWL_CREATE, OWL.OWL_RIGID_TRACKER)
for i, (marker_id, pos) in enumerate(marker_map):
OWL.owlMarkeri(OWL.MARKER(index, i), OWL.OWL_SET_LED, marker_id)
OWL.owlMarkerfv(OWL.MARKER(index, i), OWL.OWL_SET_POSITION, pos)
OWL.owlTracker(index, OWL.OWL_ENABLE)
if OWL.owlGetStatus() == 0:
raise OwlError('rigid tracker (index {})'.format(index))
tracker = RigidTracker(index, marker_ids, center_markers or marker_ids,
localOffest_xyz)
if isinstance(markers_orFilename, str):
tracker.filepath = self.phaseSpaceFilePath
tracker.filename = markers_orFilename
self.trackers.append(tracker)
return tracker
def __init__(self, config = None):
super(phasespaceInterface, self).__init__()
self.rbTrackers_rbIdx = []; # A list full of rigidBodyTrackers
self.markerTrackers_mIdx = []; # Note that these are vectors of Phasespace marker objects
self.config = config
if config==None:
print('***Debug mode***')
self.phaseSpaceFilePath = 'Resources/'
self.origin = [0,0,0];
self.scale = [0.001,0.001,0.001];
self.serverAddress = '192.168.1.230';
self.rigidFileNames_ridx= ['hmd-nvisMount.rb','paddle-hand.rb']
self.rigidAvgMarkerList_rIdx_mId = [[1,2],[3,5]]
self.rigidOffset_ridx_XYZ = [[0,0,0],[0,0,0]]
self.owlParamMarkerCount = 20
self.owlParamFrequ = OWL.OWL_MAX_FREQUENCY
self.owlParamInterp = 0
self.owlParamMarkerCondThresh = 50
self.owlParamPostProcess = 0
self.owlParamModeNum = 1
print '**** Using default MODE #' + str(self.owlParamModeNum) + ' ****'
else:
self.phaseSpaceFilePath = 'Resources/'
self.origin = self.config['phasespace']['origin']
self.scale = self.config['phasespace']['scale']
self.serverAddress = self.config['phasespace']['phaseSpaceIP']
self.rigidFileNames_ridx = self.config['phasespace']['rigidBodyList']
self.rigidOffset_ridx_XYZ = eval(self.config['phasespace']['rigidOffset_ridx_XYZ'])
self.rigidAvgMarkerList_rIdx_mId = eval(self.config['phasespace']['rigidAvgMarkerList_rIdx_mId'])
self.owlParamModeNum = self.config['phasespace']['owlParamModeNum']
self.owlParamMarkerCount = self.config['phasespace']['owlParamMarkerCount']
self.owlParamFrequ = self.config['phasespace']['owlParamFrequ']
self.owlParamInterp = self.config['phasespace']['owlParamInterp']
self.owlParamMarkerCondThresh = self.config['phasespace']['owlParamMarkerCondThresh']
self.owlParamRigidCondThresh = self.config['phasespace']['owlParamRigidCondThresh']
self.owlParamPostProcess = self.config['phasespace']['owlParamPostProcess']
# set default frequency
if( self.owlParamFrequ == 0 ):
self.owlParamFrequ = OWL.OWL_MAX_FREQUENCY;
flags = 'OWL.OWL_MODE' + str(self.owlParamModeNum)
if( self.owlParamPostProcess ):
flags = flags + '|OWL.OWL_POSTPROCESS'
initCode = OWL.owlInit(self.serverAddress,eval(flags))
#initCode = owlInit(self.serverAddress,0)
if (initCode < 0):
raise OwlError('phasespace')
print "Mocap: Could not connect to OWL Server"
exit()
else:
print '**** OWL Initialized with flags: ' + flags + ' ****'
OWL.owlSetFloat(OWL.OWL_FREQUENCY, self.owlParamFrequ)
OWL.owlSetInteger(OWL.OWL_STREAMING, OWL.OWL_ENABLE)
OWL.owlSetInteger(OWL.OWL_INTERPOLATION, self.owlParamInterp)
self.trackers = []
######################################################################
######################################################################
# Create rigid objects
#head = mocap.track_rigid('Resources/hmd-nvisMount.rb', center_markers=(1,2))
# for all rigid bodies passed into the init function...
for rigidIdx in range(len(self.rigidFileNames_ridx)):
# initialize rigid and store in self.rbTrackers_rbIdx
rigidOffsetMM_WorldXYZ = [0,0,0]
rigidAvgMarkerList_mIdx = [0]
if( len(self.rigidOffset_ridx_XYZ) < rigidIdx ):
print 'Rigid offset not set! Using offset of [0,0,0]'
else:
rigidOffsetMM_WorldXYZ = self.rigidOffset_ridx_XYZ[rigidIdx]
if( len(self.rigidAvgMarkerList_rIdx_mId) < rigidIdx ):
print 'Average markers not provided! Using default (marker 0)'
else:
rigidAvgMarkerList_mIdx = self.rigidAvgMarkerList_rIdx_mId[rigidIdx]
#rigidOffsetMM_WorldXYZ = self.rigidOffset_ridx_XYZ[rigidIdx]
#rigidNameAndPathStr = self.phaseSpaceFilePath + self.rigidFileNames_ridx[rigidIdx]
#self.rbTrackers_rbIdx.append( self.track_rigid(rigidNameAndPathStr , rigidAvgMarkerList_mId ) ) # rigidOffsetMM_WorldXYZ
self.rbTrackers_rbIdx.append( self.track_rigid(self.rigidFileNames_ridx[rigidIdx],rigidAvgMarkerList_mIdx, rigidOffsetMM_WorldXYZ))
#def track_rigid(self, filename, markers=None, center_markers=None):
###########################################################################
self._updated = viz.tick()
self._thread = None
self._running = False
