def main():
connectThrottle = RealtimeInterval(10)
host = "roboRIO-5495-FRC.local"
port = 5888
topics = (MQTT_TOPIC_SCREENSHOT)
client = mqttClient.MqttClient(host, port, topics, messageHandler)
params = CVParameterGroup("Sliders", debugMode)
# HUES: GREEEN=65/75 BLUE=110
params.addParameter("hue", 75, 179)
params.addParameter("hueWidth", 20, 25)
params.addParameter("low", 70, 255)
params.addParameter("high", 255, 255)
params.addParameter("countourSize", 50, 200)
params.addParameter("keystone", 0, 320)
camera = cameraReader = None
if testImage is None:
camera = createCamera()
cameraReader = CameraReaderAsync.CameraReaderAsync(camera)
distanceCalculatorH = distanceCalculatorV = None
if tuneDistance:
distanceCalculatorH = DistanceCalculator.TriangleSimilarityDistanceCalculator(TARGET_WIDTH)
distanceCalculatorV = DistanceCalculator.TriangleSimilarityDistanceCalculator(TARGET_HEIGHT)
else:
distanceCalculatorH = DistanceCalculator.TriangleSimilarityDistanceCalculator(TARGET_WIDTH, DistanceCalculator.PFL_H_LC3000)
distanceCalculatorV = DistanceCalculator.TriangleSimilarityDistanceCalculator(TARGET_HEIGHT, DistanceCalculator.PFL_V_LC3000)
fpsDisplay = True
fpsCounter = WeightedFramerateCounter()
fpsInterval = RealtimeInterval(5.0, False)
keyStoneBoxSource = [[0, 0], [cameraFrameWidth, 0], [cameraFrameWidth, cameraFrameHeight], [0, cameraFrameHeight]]
# The first frame we take off of the camera won't have the proper exposure setting
# We need to skip the first frame to make sure we don't process bad image data.
frameSkipped = False
while (True):
if (not client.isConnected()) and connectThrottle.hasElapsed():
try:
client.connect()
except:
None
# This code will load a test image from disk and process it instead of the camera input
#raw = cv2.imread('test.png')
#frameSkipped = True
#if raw == None or len(raw) == 0:
# print "Can't load image"
# break
if testImage is not None:
raw = testImage.copy()
elif cameraReader is not None:
raw = cameraReader.Read()
if raw != None and frameSkipped:
fpsCounter.tick()
if debugMode:
if fpsDisplay:
cv2.putText(raw, "{:.0f} fps".format(fpsCounter.getFramerate()), (cameraFrameWidth - 100, 13 + 6), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255,255,255), 1)
cv2.imshow("raw", raw)
# This will "deskew" or fix the keystone of a tilted camera.
#ptSrc = np.float32([keyStoneBoxSource])
#ptDst = np.float32([[params['keystone'], 0],\
# [cameraFrameWidth - params['keystone'], 0],\
# [cameraFrameWidth + params['keystone'], cameraFrameHeight],\
# [-params['keystone'], cameraFrameHeight]])
#matrix = cv2.getPerspectiveTransform(ptSrc, ptDst)
#transformed = cv2.warpPerspective(raw, matrix, (cameraFrameWidth, cameraFrameHeight))
#cv2.imshow("keystone", transformed)
#target = findTarget(transformed, params)
target = findTarget(raw, params)
if target == None or not target.any():
payload = { 'hasTarget': False, "fps": round(fpsCounter.getFramerate()) }
client.publish(MQTT_TOPIC_TARGETTING, json.dumps(payload))
else:
distance = None
targetBox = getTargetBoxTight(target)
# We can tell how off-axis we are by looking at the slope
# of the top off the targetBox. If we are on-center they will
# be even. If we are off axis they will be unequal.
# We are to the right of the target if the line slopes up to the right
# and the slope is positive.
offAxis = (targetBox[0][1] - targetBox[1][1]) / (cameraFrameHeight / 10.0)
measuredHeight, centerLine = getTargetHeight(targetBox)
center = (round((centerLine[0][0] + centerLine[1][0]) / 2),\
round((centerLine[0][1] + centerLine[1][1]) / 2))
horizontalOffset = center[0] - (cameraFrameWidth / 2.0)
perceivedFocalLengthH = perceivedFocalLengthV = 0.0
if tuneDistance:
perceivedFocalLengthH = distanceCalculatorH.CalculatePerceivedFocalLengthAtGivenDistance(w, TARGET_CALIBRATION_DISTANCE)
perceivedFocalLengthV = distanceCalculatorV.CalculatePerceivedFocalLengthAtGivenDistance(h, TARGET_CALIBRATION_DISTANCE)
distance = TARGET_CALIBRATION_DISTANCE
else:
# We use the height at the center of the taget to determine distance
# That way we hope it will be less sensitive to off-axis shooting angles
distance = distanceCalculatorV.CalculateDistance(measuredHeight)
distance = round(distance, 1)
horizDelta = horizontalOffset / cameraFrameWidth * 2
payload = {\
'horizDelta': horizDelta,\
'targetDistance': round(distance),\
'hasTarget': True,\
"fps": round(fpsCounter.getFramerate()),\
"offAxis": offAxis}
client.publish(MQTT_TOPIC_TARGETTING, json.dumps(payload))
if debugMode:
result = raw.copy()
# Draw the actual contours
#cv2.drawContours(result, target, -1, (255, 255, 255), 1)
# Draw the bounding area (targetBox)
cv2.drawContours(result, [np.int0(targetBox)], -1, (255, 0, 0), 1)
# Draw Convex Hull
#hull = cv2.convexHull(target)
#cv2.drawContours(result, hull, -1, (255, 0, 255), 1)
#temp = []
#for c in target:
# contour = [c][0][0]
# temp.append(contour)
# #print contour
##print temp
#top = getIndexOfTopLeftCorner(temp)
##print target[top][0]
#cv2.circle(result, (target[top][0][0], target[top][0][1]), 3, (255, 255, 255), -1)
# Draw the centerline that represent the height
cv2.line(result, (int(round(centerLine[0][0])), int(round(centerLine[0][1]))),\
(int(round(centerLine[1][0])), int(round(centerLine[1][1]))),\
(128, 0, 255), 1)
# draw the center of the object
cv2.circle(result, (int(round(center[0])), int(round(center[1]))), 4, (250, 250, 250), -1)
#cv2.putText(result, str(horizontalOffset), (x-50, y+15), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 0, 0), 1)
if tuneDistance:
cv2.putText(result, "PFL_H: {:.0f}".format(perceivedFocalLengthH), (3, 13 + 5), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255,255,255), 1)
cv2.putText(result, "PFL_V: {:.0f}".format(perceivedFocalLengthV), (3, 13 + 5 + 22), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255,255,255), 1)
else:
cv2.putText(result, "{} inches".format(distance), (3, 13 + 5), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255,255,255), 1)
if fpsDisplay:
cv2.putText(result, "{:.0f} fps".format(fpsCounter.getFramerate()), (cameraFrameWidth - 100, 13 + 6), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255,255,255), 1)
cv2.imshow("result", result)
if raw != None:
frameSkipped = True
if fpsDisplay and fpsInterval.hasElapsed():
print "{0:.1f} fps (processing)".format(fpsCounter.getFramerate())
if cameraReader is not None:
print "{0:.1f} fps (camera)".format(cameraReader.fps.getFramerate())
if debugMode:
keyPress = cv2.waitKey(1)
if keyPress != -1:
keyPress = keyPress & 0xFF
if keyPress == ord("f"):
fpsDisplay = not fpsDisplay
elif keyPress == ord("q"):
break
elif keyPress == ord("z"):
takeScreenshot()
client.disconnect()
if cameraReader is not None:
cameraReader.Stop()
if camera is not None:
camera.release()
cv2.destroyAllWindows()
x = params["hue"] + params["hueWidth"]
if x > 179:
x = 179
high = np.array([x, params["high"], params["high"]])
mask = cv2.inRange(hsv, low, high)
return mask
def findLargestContour(source):
contours, hierarchy = cv2.findContours(source, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
ordered = sorted(contours, key = cv2.contourArea, reverse = True)[:1]
return ordered[0]
params = CVParameterGroup("Sliders")
params.addParameter("hue", 75, 255)
params.addParameter("hueWidth", 4, 25)
params.addParameter("FOV", 13782, 50000)
params.addParameter("low", 90, 255)
params.addParameter("high", 255, 255)
camera = cv2.VideoCapture(0)
#No camera's exposure goes this low, but this will set it as low as possible
camera.set(cv2.cv.CV_CAP_PROP_EXPOSURE,-100)
#camera.set(cv2.cv.CV_CAP_PROP_FPS, 15)
#camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 640)
#camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 480)
fpsDisplay = False;
fpsCounter = WeightedFramerateCounter()
fpsInterval = RealtimeInterval(3.0)
hsvcomponents = cv2.split(hsv)
low = np.array([params["hue low"], params["sat low"], params["value low"]])
high = np.array([params["hue high"], params["sat high"], params["value high"]])
mask = cv2.inRange(hsv, low, high)
return mask
def findLargestContour(source):
contours, hierarchy = cv2.findContours(source, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
ordered = sorted(contours, key = cv2.contourArea, reverse = True)[:1]
return ordered[0]
params = CVParameterGroup("Sliders")
params.addParameter("hue high", 106, 255)
params.addParameter("hue low", 0, 255)
params.addParameter("sat high", 75, 255)
params.addParameter("sat low", 0, 255)
params.addParameter("value high", 255, 255)
params.addParameter("value low", 105, 255)
params.addParameter("ED Size", 4, 20)
camera = cv2.VideoCapture(0)
#camera.set(cv2.cv.CV_CAP_PROP_FPS, 15)
#camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 640)
#camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 480)
fpsDisplay = False;
fpsCounter = WeightedFramerateCounter()
## tPx = w
## distance = params["FOV"]/w
## cv2.putText(result, str(distance), (30, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0,0,0), 1)
#client.publish("5495.targetting", center[0]-320)
payload = { 'horizDelta': center[0] - 320, 'targetDistance': int(distance) }
client.publish(MQTT_TOPIC_TARGETTING, json.dumps(payload))
return result
MQTT_TOPIC_TARGETTING = "5495.targetting"
host = "roboRIO-5495-FRC.local"
port = 5888
topics = ()
client = mqttClient.MqttClient(host, port, topics, messageHandler)
params = CVParameterGroup("Sliders")
if debugMode:
params.showWindow()
params.addParameter("hue", 65, 255) # GREEN
#params.addParameter("hue", 105, 255) #BLUE
params.addParameter("hueWidth", 5, 25)
params.addParameter("FOV", 652, 50000)
params.addParameter("low", 70, 255)
params.addParameter("high", 255, 255)
camera = createCamera()
targetSize = (11.25, 44) # size, distance
distanceCalc = TriangleSimilarityDistanceCalculator(targetSize[0])
fpsDisplay = False;
x = params["hue"] + params["hueWidth"]
if x > 179:
x = 179
high = np.array([x, 255, 255])
mask = cv2.inRange(hsv, low, high)
return mask
def findLargestContour(source):
contours, hierarchy = cv2.findContours(source, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
ordered = sorted(contours, key = cv2.contourArea, reverse = True)[:1]
return ordered[0]
params = CVParameterGroup("Sliders")
params.addParameter("hue", 111, 255)
params.addParameter("hueWidth", 2, 25)
params.addParameter("FOV", 13782, 50000)
camera = cv2.VideoCapture(0)
#camera.set(cv2.cv.CV_CAP_PROP_FPS, 15)
#camera.set(cv2.cv.CV_CAP_PROP_FOCUS_AUTO, 0)
#camera.set(cv2.cv.CV_CAP_PROP_EXPOSURE_AUTO, 0)
#camera.set(cv2.cv.CV_CAP_PROP_EXPOSURE, 0)
#camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 640)
#camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 480)
fpsDisplay = False;
fpsCounter = WeightedFramerateCounter()
fpsInterval = RealtimeInterval(3.0)
