def __init__(self, fname):
self.f = open(fname, "rb")
print("scanning ", fname, "...")
self.scanner = recordreader.RecordScanner(self.f)
self.frametexid = None
self.fronttexid = None
self.f.seek(0, 0)
self.controlstate = []
self.controls = []
self.carstate = []
self.ts = []
self.learn_controls = False
self.lap_timer = False
self.show_frontview = False
self.startlinexy = np.array([7.4, -3])
for frdata in recordreader.RecordIterator(self.f):
self.ts.append(frdata['tstamp'])
(throttle, steering, accel, gyro, servo, wheels,
periods) = frdata['carstate']
self.carstate.append(frdata['carstate'])
self.controls.append([throttle, steering])
self.controlstate.append(frdata['controldata2'])
self.controlstate = np.float32(self.controlstate)
self.controls = np.float32(self.controls)
self.ts = np.array(self.ts)
self.loadframe(0)
self.playing = False
print("done")
self.lm = None
self.track = None
self.unloadlist = []
try:
f = open("lm.txt", "r")
n = int(f.readline())
self.lm = np.zeros((n, 2))
for i in range(n):
self.lm[i] = [float(x) for x in f.readline().strip().split()]
f.close()
except IOError:
print("no lm.txt found; skipping")
try:
#f = open("track.txt", "r")
#n = int(f.readline())
#self.track = np.zeros((n, 5))
#for i in range(n):
# self.track[i] = [
# float(x) for x in f.readline().strip().split()]
#f.close()
self.track = np.load("../trackplan/trackdata.npy")
self.track[:,
2:4] = np.stack([-self.track[:, 3], self.track[:, 2]]).T
except IOError:
print("no track.txt found; skipping")
def __init__(self, fname):
self.unloadlist = []
self.f = open(fname, "rb")
print("scanning ", fname, "...")
self.scanner = recordreader.RecordScanner(self.f)
self.frametexid = None
self.playing = False
self.ts = []
self.camdata = ceiltrack.ceillut()
self.f.seek(0, 0)
self.ceilheight = ceiltrack.CEIL_HEIGHT
# do a full tracking here on load
B = np.float32([HOME[0], HOME[1], 0])
self.track = []
match_time = 0
opt_time = 0
first = True
floordata = []
floormask = None
for frdata in recordreader.RecordIterator(self.f):
if 'yuv420' not in frdata:
continue
self.ts.append(frdata['tstamp'])
yuv420 = frdata['yuv420']
gray = yuv420[:480]
bgr = cv2.cvtColor(yuv420, cv2.COLOR_YUV2BGR_I420)
t0 = time.time()
xy = ceiltrack.match(gray, *self.camdata)
tm = time.time()
if first:
first = False
for i in range(6):
cost, dB = ceiltrack.cost(xy, *B)
B += dB
#B_straight, cost_straight = B, cost
#B = np.float32([HOME[0], HOME[1], np.pi/2])
#for i in range(6):
# cost, dB = ceiltrack.cost(xy, *B)
# B += dB
#if cost_straight < cost:
# B = B_straight
# we need an example frame to initialize the floor lookup table
# to filter out the visible body posts
self.floorlut = ceiltrack.floorlut(gray)
floormask = self.floorlut[0]
else:
for i in range(2):
c, dB = ceiltrack.cost(xy, *B)
B += dB
topt = time.time()
match_time += tm - t0
opt_time += topt - tm
self.track.append(B.copy())
floordata.append(bgr[floormask])
self.ts = np.array(self.ts)
self.track = np.array(self.track)
self.origtrack = self.track.copy()
self.track[:, 0] = -self.track[:, 0]
self.track[:, 2] = -self.track[:, 2]
# mirror the floor-pixel lookup table x coordinates also
self.floorlut[1][0] = -self.floorlut[1][0]
self.floordata = np.array(floordata)
self.loadframe(0)
print("done,", match_time, "secs match_time", opt_time, "sec opt_time")
floorimg = ceiltrack.render_floor(self.track, self.floordata,
self.floorlut[1])
if True:
xgm = ceiltrack.X_GRID * ceiltrack.CEIL_HEIGHT
ygm = ceiltrack.Y_GRID * ceiltrack.CEIL_HEIGHT
Z = 50 # pixels per meter
for x in range(0, 1 + int(1000 / (xgm * Z))):
for y in range(0, 1 + int(500 / (ygm * Z))):
cv2.circle(floorimg, (int(x * xgm * Z), int(y * ygm * Z)),
int(0.25 * Z), (255, 255, 0))
cv2.imwrite("map.png", floorimg)
self.floortex = load_texture(floorimg)
print("home location:", HOME)
