def run(rdlfile, agent, analyze_keys):
# prepare timestamp lists
timestamps = OrderedDict()
for key in analyze_keys:
timestamps[key] = []
# go through the frames
rdlFile = RDLFile(args.file)
rdlFile.parseRDL()
frameCounter = 0
for frame in rdlFile.frames:
if agent in frame.data.keys():
for key in frame.data[agent].keys():
if key in analyze_keys:
item = frame.data[agent][key]
t = item.timestamp[0] + 1e-6 * item.timestamp[1]
timestamps[key].append(t)
# next
frameCounter += 1
# check for missing keys
for key in analyze_keys:
if key not in timestamps.keys():
raise Exception('no data found for agent {}, key {}'.format(agent, key))
if len(timestamps[key]) == 0:
raise Exception('no data found for agent {}, key {}'.format(agent, key))
# analyze
analyze(timestamps)
class RDLBallDataAdapter():
def __init__(self):
self.data = OrderedDict() # key: age, value: BallData
def load(self, rdlfile, timestamp, timewindow=0.001):
# create a list of BallData objects (one per age) which can be browsed
# form age list
frequency = 30.0
ageList = [timestamp]
c = 1
while c / frequency < timewindow:
ageList = [timestamp - c / frequency
] + ageList + [timestamp + c / frequency]
c += 1
# maintain a list with latest RTDB item, to handle sparsity of RDL
latestData = {}
keys = ["BALL_CANDIDATES_FCS", "BALLS", "DIAG_WORLDMODEL_LOCAL"]
# load RDL
self.rdl = RDLFile(rdlfile)
self.rdl.parseRDL()
# iterate over frames
c = 0 # index of ageList currently being constructed
for frame in self.rdl.frames:
# store as BallData
if frame.age > ageList[c]:
bd = BallData()
key2function = {}
key2function["BALL_CANDIDATES_FCS"] = bd.feedBallCandidates
# it is not useful anymore to look at BALL_CANDIDATES_FCS,
# since the plotter now browses through a few timestamps of tracker-associated data,
# instead of plotting point clouds from entire time ranges at once
key2function["BALLS"] = bd.feedBallResults
key2function["DIAG_WORLDMODEL_LOCAL"] = bd.feedBallDiagnostics
# feed the items
for agent in range(0, NUMROBOTS + 1):
if agent in frame.data:
agentData = frame.data[agent]
for key in key2function.keys():
akey = str((agent, key))
if key in agentData and akey in latestData:
key2function[key](latestData[akey])
del latestData[
akey] # prevent item being copied into next BallData
# store
self.data[ageList[c]] = bd
# next
c += 1
if c >= len(ageList):
break
# update latestData
if (frame.age >= timestamp - timewindow - 1.0
and frame.age <= timestamp + timewindow):
for agent in range(0, NUMROBOTS + 1):
if agent in frame.data:
agentData = frame.data[agent]
# feed the items
for key in keys:
if key in agentData:
akey = str((agent, key))
latestData[akey] = agentData[key]
def find_events(rdl_file_or_frames):
result = []
# allow two kind of arguments: rdl file name (string) or rdl frames (list)
if isinstance(rdl_file_or_frames, str):
rdlfile = rdl_file_or_frames
# load RDL
rdl = RDLFile(rdlfile)
rdl.parseRDL()
rdlframes = rdl.frames
else:
rdlframes = rdl_file_or_frames
# helper
seen = {}
def handle(events, agent):
for e in events:
if not str(e) in seen:
result.append(Event(e, agent))
seen[str(e)] = True
# iterate over frames
key = "EVENT_LIST"
for frame in rdlframes:
for agent in range(0, 20):
if agent in frame.data:
agentData = frame.data[agent]
if key in agentData:
handle(agentData[key].value, agent)
return result
class RDLDiagnostics(FolderWriter):
def __init__(self, rdlfile, agent, folder="/var/tmp/kstplot"):
# setup FolderWriter
FolderWriter.__init__(self, folder)
# setup the rest
self.agent = agent
# guess agent from rdl file
if agent == 0:
self.agent = analyze_lib.get_agent_from_rdl_filename(rdlfile)
self.keys = set()
self.handles = {}
self.prependTimestamp = True
# load RDL
self.rdl = RDLFile(rdlfile)
self.rdl.parseRDL() # TODO improve slowness and mem consumption
def subscribe(self, key, handle=str):
"""
Subscribe to a key. The value may be nicely formatted through a custom function.
"""
self.keys.add(key)
self.handles[key] = handle
def handle(self, key, item):
"""
Handle a RTDB frame item.
"""
try:
valueAsString = self.handles[key](item.value)
except:
print("WARNING: blacklisting key " + key +
" after callback failure:")
traceback.print_exc()
self.keys.remove(key)
return
line = valueAsString
if self.prependTimestamp:
line = "%d.%06d %s" % (item.timestamp[0], item.timestamp[1], line)
self.output(line, key)
def run(self):
nodata = True
for frame in self.rdl.frames:
if frame.data.has_key(self.agent):
agentData = frame.data[self.agent]
for key in list(self.keys):
if agentData.has_key(key):
item = agentData[key] # type: RtDBFrameItem
self.handle(key, item)
nodata = False
if nodata:
raise Exception("no matching data found for agent " +
str(self.agent))
def __init__(self, rdlfile, agent, folder="/var/tmp/kstplot"):
# setup FolderWriter
FolderWriter.__init__(self, folder)
# setup the rest
self.agent = agent
# guess agent from rdl file
if agent == 0:
self.agent = analyze_lib.get_agent_from_rdl_filename(rdlfile)
self.keys = set()
self.handles = {}
self.prependTimestamp = True
# load RDL
self.rdl = RDLFile(rdlfile)
self.rdl.parseRDL() # TODO improve slowness and mem consumption
class RdlParser():
def __init__(self, rdlfile):
self.current_frame = 0
self.rdl = RDLFile(rdlfile)
self.rdl.parseRDL()
def readFrame(self):
if (self.current_frame + 1) < len(self.rdl.frames):
self.current_frame = self.current_frame + 1
return self.rdl.frames[self.current_frame]
else:
return None
def rewind(self):
self.current_frame = 0
def __init__(self, rdlfile):
self.current_frame = 0
self.rdl = RDLFile(rdlfile)
self.rdl.parseRDL()
"4. Move to a position where the ball is visible", "5. Get the ball",
"6. Move to the center", "7. Shoot the Ball \n The ball hits the robot"
]
file = "/home/robocup/ball_tracking/exp/190829/20190829_193432_r3.rdl"
k = 0
if __name__ == "__main__":
# check if file exists
if not os.path.exists(file):
print "Error: '%s' not found." % (file)
exit()
# go through the frames
rdlFile = RDLFile(file)
rdlFile.parseRDL()
frameCounter = 0
## Collected data
# timesamp
T = []
# ball position
X = []
Y = []
Z = []
# ball candidate
X0 = []
Y0 = []
if not os.path.exists(args.file):
print("Error: '%s' not found." % (args.file))
exit()
# expand default arguments, resolve time frame, etc.
ageMin = 0
ageMax = 1e9
if args.agent == []:
args.agent = range(0, 10)
if args.timestamp != "all":
# for now: age -- TODO: parse human-readable timestamp (e.g. '2019-02-28,23:01:33.221551' or substrings without date/microseconds)
ageMin = float(args.timestamp) - args.deltatime
ageMax = float(args.timestamp) + args.deltatime
# go through the frames
rdlFile = RDLFile(args.file)
rdlFile.parseRDL(ageMin, ageMax)
frameCounter = 0
for frame in rdlFile.frames:
# determine whether this frame needs to be shown
inspectFrame = True
if (frame.age < ageMin or frame.age > ageMax):
inspectFrame = False
# print?
if inspectFrame:
# header?
print("frame=%d/%d age=%.3fs" %
(frameCounter, len(rdlFile.frames), frame.age))
# TODO include human-readable timestamp, without date
for agent in frame.data.keys():
showAgent = False
# expand default arguments, resolve time frame, etc.
ageMin = 0
ageMax = 1e9
#if args.agent == []:
# args.agent = range(0,10)
if args.timestamp != "all":
# for now: age -- TODO: parse human-readable timestamp (e.g. '2019-02-28,23:01:33.221551' or substrings without date/microseconds)
ageMin = float(args.timestamp) - args.deltatime
ageMax = float(args.timestamp) + args.deltatime
# setup output file and load input entirely into memory (TODO: make nicer)
if args.inputfile == args.outputfile:
print("Error: input and outputfiles may not be equal.")
exit()
rdlInputFile = RDLFile(args.inputfile)
rdlInputFile.parseRDL(ageMin, ageMax)
rdlOutputFile = RDLFile(args.outputfile)
# go through the frames
for frame in rdlInputFile.frames:
# determine whether this frame needs to be kept
if (frame.age >= ageMin and frame.age <= ageMax):
# correct frame.age w.r.t. new creation timestamp
frame.age = frame.age - ageMin
frame.raw_frame[0] = frame.age # TODO: make nicer
# keep
rdlOutputFile.frames.append(frame)
# TODO: satisfy other options (agent, key)
# TODO: minimize code duplication w.r.t. rdlDump
def analyzeRDL(rdlfile):
# TODO: improve mem usage by rdlLib and remove the need for calling parseRDL()
f = RDLFile(rdlfile)
f.parseRDL()
# helpers
result = {}
emptyCal = {
'robotId': None,
'leftArm': {
'down': 1e6,
'up': 0.0
},
'rightArm': {
'down': 1e6,
'up': 0.0
}
}
def processItem(agent, angleLeft, angleRight):
# zero reading might be produced by peripheralsInterface at init time, ignore them
if angleLeft == 0.0 or angleRight == 0.0:
return
if not result.has_key(agent):
result[agent] = copy.deepcopy(emptyCal)
result[agent]['robotId'] = agent
result[agent]['leftArm']['down'] = min(
result[agent]['leftArm']['down'], angleLeft)
result[agent]['leftArm']['up'] = max(result[agent]['leftArm']['up'],
angleLeft)
result[agent]['rightArm']['down'] = min(
result[agent]['rightArm']['down'], angleRight)
result[agent]['rightArm']['up'] = max(result[agent]['rightArm']['up'],
angleRight)
# work through the data
robotsWithoutBall = set()
robotsWithBall = set()
for frame in f.frames:
for agent in frame.data.keys():
# store the ballHandler angles
key = "BALLHANDLERS_FEEDBACK"
if frame.data[agent].has_key(key):
item = frame.data[agent][key]
angleLeft = item.value[0]
angleRight = item.value[1]
processItem(agent, angleLeft, angleRight)
# extra check on ball possession, since calibration only makes sense if the robot was
# without ball for a while, AND with ball
key = "ROBOT_STATE"
if frame.data[agent].has_key(key):
item = frame.data[agent][key]
hasball = item.value[4]
if hasball:
robotsWithBall.add(agent)
else:
robotsWithoutBall.add(agent)
# warn for bad data regarding ball possession
for agent in result.keys():
if agent in AGENTS_IGNORE:
continue
if agent not in robotsWithoutBall:
print "WARNING: agent %d never lost the ball, so DOWN value is unreliable" % (
agent)
if agent not in robotsWithBall:
print "WARNING: agent %d never got the ball, so UP value is unreliable" % (
agent)
return result