def computeNormals(self, useExisting=False):
if useExisting and 'normal' in self.vertAttr:
#print("Skipping normal computation, normals are already defined")
return
# Expand out an indexed view of our vertices
faceVerts = self.verts[self.tris]
# Compute face normals as a cross product of the face vertices
# TODO why the double colon...?
faceNormals = np.cross(faceVerts[::, 1] - faceVerts[::, 0],
faceVerts[::, 2] - faceVerts[::, 0])
# Area of each face is used for a weighted average below, so save that
faceAreas = 0.5 * norm(faceNormals, axis=1).reshape((self.nTris, 1))
faceNormals = normalize(faceNormals)
self.faceAttr['normal'] = faceNormals
# Now compute vertex normals as a area-weighted average of the face normals
vertNormals = np.zeros(self.verts.shape, dtype=self.verts.dtype)
vertNormals[self.tris[:, 0]] += faceNormals * faceAreas
vertNormals[self.tris[:, 1]] += faceNormals * faceAreas
vertNormals[self.tris[:, 2]] += faceNormals * faceAreas
normalize(vertNormals)
self.vertAttr['normal'] = vertNormals
def ProcessFrame(frame):
out = obj()
frameOut = frame.copy()
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], mean[2]))
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
r = cv2.GaussianBlur(r, (9, 9), 0)
debugFrame("ChannelOfInterest", r)
edges = cv2.Canny(r, std * 3.5, 1.2* std)
debugFrame("edges", edges)
lines = cv2.HoughLinesP(edges, 4, math.pi/180, 200, minLineLength = 100, maxLineGap = 50)
if isinstance(lines, list):
print "numLines: %d" % len(lines[0])
for line in lines[0]:
p1 = (line[0], line[1])
p2 = (line[2], line[3])
dx = p1[0] - p2[0]
dy = abs(p1[1] - p2[1])
theta = math.atan2(dy, dx)
if abs(theta - math.pi/2) < 10 *math.pi/180:
cv2.line(frameOut, p1, p2, (255, 0, 255), 5)
"""
Below is the param info for HoughCircles
image is the imaged being looked at by the function
method cv2.CV_HOUGH_GRADIENT is the only method available
dp the size of accumulator inverse relative to input image, dp = 2 means accumulator is half dim of input image
minDist minimum distance between detected circles, too small and mult neighbor circ founds, big and neighbor circ counted as same
param1 bigger value input into canny, unsure why it is useful therefore not using
param2 vote threshold
minRadius min Radius of circle to look for
maxRadius max Radius of circle to look for
"""
maxrad = 300
minrad = 10
step = 50
for radius in range(minrad + step, maxrad + 1, step):
circles = cv2.HoughCircles(image = r, method = cv2.cv.CV_HOUGH_GRADIENT, dp = .5, minDist = radius * 2, param2 = int((2 * radius * math.pi)/15), minRadius = radius - step, maxRadius = radius)
msg = "minRadius: %d, maxRadius %d" % (radius - step, radius)
if type(circles) != type(None):
print msg + " found: %d" % (len(circles))
for circ in circles[0,:]:
out.append(circ)
out.draw(frameOut)
else:
print msg + " no circ found"
frameOut = out.draw(frameOut)
debugFrame("houghProb", frameOut)
print "-------------------------------"
return out
def ProcessFrame(frame):
out = obj([False, 0, 0, 0])
frameOut = np.copy(frame)
frame = norm(frame)
m = cv2.mean(frame)
red = dist(frame, [m[0], m[1], 255])
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col) / 3
minArea = 200
red = cv2.medianBlur(red, ksize=3)
ret, thresh = cv2.threshold(red,
np.mean(red) - max(np.std(red), 10) * 2.65,
255, cv2.THRESH_BINARY_INV)
debugFrame("threshOg", thresh)
kernel = np.ones((7, 7), np.uint8)
thresh = cv2.erode(thresh, kernel, iterations=2)
debugFrame("thresh", thresh)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
for cont in contours:
if len(cont) < 3:
continue
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
aspectRatio = 0
if w != 0:
aspectRatio = h / w
if aspectRatio < 1 and aspectRatio != 0:
aspectRatio = 1 / aspectRatio
ang += 90
else:
aspectRatio = 1
#caluclating solidity
contArea = cv2.contourArea(cont)
if contArea < .1:
conArea = 1.0
if area == 0:
area = 1
solidity = contArea / (1.0 * area)
loc = (int(center[0]), int(center[1]))
stringOut = "%f" % (solidity)
#cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
if area > 1500 and aspectRatio > 3 and aspectRatio < 13 and solidity > .8:
angp = -1 * ang
#angp = angp - round((angp/180.0)) * 180
stringOut += ", ang: %d" % (angp)
cv2.drawContours(frameOut, [cont], -1, (0, 0, 0), 3)
out = obj([True, angp, center[0], center[1]])
frameOut = out.draw(frameOut)
cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5,
255)
print stringOut
debugFrame("out", frameOut)
return out
def ProcessFrame(frame):
print("called")
out = obj()
frameOut = frame.copy()
HEIGHT, WIDTH, _ = frame.shape
contImg = np.zeros((HEIGHT, WIDTH, 3), np.uint8)
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], mean[2]))
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
r = cv2.GaussianBlur(r, (9, 9), 0)
debugFrame("red", r)
if std > 6:
edges = cv2.Canny(r, std * 1.8, std * 1.2)
else:
edges = cv2.Canny(r, 30, 20)
debugFrame("edges", edges)
lines = cv2.HoughLinesP(edges,
4,
math.pi / 180,
200,
minLineLength=100,
maxLineGap=50)
if isinstance(lines, list):
print "numLines: %d" % len(lines[0])
for line in lines[0]:
p1 = (line[0], line[1])
p2 = (line[2], line[3])
dx = p1[0] - p2[0]
dy = abs(p1[1] - p2[1])
theta = math.atan2(dy, dx)
if abs(theta - math.pi / 2) < 10 * math.pi / 180:
cv2.line(frameOut, p1, p2, (255, 0, 255), 5)
contours, _ = cv2.findContours(edges, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
dice = []
for i in range(len(contours)):
area = cv2.contourArea(contours[i])
if area < WIDTH * HEIGHT / 2:
cv2.drawContours(contImg, contours, i, (255, 255, 255), 5)
x, y, w, h = cv2.boundingRect(contours[i])
if w * h > 700 and h != 0 and w / h < 2 and w / h > 1 / 2:
cv2.rectangle(frameOut, (x, y), (x + w, y + h), (0, 0, 255), 2)
dice.append([x + w / 2, y + h / 2, (w + h) / 2])
debugFrame("contours", contImg)
print "Contours: ", len(contours)
if (len(dice) >= 2):
for die in dice:
out.append(die)
frameOut = out.draw(frameOut)
debugFrame("houghProb", frameOut)
print "-------------------------------"
return out
def computeNormals(self, useExisting=False):
if useExisting and 'normal' in self.vertAttr:
#print("Skipping normal computation, normals are already defined")
return
# Expand out an indexed view of our vertices
faceVerts = self.verts[self.tris]
# Compute face normals as a cross product of the face vertices
# TODO why the double colon...?
faceNormals = np.cross(faceVerts[::,1 ] - faceVerts[::,0],
faceVerts[::,2 ] - faceVerts[::,0])
# Area of each face is used for a weighted average below, so save that
faceAreas = 0.5 * norm(faceNormals, axis=1).reshape((self.nTris, 1))
faceNormals = normalize(faceNormals)
self.faceAttr['normal'] = faceNormals
# Now compute vertex normals as a area-weighted average of the face normals
vertNormals = np.zeros( self.verts.shape, dtype=self.verts.dtype)
vertNormals[ self.tris[:,0] ] += faceNormals * faceAreas
vertNormals[ self.tris[:,1] ] += faceNormals * faceAreas
vertNormals[ self.tris[:,2] ] += faceNormals * faceAreas
normalize(vertNormals)
self.vertAttr['normal'] = vertNormals
def ProcessFrame(frame):
out = obj([False, 0, 0])
frameOut = frame.copy()
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], mean[2]))
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
r = cv2.GaussianBlur(r, (9, 9), 0)
debugFrame("ChannelOfInterest", r)
edges = cv2.Canny(r, std * 3.5, 1.2* std)
debugFrame("edges", edges)
lines = cv2.HoughLinesP(edges, 4, math.pi/180, 200, minLineLength = 100, maxLineGap = 50)
if lines != None:
print "numLines: %d" % len(lines[0])
for line in lines[0]:
p1 = (line[0], line[1])
p2 = (line[2], line[3])
dx = p1[0] - p2[0]
dy = abs(p1[1] - p2[1])
theta = math.atan2(dy, dx)
if abs(theta - math.pi/2) < 10 *math.pi/180:
cv2.line(frameOut, p1, p2, (255, 0, 255), 5)
"""
Below is the param info for HoughCircles
image is the imaged being looked at by the function
method cv2.CV_HOUGH_GRADIENT is the only method available
dp the size of accumulator inverse relative to input image, dp = 2 means accumulator is half dim of input image
minDist minimum distance between detected circles, too small and mult neighbor circ founds, big and neighbor circ counted as same
param1 bigger value input into canny, unsure why it is useful therefore not using
param2 vote threshold
minRadius min Radius of circle to look for
maxRadius max Radius of circle to look for
"""
maxrad = 300
minrad = 10
step = 50
for radius in range(minrad + step, maxrad + 1, step):
circles = cv2.HoughCircles(image = r, method = cv2.cv.CV_HOUGH_GRADIENT, dp = .5, minDist = radius * 2, param2 = int((2 * radius * math.pi)/20), minRadius = radius - step, maxRadius = radius)
msg = "minRadius: %d, maxRadius %d" % (radius - step, radius)
if circles != None:
print msg + " found: %d" % (len(circles))
for circ in circles[0,:]:
cv2.circle(frameOut, (int(circ[0]), int(circ[1])), circ[2], (0, 255, 255), 2)
cv2.circle(frameOut, (int(circ[0]), int(circ[1])), 2, (0, 0, 255), 2)
else:
print msg + " no circ found"
frameOut = out.draw(frameOut)
debugFrame("houghProb", frameOut)
print "-------------------------------"
return out.dict()
def ProcessFrame(frame):
out = obj([False, 0, 0, 0])
frameOut = np.copy(frame)
frame = norm(frame)
m = cv2.mean(frame)
red = dist(frame, [m[0], m[1], 255])
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col)/3
minArea = 200
red = cv2.medianBlur(red, ksize = 3)
ret, thresh = cv2.threshold(red, np.mean(red) - max(np.std(red), 10) * 2.65, 255, cv2.THRESH_BINARY_INV)
debugFrame("threshOg", thresh)
kernel = np.ones((7,7),np.uint8)
thresh = cv2.erode(thresh,kernel, iterations = 2)
debugFrame("thresh", thresh)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
for cont in contours:
if len(cont) < 3:
continue
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
aspectRatio = 0
if w != 0:
aspectRatio = h / w
if aspectRatio < 1 and aspectRatio != 0:
aspectRatio = 1/aspectRatio
ang+=90
else:
aspectRatio = 1
#caluclating solidity
contArea = cv2.contourArea(cont)
if contArea < .1:
conArea = 1.0
if area == 0:
area = 1
solidity = contArea / (1.0 * area)
loc = (int(center[0]), int(center[1]))
stringOut = "%f" % (solidity)
#cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
if area > 1500 and aspectRatio > 3 and aspectRatio < 13 and solidity > .8:
angp = -1 * ang
#angp = angp - round((angp/180.0)) * 180
stringOut += ", ang: %d" % (angp)
cv2.drawContours(frameOut, [cont], -1, (0,0, 0), 3)
out = obj([True, angp, center[0], center[1]])
frameOut = out.draw(frameOut)
cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
print stringOut
debugFrame("out", frameOut)
return out
def ProcessFrame(frame):
print("called")
out = obj()
frameOut = frame.copy()
HEIGHT,WIDTH,_ = frame.shape
contImg = np.zeros((HEIGHT,WIDTH,3), np.uint8)
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], mean[2]))
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
r = cv2.GaussianBlur(r, (9, 9), 0)
debugFrame("red", r)
if std > 6:
edges = cv2.Canny(r, std * 1.8 , std * 1.2)
else:
edges = cv2.Canny(r, 30 , 20)
debugFrame("edges", edges)
lines = cv2.HoughLinesP(edges, 4, math.pi/180, 200, minLineLength = 100, maxLineGap = 50)
if isinstance(lines, list):
print "numLines: %d" % len(lines[0])
for line in lines[0]:
p1 = (line[0], line[1])
p2 = (line[2], line[3])
dx = p1[0] - p2[0]
dy = abs(p1[1] - p2[1])
theta = math.atan2(dy, dx)
if abs(theta - math.pi/2) < 10 *math.pi/180:
cv2.line(frameOut, p1, p2, (255, 0, 255), 5)
contours, _ = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
dice = []
for i in range(len(contours)):
area = cv2.contourArea(contours[i])
if area < WIDTH * HEIGHT / 2:
cv2.drawContours(contImg, contours, i, (255, 255, 255), 5)
x,y,w,h = cv2.boundingRect(contours[i])
if w * h > 700 and h != 0 and w/h < 2 and w/h > 1/2:
cv2.rectangle(frameOut,(x,y),(x+w,y+h),(0,0,255),2)
dice.append([x + w/2, y + h/2, (w+h) / 2])
debugFrame("contours", contImg)
print "Contours: ", len(contours)
if(len(dice) >= 2):
for die in dice:
out.append(die)
frameOut = out.draw(frameOut)
debugFrame("houghProb", frameOut)
print "-------------------------------"
return out
def ProcessFrame(frame):
out = obj([False, 0, 0])
frameOut = frame.copy()
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
print mean
r = cv2.cvtColor(frameOut, cv2.COLOR_BGR2GRAY)
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
debugFrame("COI", r)
r = cv2.GaussianBlur(r, (7, 7), 0)
edges = cv2.Canny(r, std * 2.0 , 1.3 * std)
debugFrame("edges", edges)
lines = cv2.HoughLines(edges, 3, math.pi/180, 110)
poles = []
if lines is list:
print "numLines: %d" % len(lines[0])
for line in lines[0]:
r = line[0]
theta = line[1]
if ( abs(theta - round((theta/math.pi)) * math.pi) < 3.0 * math.pi / 180.0):
a = math.cos(theta)
b = math.sin(theta)
x0 = a * r
y0 = b * r
pt1 = (int(x0 - 100*b), int(y0 + 100 * a))
pt2 = (int(x0 + 100 * b), int(y0 - 100 * a))
poles.append(((x0, y0), pt1, pt2))
cv2.line(frameOut, pt1, pt2, (255, 0, 255), 5)
#filtering out the matching poles
poles = sorted(poles, key = sortPoles)
if len(poles) > 0:
gatePoles = [poles[0]]
for pole in poles:
if abs(pole[0][0] - gatePoles[-1][0][0]) > 80:
gatePoles.append(pole)
for pole in gatePoles:
cv2.line(frameOut, pole[1], pole[2], (0, 0, 255), 10)
if len(gatePoles) == 2:
out = obj([True, gatePoles[0][0][0], gatePoles[1][0][0]])
print "len Poles: %d, len gatePoles: %d" % (len(poles), len(gatePoles))
frameOut = out.draw(frameOut)
debugFrame("houghReg", frameOut)
print "-------------------------------"
return out
def ProcessFrame(frame):
out = {}
out[ort] = -400
out[cpX] = 0
out[cpY] = 0
out[found] = False
pRed = frame[:, :, 2]
frame = norm(frame)
red = frame[:,:,2]
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col)/3
minArea = 200
red = cv2.medianBlur(red, ksize = 3)
ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY)
debugFrame("threshOg", thresh)
kernel = np.ones((7,7),np.uint8)
thresh = cv2.erode(thresh,kernel, iterations = 2)
debugFrame("thresh", thresh)
res, contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
possiblePoles = []
for cont in contours:
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
aspectRatio = 0
if w != 0:
aspectRatio = h / w
if aspectRatio < 1 and aspectRatio != 0:
aspectRatio = 1/aspectRatio
if area > 1000 and aspectRatio > 3 and aspectRatio < 13:
angp = abs(ang)
angp = abs(angp - round((angp/90.0)) * 90)
loc = (int(center[0]), int(center[1]))
cv2.drawContours(frame, [cont], -1, (0,0, 0), 3)
[vx,vy,x,y] = cv2.fitLine(cont, cv2.DIST_L2,0,0.01,0.01)
if vx > .01:
lefty = int((-x*vy/vx) + y)
righty = int(((col-x)*vy/vx)+y)
cv2.line(frame,(col-1,righty),(0,lefty),(0,255,0),2)
angm = math.degrees(math.atan2(vy,vx))
angM = angm - round((angm/90.0)) * 90
stringOut = "angP %.2f, asp %.2f" % (angM, aspectRatio)
cv2.putText(frame, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
out[ort] = int(angM)
out[cpY] = center[1]
out[cpX] = center[0]
out[found] = True
debugFrame("out", frame)
return out
def ProcessFrame(frame):
out = obj([False, 0, 0])
frameOut = frame.copy()
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], 255))
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
r = cv2.GaussianBlur(r, (9, 9), 0)
debugFrame("red", r)
edges = cv2.Canny(r, std * 1.8, 1.2 * std)
debugFrame("edges", edges)
lines = cv2.HoughLinesP(edges,
4,
math.pi / 180,
145,
minLineLength=200,
maxLineGap=80)
poles = []
if lines != None:
print "numLines: %d" % len(lines[0])
for line in lines[0]:
p1 = (line[0], line[1])
p2 = (line[2], line[3])
dx = p1[0] - p2[0]
dy = abs(p1[1] - p2[1])
theta = math.atan2(dy, dx)
if abs(theta - math.pi / 2) < 10 * math.pi / 180:
cv2.line(frameOut, p1, p2, (255, 0, 255), 5)
poles.append((p1, p2))
#filtering out the matching poles
poles = sorted(poles, key=sortPoles)
if len(poles) > 0:
gatePoles = [poles[0]]
for pole in poles:
if abs(pole[0][0] - gatePoles[-1][0][0]) > 80:
gatePoles.append(pole)
for pole in gatePoles:
cv2.line(frameOut, pole[0], pole[1], (0, 0, 255), 10)
if len(gatePoles) == 2:
out = obj([True, gatePoles[0][0][0], gatePoles[1][0][0]])
print "len Poles: %d, len gatePoles: %d" % (len(poles), len(gatePoles))
frameOut = out.draw(frameOut)
debugFrame("houghProb", frameOut)
print "-------------------------------"
return out.dict()
def ProcessFrame(frame):
out = {}
out[lp] = 0
out[rp] = 0
out[cp] = 0
frame = norm(frame)
red = frame[:, :, 2]
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col) / 3
minArea = 200
red = cv2.medianBlur(red, ksize=3)
ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY)
kernel = np.ones((5, 5), np.uint8)
thresh = cv2.dilate(thresh, kernel, iterations=1)
debugFrame("thresh", thresh)
_, contours, hierchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
possiblePoles = []
for cont in contours:
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
aspectRatio = h / w
if aspectRatio < 1:
aspectRatio = 1 / aspectRatio
angp = abs(ang)
angp = abs(angp - round((angp / 90.0)) * 90)
if angp < 10.0 and area > minArea and abs(aspectRatio - 7) < 2:
possiblePoles.append((cont, center))
possiblePoles = sorted(possiblePoles, key=sortPoles)
if len(possiblePoles) > 1:
prevPole = possiblePoles[0]
poles = [prevPole]
for pole in possiblePoles:
diff = abs(pole[1][0] - prevPole[1][0])
if (diff > 20):
poles.append(pole)
prevPole = pole
if (len(poles) == 2):
left = poles[0]
right = poles[1]
out[rp] = int(right[1][0])
out[lp] = int(left[1][0])
out[cp] = out[lp] + (out[rp] - out[lp]) / 2
cv2.circle(frame, (out[cp], int((right[1][1] + left[1][1]) / 2)),
10, (0, 0, 0), 3)
cv2.drawContours(frame, [left[0], right[0]], -1, (0, 0, 0), 3)
debugFrame("out", frame)
return out
def ProcessFrame(frame):
out = obj([False, 0, 0])
frameOut = frame.copy()
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], 255))
r = cv2.cvtColor(frameOut, cv2.COLOR_BGR2GRAY)
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
r = frame[:, :, 2]
debugFrame("red", r)
r = cv2.GaussianBlur(r, (7, 7), 0)
edges = cv2.Canny(r, std * 2.0, 1.3 * std)
debugFrame("edges", edges)
lines = cv2.HoughLines(edges, 3, math.pi / 180, 110)
poles = []
if lines != None:
print "numLines: %d" % len(lines[0])
for line in lines[0]:
r = line[0]
theta = line[1]
if (abs(theta - round((theta / math.pi)) * math.pi) <
3.0 * math.pi / 180.0):
a = math.cos(theta)
b = math.sin(theta)
x0 = a * r
y0 = b * r
pt1 = (int(x0 - 100 * b), int(y0 + 100 * a))
pt2 = (int(x0 + 100 * b), int(y0 - 100 * a))
poles.append(((x0, y0), pt1, pt2))
cv2.line(frameOut, pt1, pt2, (255, 0, 255), 5)
#filtering out the matching poles
poles = sorted(poles, key=sortPoles)
if len(poles) > 0:
gatePoles = [poles[0]]
for pole in poles:
if abs(pole[0][0] - gatePoles[-1][0][0]) > 80:
gatePoles.append(pole)
for pole in gatePoles:
cv2.line(frameOut, pole[1], pole[2], (0, 0, 255), 10)
if len(gatePoles) == 2:
out = obj([True, gatePoles[0][0][0], gatePoles[1][0][0]])
print "len Poles: %d, len gatePoles: %d" % (len(poles), len(gatePoles))
frameOut = out.draw(frameOut)
debugFrame("houghReg", frameOut)
print "-------------------------------"
return out.dict()
def ProcessFrame(frame):
out = {}
out[lp] = 0
out[rp] = 0
out[cp] = 0
frame = norm(frame)
red = frame[:,:,2]
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col)/3
minArea = 200
red = cv2.medianBlur(red, ksize = 3)
ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY)
kernel = np.ones((5,5),np.uint8)
thresh = cv2.dilate(thresh,kernel, iterations = 1)
debugFrame("thresh", thresh)
_, contours, hierchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
possiblePoles = []
for cont in contours:
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
aspectRatio = h / w
if aspectRatio < 1:
aspectRatio = 1/aspectRatio
angp = abs(ang)
angp = abs(angp - round((angp/90.0)) * 90)
if angp < 10.0 and area > minArea and abs(aspectRatio - 7) < 2:
possiblePoles.append((cont, center))
possiblePoles = sorted(possiblePoles, key = sortPoles)
if len(possiblePoles) > 1:
prevPole = possiblePoles[0]
poles = [prevPole]
for pole in possiblePoles:
diff = abs( pole[1][0] - prevPole[1][0])
if (diff > 20):
poles.append(pole)
prevPole = pole
if (len(poles) == 2):
left = poles[0]
right = poles[1]
out[rp] = int(right[1][0])
out[lp] = int(left[1][0])
out[cp] = out[lp] + (out[rp] - out[lp])/2
cv2.circle(frame, (out[cp], int((right[1][1] + left[1][1])/2)), 10, (0, 0, 0), 3)
cv2.drawContours(frame, [left[0], right[0]], -1, (0,0, 0), 3)
debugFrame("out", frame)
return out
def ProcessFrame(frame):
out = obj([False, 0, 0, 0])
pRed = frame[:, :, 2]
frame = norm(frame)
red = frame[:,:,2]
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col)/3
minArea = 200
red = cv2.medianBlur(red, ksize = 3)
ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY)
debugFrame("threshOg", thresh)
kernel = np.ones((7,7),np.uint8)
thresh = cv2.erode(thresh,kernel, iterations = 2)
debugFrame("thresh", thresh)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
possiblePoles = []
if contours != None:
for cont in contours:
if len(cont) < 3:
continue;
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
aspectRatio = 0
if w != 0:
aspectRatio = h / w
if aspectRatio < 1 and aspectRatio != 0:
aspectRatio = 1/aspectRatio
if area > 1000 and aspectRatio > 3 and aspectRatio < 13:
angp = abs(ang)
angp = abs(angp - round((angp/90.0)) * 90)
loc = (int(center[0]), int(center[1]))
cv2.drawContours(frame, [cont], -1, (0,0, 0), 3)
[vx,vy,x,y] = cv2.fitLine(cont, 2,0,0.01,0.01)
if vx > .01:
lefty = int((-x*vy/vx) + y)
righty = int(((col-x)*vy/vx)+y)
cv2.line(frame,(col-1,righty),(0,lefty),(0,255,0),2)
angm = math.degrees(math.atan2(vy,vx))
angM = angm - round((angm/90.0)) * 90
stringOut = "angP %.2f, asp %.2f" % (angM, aspectRatio)
cv2.putText(frame, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
out = obj([True, angM, center[0], center[1]])
out.draw(frame)
debugFrame("out", frame)
return out
def ProcessFrame(frame):
out = obj()
frameOut = frame.copy()
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], mean[2]))
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
r = cv2.GaussianBlur(r, (9, 9), 0)
debugFrame("ChannelOfInterest", r)
edges = cv2.Canny(r, std * 2.5, 1.2* std)
debugFrame("edges", edges)
height, width = frame.shape[:2]
outImg = np.zeros((height,width,3), np.uint8)
#frameOut = outImg
"""
Below is the param info for HoughCircles
image is the imaged being looked at by the function
method cv2.CV_HOUGH_GRADIENT is the only method available
dp the size of accumulator inverse relative to input image, dp = 2 means accumulator is half dim of input image
minDist minimum distance between detected circles, too small and mult neighbor circ founds, big and neighbor circ counted as same
param1 bigger value input into canny, unsure why it is useful therefore not using
param2 vote threshold
minRadius min Radius of circle to look for
maxRadius max Radius of circle to look for
"""
""" Find and make list of circles in the image """
drawAvg = False
circleList = []
minDeg = 80.0
s = .8
d = 3.7
maxrad = 150
minrad = 30
step = 50
for radius in range(minrad + step, maxrad + 1, step):
circles = cv2.HoughCircles(image = r, method = cv2.cv.CV_HOUGH_GRADIENT, dp = 4.2, minDist = 2 * radius, param2 = int((2 * radius * math.pi)/8.94), minRadius = radius - step, maxRadius = radius)
msg = "minRadius: %d, maxRadius %d" % (radius - step, radius)
if type(circles) != type(None):
print msg + " found: %d" % (len(circles))
for circ in circles[0,:]:
circleList.append(circ)
if seeCircles:
cv2.circle(frameOut, (circ[0], circ[1]), circ[2], (0,0,0), 2, 8, 0 )
else:
print msg + " no circ found"
if len(circleList) > 2:
pts = []
for circle in circleList:
#out.append(circle)
if seeCircles:
cv2.circle(frameOut, (int(circle[0]), int(circle[1])), int(circle[2]), (255,0,0), 2, 8, 0 )
cv2.circle(frameOut, (int(circle[0]), int(circle[1])), 7, (0,0,0), 2, 8, 0 )
pts.append([circle[0], circle[1]])
contour = np.array(pts, dtype=np.int32)
(x,y), r = cv2.minEnclosingCircle(contour)
out.append([int(x), int(y), int(r)])
#cv2.circle(frameOut, (int(x), int(y)), int(r), (0,0,0), 2, 8, 0 )
frameOut = out.draw(frameOut)
out.draw(frameOut)
debugFrame("houghProb", frameOut)
print "-------------------------------"
return out
def ProcessFrame(frame):
out = obj([False, 0, 0])
frameOut = np.copy(frame)
frame = norm(frame)
m = cv2.mean(frame)
red = dist(frame, [m[0], m[1], max(m[2] + 50, 255)])
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = 30000
minArea = 1000
ret, thresh = cv2.threshold(red, np.mean(red) - np.std(red) * 1.5, 255, cv2.THRESH_BINARY_INV)
kernel = np.ones((5,5),np.uint8)
kernel[:, 0:1] = 0
kernel[:, 3:5] = 0
debugFrame("threshOg", thresh)
thresh = cv2.erode(thresh,kernel, iterations = 1)
thresh = cv2.dilate(thresh, kernel, iterations = 2)
debugFrame("thresh", thresh)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
possiblePoles = []
for cont in contours:
#cv2.drawContours(frame, [cont], -1, (0, 0, 255), 3)
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
if w > 0:
aspectRatio = h / w
if aspectRatio < 1 and aspectRatio != 0:
aspectRatio = 1/aspectRatio
ang+=90
else:
aspectRation = 20
angp = abs(ang)
angp = abs(angp - round((angp/180.0)) * 180)
stringOut = "%d %d %d" % (angp, area, aspectRatio)
cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
if (angp < 10 or angp > 170) and area > minArea and area < maxArea and abs(aspectRatio - 9) <= 4:
possiblePoles.append((cont, center))
possiblePoles = sorted(possiblePoles, key = sortPoles)
for pole in possiblePoles:
cv2.drawContours(frameOut, [pole[0]], -1, (0, 0, 255), 3)
(center, (w, h), ang) = cv2.minAreaRect(pole[0])
aspectRatio = h / w
if aspectRatio < 1:
ang+=90
angp = abs(ang)
angp = abs(angp - round((angp/180)) * 180)
area = h * w
stringOut = "%d %d %d" % (angp, area, aspectRatio)
cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, 1, 255)
if len(possiblePoles) > 1:
prevPole = possiblePoles[0]
poles = [prevPole]
for pole in possiblePoles:
cv2.drawContours(frame, [pole[0]], -1, (0, 0, 255), 3)
diff = abs( pole[1][0] - prevPole[1][0])
if (diff > 20):
poles.append(pole)
prevPole = pole
if (len(poles) == 2):
left = poles[0]
right = poles[1]
out = obj([True, int(right[1][0]), int(left[1][0])])
frameOut = out.draw(frameOut)
cv2.drawContours(frameOut, [left[0], right[0]], -1, (0,0, 0), 3)
debugFrame("out", frameOut)
return out
def ProcessFrame(frame):
out = obj([False, 0, 0])
frameOut = frame.copy()
frame = norm(frame)
mean, std = cv2.meanStdDev(frame)
print "r mean: %d" % (mean[2])
r = dist(frame, (mean[0], mean[1], max(mean[2], 80)))
mean, std = cv2.meanStdDev(r)
print "m: %d, std %d" % (mean, std)
#r = frame[:, :, 2]
r = cv2.GaussianBlur(r, (9, 9), 0)
debugFrame("red", r)
if std > 6:
edges = cv2.Canny(r, std * 2.0, std * 1.1)
else:
edges = cv2.Canny(r, 30 , 20)
debugFrame("edges", edges)
contours, _ = cv2.findContours(r, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
possiblePoles = []
for cont in contours:
cv2.drawContours(frame, [cont], -1, (0, 0, 255), 3)
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
if w > 0:
aspectRatio = h / w
if aspectRatio < 1 and aspectRatio != 0:
aspectRatio = 1/aspectRatio
ang+=90
else:
aspectRation = 20
angp = abs(ang)
angp = abs(angp - round((angp/180.0)) * 180)
stringOut = "%d %d %d" % (angp, area, aspectRatio)
cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
if (angp < 10 or angp > 170) and area > minArea and area < maxArea and abs(aspectRatio) <= 2:
possiblePoles.append((cont, center))
possiblePoles = sorted(possiblePoles, key = sortPoles)
for pole in possiblePoles:
cv2.drawContours(frameOut, [pole[0]], -1, (0, 0, 255), 3)
(center, (w, h), ang) = cv2.minAreaRect(pole[0])
aspectRatio = h / w
if aspectRatio < 1:
ang+=90
angp = abs(ang)
angp = abs(angp - round((angp/180)) * 180)
area = h * w
stringOut = "%d %d %d" % (angp, area, aspectRatio)
cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, 1, 255)
if len(possiblePoles) > 1:
prevPole = possiblePoles[0]
poles = [prevPole]
for pole in possiblePoles:
cv2.drawContours(frame, [pole[0]], -1, (0, 0, 255), 3)
diff = abs( pole[1][0] - prevPole[1][0])
if (diff > 20):
poles.append(pole)
prevPole = pole
if (len(poles) == 2):
left = poles[0]
right = poles[1]
out = obj([True, int(right[1][0]), int(left[1][0])])
frameOut = out.draw(frameOut)
cv2.drawContours(frameOut, [left[0], right[0]], -1, (0,0, 0), 3)
debugFrame("out", frameOut)
return out
def panel_control(data_array, mQ):
n_program_state = 1
t_quickload_timer = 0
t_frame_start_time = time.time()
BA_input_buffer = bytearray()
f_first_pass = 1
x_trim = [0, 0, 0]
throttle_inhib = False
spd_err = 0
spd_err_p = 0
while 1:
if time.time() - t_frame_start_time >= Settings.c_loop_frame_rate:
# record the start of the processing so we can get timing data
t_frame_start_time = time.time()
# STATE = PANEL CONN - Connect to the panel
if n_program_state == 1:
mQ.put((0, 'Connecting to the panel....'))
try:
ser = serial.Serial('COM3', 115200, timeout=0.1)
mQ.put((0, 'Connected to the panel'))
time.sleep(
1
) # serial needs a little bit of time to initialise, otherwise later code - esp CNIA fails
n_program_state = 2
except serial.serialutil.SerialException:
mQ.put((1, 'Could not connect to the panel'))
time.sleep(5) # to avoid spamming the message queue
pass
# STATE = GAME CONN - Connect to the KRPC Server
if n_program_state == 2:
mQ.put((0, 'Connecting to the game server....'))
try:
conn = krpc.connect(name='Game Controller')
mQ.put((0, 'Connected to the game server'))
n_program_state = 3
except ConnectionRefusedError:
mQ.put((1, 'Could not connect to the game server'))
pass
# STATE = LINKING - Link to the active Vessel
if n_program_state == 3 and conn.krpc.current_game_scene == conn.krpc.current_game_scene.flight:
mQ.put((0, 'Connecting to the vessel....'))
try:
vessel = conn.space_center.active_vessel
mQ.put((0, 'Linked to ' + vessel.name))
n_program_state = 4
except krpc.client.RPCError:
mQ.put((1, 'Could not connect to a vessel'))
pass
# STATE = Perform CNIA
if n_program_state == 4:
mQ.put((0, 'Starting CNIA...'))
cnia(ser, conn, vessel)
mQ.put((0, 'CNIA Complete'))
n_program_state = 5
# STATE = Streams and objects- setup data input streams and reused objects
if n_program_state == 5:
# Camera object
cam = conn.space_center.camera
# Part temperatures
part_temp_list = []
for x in vessel.parts.all:
temp = [
conn.add_stream(getattr, x,
'temperature'), x.max_temperature,
conn.add_stream(getattr, x, 'skin_temperature'),
x.max_skin_temperature
]
part_temp_list.append(temp)
# Engine propellant status
engine_propellant_status = []
for x in [
item for sublist in
[p.propellants for p in vessel.parts.engines]
for item in sublist
]:
temp = [
conn.add_stream(getattr, x,
'total_resource_available'),
conn.add_stream(getattr, x, 'total_resource_capacity')
]
engine_propellant_status.append(temp)
# Monoprop and electricity status
resources = vessel.resources_in_decouple_stage(
vessel.control.current_stage)
mono_status = [
conn.add_stream(resources.amount, 'MonoPropellant'),
conn.add_stream(resources.max, 'MonoPropellant')
]
elec_status = [
conn.add_stream(resources.amount, 'ElectricCharge'),
conn.add_stream(resources.max, 'ElectricCharge')
]
# Gear Status
gear_status = []
temp = []
for x in vessel.parts.landing_gear:
temp.append(conn.add_stream(getattr, x, 'state'))
gear_status.append(temp)
temp = []
for x in vessel.parts.landing_legs:
temp.append(conn.add_stream(getattr, x, 'state'))
gear_status.append(temp)
# Vessel and body list for map mode focus switching
sorted_bodies = sorted(conn.space_center.bodies.items(),
key=operator.itemgetter(1))
map_view_list = [(vessel.name, vessel)]
map_view_list.extend(sorted_bodies)
mQ.put((0, 'Stream setup complete'))
n_program_state = 6
# STATE = RUNNING
if n_program_state == 6:
try: # catch RPC errors as they generally result from a scene change. Make more specific KRPC issue 256
# Send data to the arduino request it to process inputs - command byte = 0x00
BA_output_buffer = bytearray([0x00, 0x00, 0x00])
ser.write(BA_output_buffer)
# Now while the Arduino is busy with inputs we processes the outputs - comamand byte = 0x01
BA_output_buffer = bytearray([0x01, 0x00, 0x00])
output_mapping(BA_output_buffer, conn, part_temp_list,
engine_propellant_status, mono_status,
elec_status, gear_status)
# Make sure the Arduino has responded
while ser.in_waiting != 40:
pass
# read back the data from the arduino
BA_input_buffer_prev = BA_input_buffer
BA_input_buffer = ser.read(40)
# Now send the output date we calculated earlier
ser.write(BA_output_buffer)
if f_first_pass: # On the first pass copy the data in to avoid an error.
BA_input_buffer_prev = BA_input_buffer
f_first_pass = 0
# Check the status of the Arduino
if BA_input_buffer[
0] == 3: # status of 00000011 is fully powered
# Action Groups
for i in range(0, 10):
if is_set(BA_input_buffer[int(i / 8) + 6],
i % 8) and not is_set(
BA_input_buffer_prev[int(i / 8) + 6],
i % 8):
vessel.control.toggle_action_group(
(i + 1) % 10)
if is_set(BA_input_buffer[11], 7) and not is_set(
BA_input_buffer_prev[11], 7
): # todo - Remove this when mux 0 pin A3 is resolved
vessel.control.toggle_action_group(3)
# Staging
if is_set(BA_input_buffer[7], 7) and not is_set(
BA_input_buffer_prev[7], 7) and is_set(
BA_input_buffer[9], 7):
vessel.control.activate_next_stage()
n_program_state = 5 # trigger a stream refresh!
# todo do we need this for decouple?
# Systems
if is_set(BA_input_buffer[11], 1) != is_set(
BA_input_buffer_prev[11], 1):
vessel.control.lights = is_set(
BA_input_buffer[11], 1)
if is_set(BA_input_buffer[11], 2) != is_set(
BA_input_buffer_prev[11], 2):
vessel.control.rcs = is_set(BA_input_buffer[11], 2)
if is_set(BA_input_buffer[11], 4) != is_set(
BA_input_buffer_prev[11], 4):
vessel.control.gear = not is_set(
BA_input_buffer[11],
4) # gear is opposite sense
if is_set(BA_input_buffer[11], 5) != is_set(
BA_input_buffer_prev[11], 5) or (is_set(
BA_input_buffer[11], 6) != is_set(
BA_input_buffer_prev[11], 6)):
vessel.control.brakes = is_set(
BA_input_buffer[11], 5) or is_set(
BA_input_buffer[11], 6)
# Navball Mode
if is_set(BA_input_buffer[12], 1):
vessel.control.speed_mode = vessel.control.speed_mode.target
if is_set(BA_input_buffer[12], 2):
vessel.control.speed_mode = vessel.control.speed_mode.orbit
if is_set(BA_input_buffer[12], 3):
vessel.control.speed_mode = vessel.control.speed_mode.surface
# Landing Guidance
if is_set(BA_input_buffer[12], 4) and not is_set(
BA_input_buffer_prev[12], 4):
ldg_guidance_draw(conn, vessel)
elif not is_set(BA_input_buffer[12], 4) and is_set(
BA_input_buffer_prev[12], 4):
ldg_guidance_clear(conn)
# Autopiliot
spd_err_p = spd_err
throttle_inhib, spd_err = Autopilot(
BA_input_buffer, BA_input_buffer_prev, vessel,
spd_err_p, 100)
# Flight Control and Trims
sas_overide = flight_control_inputs(
BA_input_buffer, vessel, x_trim, throttle_inhib)
# SAS
SAS_inputs(BA_input_buffer, BA_input_buffer_prev,
vessel, mQ, sas_overide)
# Save/Load
if is_set(BA_input_buffer[1], 0) and not is_set(
BA_input_buffer_prev[1], 0):
conn.space_center.quicksave()
conn.ui.message('Quicksaving...', duration=5)
if is_set(BA_input_buffer[1], 1) and not is_set(
BA_input_buffer_prev[1], 1):
t_quickload_timer = time.time() + 5
conn.ui.message(
'Hold for 5 seconds to Quickload...',
duration=5)
if not is_set(BA_input_buffer[1], 1):
t_quickload_timer = 0
if time.time() >= t_quickload_timer > 0:
conn.space_center.quickload()
# Warp
if is_set(BA_input_buffer[4], 7):
conn.space_center.physics_warp_factor = 0
conn.space_center.rails_warp_factor = 0
elif is_set(BA_input_buffer[5], 1) and not is_set(
BA_input_buffer_prev[5], 1
) and conn.space_center.physics_warp_factor == 0:
conn.space_center.rails_warp_factor = min(
conn.space_center.rails_warp_factor + 1,
conn.space_center.maximum_rails_warp_factor)
elif is_set(BA_input_buffer[5], 3) and not is_set(
BA_input_buffer_prev[5], 3):
conn.space_center.rails_warp_factor = max(
conn.space_center.rails_warp_factor - 1, 0)
elif is_set(BA_input_buffer[5], 5) and not is_set(
BA_input_buffer_prev[5], 5
) and conn.space_center.rails_warp_factor == 0:
conn.space_center.physics_warp_factor = min(
conn.space_center.physics_warp_factor + 1, 3)
elif is_set(BA_input_buffer[5], 7) and not is_set(
BA_input_buffer_prev[5], 7):
conn.space_center.physics_warp_factor = max(
conn.space_center.physics_warp_factor - 1, 0)
# Clear Target
if is_set(BA_input_buffer[3], 1) and not is_set(
BA_input_buffer_prev[3], 1):
conn.space_center.clear_target()
# Camera Control
if Settings.G_cam_change_timer > 0:
Settings.G_cam_change_timer = max(
0, Settings.G_cam_change_timer -
Settings.c_loop_frame_rate)
camera_inputs(cam, BA_input_buffer, mQ)
# Map focus
if cam.mode == cam.mode.map:
if cam.focussed_vessel is not None:
map_idx = [x[0] for x in map_view_list
].index(cam.focussed_vessel.name)
elif cam.focussed_body is not None:
map_idx = [x[0] for x in map_view_list
].index(cam.focussed_body.name)
map_idx_new = map_idx
if is_set(BA_input_buffer[2], 2) and not is_set(
BA_input_buffer_prev[2], 2):
map_idx_new = (map_idx +
1) % len(map_view_list)
if is_set(BA_input_buffer[2], 0) and not is_set(
BA_input_buffer_prev[2], 0):
map_idx_new = (map_idx -
1) % len(map_view_list)
if map_idx_new != map_idx:
if map_idx_new == 0: # vessel
cam.focussed_vessel = vessel
cam.distance = cam.default_distance
else:
cam.focussed_body = map_view_list[
map_idx_new][1]
cam.distance = cam.default_distance
if is_set(BA_input_buffer[2], 1) and not is_set(
BA_input_buffer_prev[2],
1): # always reset to te current vessel
cam.focussed_vessel = vessel
cam.distance = cam.default_distance
# Vessel Switch
if is_set(BA_input_buffer[2], 5) and not is_set(
BA_input_buffer_prev[2], 5):
vessel_list = [
v for v in conn.space_center.vessels if
norm(v.position(vessel.reference_frame)) < 2000
]
conn.space_center.active_vessel = vessel_list[
(vessel_list.index(vessel) + 1) %
len(vessel_list)]
n_program_state = 4
elif is_set(BA_input_buffer[2], 7) and not is_set(
BA_input_buffer_prev[2], 7):
vessel_list = [
v for v in conn.space_center.vessels if
norm(v.position(vessel.reference_frame)) < 2000
]
conn.space_center.active_vessel = vessel_list[
(vessel_list.index(vessel) - 1) %
len(vessel_list)]
n_program_state = 4
# put all the data onto the shared array for use by the GUI
for i in range(len(BA_input_buffer)):
data_array[i] = BA_input_buffer[i]
except krpc.client.RPCError:
n_program_state = 3
mQ.put((1, 'Main Loop Error'))
# Check for Overuns and send a warning.
if (time.time() -
t_frame_start_time) > Settings.c_loop_frame_rate * 1.1:
mQ.put(
(1, 'OVERUN - ' +
str(int(
(time.time() - t_frame_start_time) * 1000)) + 'ms'))
def ProcessFrame(frame):
import time
#time.sleep(.1)
out = obj([False, 0, 0, 0, 0, 0, 0])
frameOut = np.copy(frame)
frame = norm(frame)
m = cv2.mean(frame)
red = dist(frame, [m[0], m[1], 255])
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col) / 3
minArea = 200
red = cv2.medianBlur(red, ksize=3)
ret, thresh = cv2.threshold(red,
np.mean(red) - max(np.std(red), 10) * 2.65,
255, cv2.THRESH_BINARY_INV)
debugFrame("threshOg", thresh)
kernel = np.ones((7, 7), np.uint8)
thresh = cv2.erode(thresh, kernel, iterations=2)
debugFrame("thresh", thresh)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], mean[2]))
mean, std = cv2.meanStdDev(r)
r = cv2.GaussianBlur(r, (9, 9), 0)
for i in range(len(contours)):
epsilon = 0.045 * cv2.arcLength(contours[i], True)
approx = cv2.approxPolyDP(contours[i], epsilon, True)
#check if shape triangle
cv2.fillConvexPoly(frameOut, approx, (255, 0, 0))
foundThreeOrFour = False
""""if the approximated contour is a triangle"""
if len(approx) == 3:
#foundThreeOrFour = True
"""Finding the point opposite of the hypotenuse"""
pts = []
for ptWrapper in approx:
pts.append(ptWrapper[0])
#find hypotenuse
maxLen = 0
maxIdx = 0
for j in range(3):
#print j
p1 = pts[j]
#print (j+1) %3
p2 = pts[(j + 1) % 3]
sideLen = math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
if sideLen > maxLen:
maxLen = sideLen
maxIdx = j
p1 = pts[maxIdx]
p2 = pts[(maxIdx + 1) % 3]
center = pts[(maxIdx + 2) % 3]
#cv2.circle(frameOut,(center[0], center[1]) , 5, color = (0,0,0), thickness = -1)
elif len(approx) == 4:
foundThreeOrFour = True
"""Figure out closest and furthest points"""
pts = []
for ptWrapper in approx:
pts.append(ptWrapper[0])
maxDist = 0
end1 = 0
end2 = 0
for i in range(0, len(pts)):
for j in range(i, len(pts)):
ptdist = math.sqrt((pts[i][0] - pts[j][0])**2 +
(pts[i][1] - pts[j][1])**2)
if (ptdist >= maxDist):
maxDist = ptdist
end1 = i
end2 = j
p1, p2 = pts[end1], pts[end2]
centerPts = []
for i in range(len(pts)):
if i != end1 and i != end2:
centerPts.append(pts[i])
center = ((centerPts[0][0] + centerPts[1][0]) / 2,
(centerPts[0][1] + centerPts[1][1]) / 2)
#out = obj([True, int(center[0]), int(center[1]), int(endPts[0][0]), int(endPts[0][1]), int(endPts[1][0]), int(endPts[1][1])])
#frameOut = out.draw(frameOut)
if foundThreeOrFour:
v1 = [p1[0] - center[0], p1[1] - center[1]]
v2 = [p2[0] - center[0], p2[1] - center[1]]
dot = v1[0] * v2[0] + v1[1] * v2[1]
"""Finding magnitude of 2 shorter sides and the angle opposite of the hypotenuse"""
mag1 = math.sqrt((v1[0])**2 + (v1[1])**2)
mag2 = math.sqrt((v2[0])**2 + (v2[1])**2)
#may increment mags and arg inside acos to be not 0 or in acos domain
if mag1 == 0:
mag1 += .001
if mag2 == 0:
mag2 += .001
angle = math.acos(.999 * dot / (mag1 * mag2)) * 180.0 / math.pi
"""
checks that the biggest angle of the triangle is roughly 130 degrees and that the sides are similar sizes
130 degrees corresponds to the 45 degree bend
"""
MAX_ANG_DIF = 12
MAX_LEN_DIF = 60
MIN_LEN = 60
TARGET_ANGLE = 130
if mag1 > 100 or mag2 > 100:
print "Angle, Mag1, Mag2: ", int(angle), int(mag1), int(mag2)
#print angle
if (abs(TARGET_ANGLE - abs(angle)) < MAX_ANG_DIF
) and abs(mag1 - mag2) < MAX_LEN_DIF and mag1 > MIN_LEN:
out = obj([
True,
int(center[0]),
int(center[1]),
int(p1[0]),
int(p1[1]),
int(p2[0]),
int(p2[1])
])
frameOut = out.draw(frameOut)
debugFrame("out", frameOut)
return out
def update(self, data_array, msgQ):
# Manage notebook tab switching
if self.panel_connected:
if is_set(data_array[4], 6):
self.notebook.select(7)
elif is_set(data_array[5], 0):
self.notebook.select(6)
elif is_set(data_array[5], 2):
self.notebook.select(5)
elif is_set(data_array[5], 4):
self.notebook.select(4)
elif is_set(data_array[5], 6):
self.notebook.select(3)
elif is_set(data_array[2], 4):
self.notebook.select(2)
elif is_set(data_array[2], 6):
self.notebook.select(1)
elif is_set(data_array[3], 0):
self.notebook.select(0)
else:
self.notebook.select(8)
self.notebook_page = self.notebook.index(self.notebook.select())
# Update reused elements
OI_data = [si_val(self.orbital_speed()) + 'm/s',
si_val(self.apoapsis(), 2) + 'm',
si_val(self.periapsis(), 2) + 'm',
sec2time(self.orbital_period()),
sec2time(self.time_to_apoapsis()),
sec2time(self.time_to_periapsis()),
'{0:.2f} deg'.format(degrees(self.inclination())),
'{0:.4f}'.format(self.eccentricity()),
'{0:.2f} deg'.format(degrees(self.long_asc_node()))]
SI_data = [si_val(self.agl_altitude(), 3) + 'm',
'{0:.1f} deg'.format(self.pitch()),
'{0:.0f} deg'.format(self.heading()),
'{0:.1f} deg'.format(self.roll()),
si_val(self.speed(), 2) + 'm/s',
si_val(self.v_speed(), 2) + 'm/s',
si_val(self.h_speed(), 2) + 'm/s']
try:
twr = self.max_thrust() / (self.mass() * self.surface_gravity())
except ZeroDivisionError:
twr = 0
VI_data = [si_val(self.mass() * 1000) + 'g',
si_val(self.max_thrust(), 3) + 'N',
si_val(self.thrust(), 3) + 'N',
'{0:.2f}'.format(twr)]
# Tab 1 - Launch
if self.notebook_page == 0:
self.T1_OI.update(OI_data)
self.T1_SI.update(SI_data)
self.T1_VI.update(VI_data)
self.T1_LP.animate(self.vessel, (self.latitude(), self.longitude()), self.mean_altitude())
# Tab 2 - Orbital
if self.notebook_page == 1:
self.T2_OI.update(OI_data)
self.T2_VI.update(VI_data)
self.T2_OP.animate(self.vessel, self.periapsis(), self.eccentricity(), self.ecc_anomaly(), self.inclination(), self.arg_periapsis())
# Tab 3 - Landing
if self.notebook_page == 2:
self.T3_OI.update(OI_data)
self.T3_VI.update(VI_data)
self.T3_SI.update(SI_data)
# Tab 4 - Rondezvous
if self.notebook_page == 3:
self.T4_OI.update(OI_data)
self.T4_VI.update(VI_data)
self.T4_SI.update(SI_data)
target_vessel = self.conn.space_center.target_vessel
if target_vessel is None:
T4_TI_data = ["No Target",
'',
'',
'',
'',
'',
'',
'',
'',
'',
'',
'']
else:
target_pos = target_vessel.position(self.vessel.reference_frame)
target_vel = target_vessel.velocity(self.vessel.reference_frame)
T4_TI_data = [target_vessel.name,
'',
'',
'',
si_val(norm(target_pos), 1) + 'm',
si_val(norm(target_vel), 1) + 'm/s',
'{0:.2f}m'.format(target_pos[0]),
'{0:.2f}m'.format(target_pos[1]),
'{0:.2f}m'.format(target_pos[2]),
'{0:.2f}m/s'.format(target_vel[0]),
'{0:.2f}m/s'.format(target_vel[1]),
'{0:.2f}m/s'.format(target_vel[2])]
self.T4_TI.update(T4_TI_data)
# Tab 8 = Systems
if self.notebook_page == 7:
res_vals = []
for res in self.res_streams:
res_vals.append('{0:.0f}'.format(res()))
self.T8_TRD.update(self.res_names, res_vals)
resIDS_vals = []
for res in self.resIDS_streams:
resIDS_vals.append('{0:.0f}'.format(res()))
self.T8_SRD.update(self.resIDS_names, resIDS_vals)
# Tab 9 - Maintanance
if self.notebook_page == 8:
temp = ['{0:08b}'.format(data_array[0]),
'{:d}ms'.format(data_array[27]),
'{0:.0f}deg'.format(data_array[18] * 0.69310345 - 68.0241379),
'{0:.0f}%'.format(data_array[28] / 255 * 100),
'{0:.0f}%'.format(data_array[19] / 255 * 100)]
self.T9_SD.update(temp)
# Tab 9 --> Left Frame --> Digital Data SubframeLabel
temp = ['{0:08b}'.format(data_array[1]),
'{0:08b}'.format(data_array[2]),
'{0:08b}'.format(data_array[3]),
'{0:08b}'.format(data_array[4]),
'{0:08b}'.format(data_array[5]),
'{0:08b}'.format(data_array[6]),
'{0:08b}'.format(data_array[7]),
'{0:08b}'.format(data_array[8]),
'{0:08b}'.format(data_array[9]),
'{0:08b}'.format(data_array[10]),
'{0:08b}'.format(data_array[33]),
'{0:08b}'.format(data_array[34]),
'{0:08b}'.format(data_array[35]),
'{0:08b}'.format(data_array[36]),
'{0:08b}'.format(data_array[37]),
'{0:08b}'.format(data_array[38])
]
self.T9_DI.update(temp)
# Tab 9 --> Left Frame --> Analogure Data SubframeLabel
temp = [data_array[20],
data_array[21],
data_array[22],
data_array[23],
data_array[24],
data_array[25],
data_array[26]]
self.T9_AI.update(temp)
# Tab 9 --> Left Frame --> Analogure Data SubframeLabel
temp = ["ON" if is_set(data_array[29], 0) else "OFF",
"RUNNING" if is_set(data_array[29], 1) else "INITIATING",
"CONNECTED" if is_set(data_array[29], 2) else "DISCON",
data_array[30],
bytes2int([data_array[33], data_array[34]]),
"CONNECTED" if is_set(data_array[29], 3) else "DISCON",
data_array[31],
bytes2int([data_array[35], data_array[36]]),
"CONNECTED" if is_set(data_array[29], 4) else "DISCON",
data_array[32],
bytes2int([data_array[37], data_array[38]])]
self.T9_AP.update(temp)
# Update message area
if not msgQ.empty():
m = msgQ.get()
self.msgL1.set(self.msgL2.get())
self.msgL2.set(self.msgL3.get())
self.msgL3.set(self.msgL4.get())
self.msgL4.set(msg_prefix[m[0]] + ': ' + m[1])
def update(self, data_array, msgQ):
# Manage notebook tab switching
if self.panel_connected:
if is_set(data_array[4], 6):
self.notebook.select(7)
elif is_set(data_array[5], 0):
self.notebook.select(6)
elif is_set(data_array[5], 2):
self.notebook.select(5)
elif is_set(data_array[5], 4):
self.notebook.select(4)
elif is_set(data_array[5], 6):
self.notebook.select(3)
elif is_set(data_array[2], 4):
self.notebook.select(2)
elif is_set(data_array[2], 6):
self.notebook.select(1)
elif is_set(data_array[3], 0):
self.notebook.select(0)
else:
self.notebook.select(8)
self.notebook_page = self.notebook.index(self.notebook.select())
# Update reused elements
OI_data = [
si_val(self.orbital_speed()) + 'm/s',
si_val(self.apoapsis(), 2) + 'm',
si_val(self.periapsis(), 2) + 'm',
sec2time(self.orbital_period()),
sec2time(self.time_to_apoapsis()),
sec2time(self.time_to_periapsis()),
'{0:.2f} deg'.format(degrees(self.inclination())),
'{0:.4f}'.format(self.eccentricity()),
'{0:.2f} deg'.format(degrees(self.long_asc_node()))
]
SI_data = [
si_val(self.agl_altitude(), 3) + 'm',
'{0:.1f} deg'.format(self.pitch()),
'{0:.0f} deg'.format(self.heading()),
'{0:.1f} deg'.format(self.roll()),
si_val(self.speed(), 2) + 'm/s',
si_val(self.v_speed(), 2) + 'm/s',
si_val(self.h_speed(), 2) + 'm/s'
]
try:
twr = self.max_thrust() / (self.mass() * self.surface_gravity())
except ZeroDivisionError:
twr = 0
VI_data = [
si_val(self.mass() * 1000) + 'g',
si_val(self.max_thrust(), 3) + 'N',
si_val(self.thrust(), 3) + 'N', '{0:.2f}'.format(twr)
]
# Tab 1 - Launch
if self.notebook_page == 0:
self.T1_OI.update(OI_data)
self.T1_SI.update(SI_data)
self.T1_VI.update(VI_data)
self.T1_LP.animate(self.vessel,
(self.latitude(), self.longitude()),
self.mean_altitude())
# Tab 2 - Orbital
if self.notebook_page == 1:
self.T2_OI.update(OI_data)
self.T2_VI.update(VI_data)
self.T2_OP.animate(self.vessel, self.periapsis(),
self.eccentricity(), self.ecc_anomaly(),
self.inclination(), self.arg_periapsis())
# Tab 3 - Landing
if self.notebook_page == 2:
self.T3_OI.update(OI_data)
self.T3_VI.update(VI_data)
self.T3_SI.update(SI_data)
# Tab 4 - Rondezvous
if self.notebook_page == 3:
self.T4_OI.update(OI_data)
self.T4_VI.update(VI_data)
self.T4_SI.update(SI_data)
target_vessel = self.conn.space_center.target_vessel
if target_vessel is None:
T4_TI_data = [
"No Target", '', '', '', '', '', '', '', '', '', '', ''
]
else:
target_pos = target_vessel.position(
self.vessel.reference_frame)
target_vel = target_vessel.velocity(
self.vessel.reference_frame)
T4_TI_data = [
target_vessel.name, '', '', '',
si_val(norm(target_pos), 1) + 'm',
si_val(norm(target_vel), 1) + 'm/s', '{0:.2f}m'.format(
target_pos[0]), '{0:.2f}m'.format(target_pos[1]),
'{0:.2f}m'.format(target_pos[2]),
'{0:.2f}m/s'.format(target_vel[0]),
'{0:.2f}m/s'.format(target_vel[1]),
'{0:.2f}m/s'.format(target_vel[2])
]
self.T4_TI.update(T4_TI_data)
# Tab 8 = Systems
if self.notebook_page == 7:
res_vals = []
for res in self.res_streams:
res_vals.append('{0:.0f}'.format(res()))
self.T8_TRD.update(self.res_names, res_vals)
resIDS_vals = []
for res in self.resIDS_streams:
resIDS_vals.append('{0:.0f}'.format(res()))
self.T8_SRD.update(self.resIDS_names, resIDS_vals)
# Tab 9 - Maintanance
if self.notebook_page == 8:
temp = [
'{0:08b}'.format(data_array[0]),
'{:d}ms'.format(data_array[27]),
'{0:.0f}deg'.format(data_array[18] * 0.69310345 - 68.0241379),
'{0:.0f}%'.format(data_array[28] / 255 * 100),
'{0:.0f}%'.format(data_array[19] / 255 * 100)
]
self.T9_SD.update(temp)
# Tab 9 --> Left Frame --> Digital Data SubframeLabel
temp = [
'{0:08b}'.format(data_array[1]), '{0:08b}'.format(
data_array[2]), '{0:08b}'.format(data_array[3]),
'{0:08b}'.format(data_array[4]), '{0:08b}'.format(
data_array[5]), '{0:08b}'.format(data_array[6]),
'{0:08b}'.format(data_array[7]), '{0:08b}'.format(
data_array[8]), '{0:08b}'.format(data_array[9]),
'{0:08b}'.format(data_array[10]), '{0:08b}'.format(
data_array[33]), '{0:08b}'.format(data_array[34]),
'{0:08b}'.format(data_array[35]), '{0:08b}'.format(
data_array[36]), '{0:08b}'.format(data_array[37]),
'{0:08b}'.format(data_array[38])
]
self.T9_DI.update(temp)
# Tab 9 --> Left Frame --> Analogure Data SubframeLabel
temp = [
data_array[20], data_array[21], data_array[22], data_array[23],
data_array[24], data_array[25], data_array[26]
]
self.T9_AI.update(temp)
# Tab 9 --> Left Frame --> Analogure Data SubframeLabel
temp = [
"ON" if is_set(data_array[29], 0) else "OFF",
"RUNNING" if is_set(data_array[29], 1) else "INITIATING",
"CONNECTED" if is_set(data_array[29], 2) else "DISCON",
data_array[30],
bytes2int([data_array[33], data_array[34]]),
"CONNECTED" if is_set(data_array[29], 3) else "DISCON",
data_array[31],
bytes2int([data_array[35], data_array[36]]),
"CONNECTED" if is_set(data_array[29], 4) else "DISCON",
data_array[32],
bytes2int([data_array[37], data_array[38]])
]
self.T9_AP.update(temp)
# Update message area
if not msgQ.empty():
m = msgQ.get()
self.msgL1.set(self.msgL2.get())
self.msgL2.set(self.msgL3.get())
self.msgL3.set(self.msgL4.get())
self.msgL4.set(msg_prefix[m[0]] + ': ' + m[1])
def ProcessFrame(frame):
out = obj([False, 0, 0])
frameOut = np.copy(frame)
frame = norm(frame)
m = cv2.mean(frame)
red = dist(frame, [m[0], m[1], max(m[2] + 50, 255)])
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = 30000
minArea = 1000
ret, thresh = cv2.threshold(red,
np.mean(red) - np.std(red) * 1.5, 255,
cv2.THRESH_BINARY_INV)
kernel = np.ones((5, 5), np.uint8)
kernel[:, 0:1] = 0
kernel[:, 3:5] = 0
debugFrame("threshOg", thresh)
thresh = cv2.erode(thresh, kernel, iterations=1)
thresh = cv2.dilate(thresh, kernel, iterations=2)
debugFrame("thresh", thresh)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
possiblePoles = []
for cont in contours:
#cv2.drawContours(frame, [cont], -1, (0, 0, 255), 3)
(center, (w, h), ang) = cv2.minAreaRect(cont)
area = w * h
if w > 0:
aspectRatio = h / w
if aspectRatio < 1 and aspectRatio != 0:
aspectRatio = 1 / aspectRatio
ang += 90
else:
aspectRation = 20
angp = abs(ang)
angp = abs(angp - round((angp / 180.0)) * 180)
stringOut = "%d %d %d" % (angp, area, aspectRatio)
cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])),
cv2.FONT_HERSHEY_SIMPLEX, .5, 255)
if (
angp < 10 or angp > 170
) and area > minArea and area < maxArea and abs(aspectRatio - 9) <= 4:
possiblePoles.append((cont, center))
possiblePoles = sorted(possiblePoles, key=sortPoles)
for pole in possiblePoles:
cv2.drawContours(frameOut, [pole[0]], -1, (0, 0, 255), 3)
(center, (w, h), ang) = cv2.minAreaRect(pole[0])
aspectRatio = h / w
if aspectRatio < 1:
ang += 90
angp = abs(ang)
angp = abs(angp - round((angp / 180)) * 180)
area = h * w
stringOut = "%d %d %d" % (angp, area, aspectRatio)
cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])),
cv2.FONT_HERSHEY_SIMPLEX, 1, 255)
if len(possiblePoles) > 1:
prevPole = possiblePoles[0]
poles = [prevPole]
for pole in possiblePoles:
cv2.drawContours(frame, [pole[0]], -1, (0, 0, 255), 3)
diff = abs(pole[1][0] - prevPole[1][0])
if (diff > 20):
poles.append(pole)
prevPole = pole
if (len(poles) == 2):
left = poles[0]
right = poles[1]
out = obj([True, int(right[1][0]), int(left[1][0])])
frameOut = out.draw(frameOut)
cv2.drawContours(frameOut, [left[0], right[0]], -1, (0, 0, 0), 3)
debugFrame("out", frameOut)
return out
def panel_control(data_array, mQ):
n_program_state = 1
t_quickload_timer = 0
t_frame_start_time = time.time()
BA_input_buffer = bytearray()
f_first_pass = 1
x_trim = [0, 0, 0]
throttle_inhib = False
spd_err = 0
spd_err_p = 0
while 1:
if time.time() - t_frame_start_time >= Settings.c_loop_frame_rate:
# record the start of the processing so we can get timing data
t_frame_start_time = time.time()
# STATE = PANEL CONN - Connect to the panel
if n_program_state == 1:
mQ.put((0, 'Connecting to the panel....'))
try:
ser = serial.Serial('COM3', 115200, timeout=0.1)
mQ.put((0, 'Connected to the panel'))
time.sleep(1) # serial needs a little bit of time to initialise, otherwise later code - esp CNIA fails
n_program_state = 2
except serial.serialutil.SerialException:
mQ.put((1, 'Could not connect to the panel'))
time.sleep(5) # to avoid spamming the message queue
pass
# STATE = GAME CONN - Connect to the KRPC Server
if n_program_state == 2:
mQ.put((0, 'Connecting to the game server....'))
try:
conn = krpc.connect(name='Game Controller')
mQ.put((0, 'Connected to the game server'))
n_program_state = 3
except ConnectionRefusedError:
mQ.put((1, 'Could not connect to the game server'))
pass
# STATE = LINKING - Link to the active Vessel
if n_program_state == 3 and conn.krpc.current_game_scene == conn.krpc.current_game_scene.flight:
mQ.put((0, 'Connecting to the vessel....'))
try:
vessel = conn.space_center.active_vessel
mQ.put((0, 'Linked to ' + vessel.name))
n_program_state = 4
except krpc.client.RPCError:
mQ.put((1, 'Could not connect to a vessel'))
pass
# STATE = Perform CNIA
if n_program_state == 4:
mQ.put((0, 'Starting CNIA...'))
cnia(ser, conn, vessel)
mQ.put((0, 'CNIA Complete'))
n_program_state = 5
# STATE = Streams and objects- setup data input streams and reused objects
if n_program_state == 5:
# Camera object
cam = conn.space_center.camera
# Part temperatures
part_temp_list = []
for x in vessel.parts.all:
temp = [conn.add_stream(getattr, x, 'temperature'), x.max_temperature, conn.add_stream(getattr, x, 'skin_temperature'), x.max_skin_temperature]
part_temp_list.append(temp)
# Engine propellant status
engine_propellant_status = []
for x in [item for sublist in [p.propellants for p in vessel.parts.engines] for item in sublist]:
temp = [conn.add_stream(getattr, x, 'total_resource_available'), conn.add_stream(getattr, x, 'total_resource_capacity')]
engine_propellant_status.append(temp)
# Monoprop and electricity status
resources = vessel.resources_in_decouple_stage(vessel.control.current_stage)
mono_status = [conn.add_stream(resources.amount, 'MonoPropellant'), conn.add_stream(resources.max, 'MonoPropellant')]
elec_status = [conn.add_stream(resources.amount, 'ElectricCharge'), conn.add_stream(resources.max, 'ElectricCharge')]
# Gear Status
gear_status = []
temp = []
for x in vessel.parts.landing_gear:
temp.append(conn.add_stream(getattr, x, 'state'))
gear_status.append(temp)
temp = []
for x in vessel.parts.landing_legs:
temp.append(conn.add_stream(getattr, x, 'state'))
gear_status.append(temp)
# Vessel and body list for map mode focus switching
sorted_bodies = sorted(conn.space_center.bodies.items(), key=operator.itemgetter(1))
map_view_list = [(vessel.name, vessel)]
map_view_list.extend(sorted_bodies)
mQ.put((0, 'Stream setup complete'))
n_program_state = 6
# STATE = RUNNING
if n_program_state == 6:
try: # catch RPC errors as they generally result from a scene change. Make more specific KRPC issue 256
# Send data to the arduino request it to process inputs - command byte = 0x00
BA_output_buffer = bytearray([0x00, 0x00, 0x00])
ser.write(BA_output_buffer)
# Now while the Arduino is busy with inputs we processes the outputs - comamand byte = 0x01
BA_output_buffer = bytearray([0x01, 0x00, 0x00])
output_mapping(BA_output_buffer, conn, part_temp_list, engine_propellant_status, mono_status, elec_status, gear_status)
# Make sure the Arduino has responded
while ser.in_waiting != 40:
pass
# read back the data from the arduino
BA_input_buffer_prev = BA_input_buffer
BA_input_buffer = ser.read(40)
# Now send the output date we calculated earlier
ser.write(BA_output_buffer)
if f_first_pass: # On the first pass copy the data in to avoid an error.
BA_input_buffer_prev = BA_input_buffer
f_first_pass = 0
# Check the status of the Arduino
if BA_input_buffer[0] == 3: # status of 00000011 is fully powered
# Action Groups
for i in range(0, 10):
if is_set(BA_input_buffer[int(i / 8) + 6], i % 8) and not is_set(BA_input_buffer_prev[int(i / 8) + 6], i % 8):
vessel.control.toggle_action_group((i + 1) % 10)
if is_set(BA_input_buffer[11], 7) and not is_set(BA_input_buffer_prev[11], 7): # todo - Remove this when mux 0 pin A3 is resolved
vessel.control.toggle_action_group(3)
# Staging
if is_set(BA_input_buffer[7], 7) and not is_set(BA_input_buffer_prev[7], 7) and is_set(BA_input_buffer[9], 7):
vessel.control.activate_next_stage()
n_program_state = 5 # trigger a stream refresh!
# todo do we need this for decouple?
# Systems
if is_set(BA_input_buffer[11], 1) != is_set(BA_input_buffer_prev[11], 1):
vessel.control.lights = is_set(BA_input_buffer[11], 1)
if is_set(BA_input_buffer[11], 2) != is_set(BA_input_buffer_prev[11], 2):
vessel.control.rcs = is_set(BA_input_buffer[11], 2)
if is_set(BA_input_buffer[11], 4) != is_set(BA_input_buffer_prev[11], 4):
vessel.control.gear = not is_set(BA_input_buffer[11], 4) # gear is opposite sense
if is_set(BA_input_buffer[11], 5) != is_set(BA_input_buffer_prev[11], 5) or (is_set(BA_input_buffer[11], 6) != is_set(BA_input_buffer_prev[11], 6)):
vessel.control.brakes = is_set(BA_input_buffer[11], 5) or is_set(BA_input_buffer[11], 6)
# Navball Mode
if is_set(BA_input_buffer[12], 1):
vessel.control.speed_mode = vessel.control.speed_mode.target
if is_set(BA_input_buffer[12], 2):
vessel.control.speed_mode = vessel.control.speed_mode.orbit
if is_set(BA_input_buffer[12], 3):
vessel.control.speed_mode = vessel.control.speed_mode.surface
# Landing Guidance
if is_set(BA_input_buffer[12], 4) and not is_set(BA_input_buffer_prev[12], 4):
ldg_guidance_draw(conn, vessel)
elif not is_set(BA_input_buffer[12], 4) and is_set(BA_input_buffer_prev[12], 4):
ldg_guidance_clear(conn)
# Autopiliot
spd_err_p = spd_err
throttle_inhib, spd_err = Autopilot(BA_input_buffer, BA_input_buffer_prev, vessel, spd_err_p, 100)
# Flight Control and Trims
sas_overide = flight_control_inputs(BA_input_buffer, vessel, x_trim, throttle_inhib)
# SAS
SAS_inputs(BA_input_buffer, BA_input_buffer_prev, vessel, mQ, sas_overide)
# Save/Load
if is_set(BA_input_buffer[1], 0) and not is_set(BA_input_buffer_prev[1], 0):
conn.space_center.quicksave()
conn.ui.message('Quicksaving...', duration=5)
if is_set(BA_input_buffer[1], 1) and not is_set(BA_input_buffer_prev[1], 1):
t_quickload_timer = time.time() + 5
conn.ui.message('Hold for 5 seconds to Quickload...', duration=5)
if not is_set(BA_input_buffer[1], 1):
t_quickload_timer = 0
if time.time() >= t_quickload_timer > 0:
conn.space_center.quickload()
# Warp
if is_set(BA_input_buffer[4], 7):
conn.space_center.physics_warp_factor = 0
conn.space_center.rails_warp_factor = 0
elif is_set(BA_input_buffer[5], 1) and not is_set(BA_input_buffer_prev[5], 1) and conn.space_center.physics_warp_factor == 0:
conn.space_center.rails_warp_factor = min(conn.space_center.rails_warp_factor + 1, conn.space_center.maximum_rails_warp_factor)
elif is_set(BA_input_buffer[5], 3) and not is_set(BA_input_buffer_prev[5], 3):
conn.space_center.rails_warp_factor = max(conn.space_center.rails_warp_factor - 1, 0)
elif is_set(BA_input_buffer[5], 5) and not is_set(BA_input_buffer_prev[5], 5) and conn.space_center.rails_warp_factor == 0:
conn.space_center.physics_warp_factor = min(conn.space_center.physics_warp_factor + 1, 3)
elif is_set(BA_input_buffer[5], 7) and not is_set(BA_input_buffer_prev[5], 7):
conn.space_center.physics_warp_factor = max(conn.space_center.physics_warp_factor - 1, 0)
# Clear Target
if is_set(BA_input_buffer[3], 1) and not is_set(BA_input_buffer_prev[3], 1):
conn.space_center.clear_target()
# Camera Control
if Settings.G_cam_change_timer > 0:
Settings.G_cam_change_timer = max(0, Settings.G_cam_change_timer - Settings.c_loop_frame_rate)
camera_inputs(cam, BA_input_buffer, mQ)
# Map focus
if cam.mode == cam.mode.map:
if cam.focussed_vessel is not None:
map_idx = [x[0] for x in map_view_list].index(cam.focussed_vessel.name)
elif cam.focussed_body is not None:
map_idx = [x[0] for x in map_view_list].index(cam.focussed_body.name)
map_idx_new = map_idx
if is_set(BA_input_buffer[2], 2) and not is_set(BA_input_buffer_prev[2], 2):
map_idx_new = (map_idx + 1) % len(map_view_list)
if is_set(BA_input_buffer[2], 0) and not is_set(BA_input_buffer_prev[2], 0):
map_idx_new = (map_idx - 1) % len(map_view_list)
if map_idx_new != map_idx:
if map_idx_new == 0: # vessel
cam.focussed_vessel = vessel
cam.distance = cam.default_distance
else:
cam.focussed_body = map_view_list[map_idx_new][1]
cam.distance = cam.default_distance
if is_set(BA_input_buffer[2], 1) and not is_set(BA_input_buffer_prev[2], 1): # always reset to te current vessel
cam.focussed_vessel = vessel
cam.distance = cam.default_distance
# Vessel Switch
if is_set(BA_input_buffer[2], 5) and not is_set(BA_input_buffer_prev[2], 5):
vessel_list = [v for v in conn.space_center.vessels if norm(v.position(vessel.reference_frame)) < 2000]
conn.space_center.active_vessel = vessel_list[(vessel_list.index(vessel) + 1) % len(vessel_list)]
n_program_state = 4
elif is_set(BA_input_buffer[2], 7) and not is_set(BA_input_buffer_prev[2], 7):
vessel_list = [v for v in conn.space_center.vessels if norm(v.position(vessel.reference_frame)) < 2000]
conn.space_center.active_vessel = vessel_list[(vessel_list.index(vessel) - 1) % len(vessel_list)]
n_program_state = 4
# put all the data onto the shared array for use by the GUI
for i in range(len(BA_input_buffer)):
data_array[i] = BA_input_buffer[i]
except krpc.client.RPCError:
n_program_state = 3
mQ.put((1, 'Main Loop Error'))
# Check for Overuns and send a warning.
if (time.time() - t_frame_start_time) > Settings.c_loop_frame_rate * 1.1:
mQ.put((1, 'OVERUN - ' + str(int((time.time() - t_frame_start_time) * 1000)) + 'ms'))
def ProcessFrame(frame):
import time
#time.sleep(.1)
out = obj([False, 0, 0, 0, 0, 0, 0])
frameOut = np.copy(frame)
frame = norm(frame)
m = cv2.mean(frame)
red = dist(frame, [m[0], m[1], 255])
debugFrame("red", red)
(row, col, pages) = frame.shape
maxArea = (row * col)/3
minArea = 200
red = cv2.medianBlur(red, ksize = 3)
ret, thresh = cv2.threshold(red, np.mean(red) - max(np.std(red), 10) * 2.65, 255, cv2.THRESH_BINARY_INV)
debugFrame("threshOg", thresh)
kernel = np.ones((7,7),np.uint8)
thresh = cv2.erode(thresh,kernel, iterations = 2)
debugFrame("thresh", thresh)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
mean, std = cv2.meanStdDev(frame)
r = dist(frame, (mean[0], mean[1], mean[2]))
mean, std = cv2.meanStdDev(r)
r = cv2.GaussianBlur(r, (9, 9), 0)
for i in range(len(contours)):
epsilon = 0.045*cv2.arcLength(contours[i],True)
approx = cv2.approxPolyDP(contours[i],epsilon,True)
#check if shape triangle
cv2.fillConvexPoly(frameOut, approx, (255,0,0))
foundThreeOrFour = False
""""if the approximated contour is a triangle"""
if len(approx) == 3:
#foundThreeOrFour = True
"""Finding the point opposite of the hypotenuse"""
pts = []
for ptWrapper in approx:
pts.append(ptWrapper[0])
#find hypotenuse
maxLen = 0
maxIdx = 0
for j in range(3):
#print j
p1 = pts[j]
#print (j+1) %3
p2 = pts[(j + 1)%3]
sideLen = math.sqrt( (p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2 )
if sideLen > maxLen:
maxLen = sideLen
maxIdx = j
p1 = pts[maxIdx]
p2 = pts[(maxIdx + 1) % 3]
center = pts[(maxIdx + 2) % 3]
#cv2.circle(frameOut,(center[0], center[1]) , 5, color = (0,0,0), thickness = -1)
elif len(approx) == 4:
foundThreeOrFour = True
"""Figure out closest and furthest points"""
pts = []
for ptWrapper in approx:
pts.append(ptWrapper[0])
maxDist = 0
end1 = 0
end2 = 0
for i in range(0,len(pts)):
for j in range(i,len(pts)):
ptdist = math.sqrt( (pts[i][0] - pts[j][0]) ** 2 + (pts[i][1] - pts[j][1]) ** 2 )
if(ptdist >= maxDist):
maxDist = ptdist
end1 = i
end2 = j
p1, p2 = pts[end1], pts[end2]
centerPts = []
for i in range(len(pts)):
if i != end1 and i != end2:
centerPts.append(pts[i])
center = ( (centerPts[0][0] + centerPts[1][0]) / 2, (centerPts[0][1] + centerPts[1][1]) / 2 )
#out = obj([True, int(center[0]), int(center[1]), int(endPts[0][0]), int(endPts[0][1]), int(endPts[1][0]), int(endPts[1][1])])
#frameOut = out.draw(frameOut)
if foundThreeOrFour:
v1 = [p1[0] - center[0], p1[1] - center[1]]
v2 = [p2[0] - center[0], p2[1] - center[1]]
dot = v1[0] * v2[0] + v1[1] * v2[1]
"""Finding magnitude of 2 shorter sides and the angle opposite of the hypotenuse"""
mag1 = math.sqrt( (v1[0]) ** 2 + (v1[1]) ** 2 )
mag2 = math.sqrt( (v2[0]) ** 2 + (v2[1]) ** 2 )
#may increment mags and arg inside acos to be not 0 or in acos domain
if mag1 == 0:
mag1 += .001
if mag2 == 0:
mag2 += .001
angle = math.acos(.999 * dot/(mag1 * mag2)) * 180.0 / math.pi
"""
checks that the biggest angle of the triangle is roughly 130 degrees and that the sides are similar sizes
130 degrees corresponds to the 45 degree bend
"""
MAX_ANG_DIF = 12
MAX_LEN_DIF = 60
MIN_LEN = 60
TARGET_ANGLE = 130
if mag1 > 100 or mag2 > 100:
print "Angle, Mag1, Mag2: ", int(angle), int(mag1), int(mag2)
#print angle
if( abs(TARGET_ANGLE - abs(angle)) < MAX_ANG_DIF ) and abs(mag1 - mag2) < MAX_LEN_DIF and mag1 > MIN_LEN:
out = obj([True, int(center[0]), int(center[1]), int(p1[0]), int(p1[1]), int(p2[0]), int(p2[1])])
frameOut = out.draw(frameOut)
debugFrame("out", frameOut)
return out
