def start_camera():
inst = CameraServer.getInstance()
camera = UsbCamera('Hatch Panels', '/dev/video0')
# with open("cam.json", encoding='utf-8') as cam_config:
# camera.setConfigJson(json.dumps(cam_config.read()))
camera.setResolution(VERTICAL_RES, HORIZONTAL_RES)
camera.setFPS(FPS)
camera.setBrightness(10)
camera.setConfigJson("""
{
"fps": """ + str(FPS) + """,
"height": """ + str(HORIZONTAL_RES) + """,
"pixel format": "mjpeg",
"properties": [
{
"name": "brightness",
"value": 0
},
{
"name": "contrast",
"value": 100
},
{
"name": "saturation",
"value": 40
}
],
"width": """ + str(VERTICAL_RES) + """
}
""")
camera.setConnectionStrategy(VideoSource.ConnectionStrategy.kKeepOpen)
return inst, camera
def main():
cs = CameraServer.getInstance()
cs.enableLogging()
outputStreamTarget = cs.putVideo("targetImage", imageResolutionSend[0], imageResolutionSend[1])
outputStreamTargetBinary = cs.putVideo("targetImageBinary", imageResolutionSend[0], imageResolutionSend[1])
outputStreamBall = cs.putVideo("ballImage", imageResolutionSend[0], imageResolutionSend[1])
# outputStreamBallBinary = cs.putVideo("ballImageBinary", imageResolutionSend[0], imageResolutionSend[1])
targetImage = np.zeros(shape=(imageResolutionRasp[1], imageResolutionRasp[0], 3), dtype=np.uint8)
ballImage = np.zeros(shape=(imageResolutionRasp[1], imageResolutionRasp[0], 3), dtype=np.uint8)
availableTargetCamera = targetCameraAddress in os.listdir("/dev")
availableBallCamera = ballCameraAddress in os.listdir("/dev")
if availableTargetCamera:
cameraTarget = UsbCamera("Camera Target", "/dev/" + targetCameraAddress)
cameraTarget.setResolution(imageResolutionRasp[0], imageResolutionRasp[1])
cameraTarget.setBrightness(brightnessTarget)
cameraTarget.setExposureManual(exposureTarget)
cs.addCamera(cameraTarget)
cvSinkTarget = cs.getVideo(name="Camera Target")
cvSinkTarget.setSource(cameraTarget)
if availableBallCamera:
cameraBall = UsbCamera("Camera Ball", "/dev/" + ballCameraAddress)
cameraBall.setResolution(imageResolutionRasp[0], imageResolutionRasp[1])
cameraBall.setBrightness(brightnessBall)
cameraBall.setExposureManual(exposureBall)
cs.addCamera(cameraBall)
cvSinkBall = cs.getVideo(name="Camera Ball")
cvSinkBall.setSource(cameraBall)
counter = 0
while True:
if availableTargetCamera:
availableTargetCamera = targetCameraAddress in os.listdir("/dev")
if availableBallCamera:
availableBallCamera = ballCameraAddress in os.listdir("/dev")
counter += 1
textOutput = ""
if calibration:
getHSVBallParameters()
getHSVTargetParameters()
getCameraTargetParameters()
getCameraBallParameters()
getTargetDistanceParameters()
if availableTargetCamera:
t, targetImage = cvSinkTarget.grabFrame(targetImage)
available, targetImage, binaryTargetImage, targetXPos, targetYPos = findTarget(targetImage)
distance = calculateDistance(targetYPos)
horizontalAngle = getHorizontalAngle(targetXPos, imageResolutionRasp)
launcherAngle = getLauncherAngle(distance)
if distance > 95 and distance < 240:
cv2.rectangle(targetImage, (20, 20), (100, 100), (0, 255, 0), -1)
netTableTargetAvailable.setBoolean(available)
netTableTargetDistance.setDouble(distance)
netTableTargetHorizontalAngle.setDouble(horizontalAngle)
netTableTargetLauncherAngle.setDouble(launcherAngle)
netTableTargetXPos.setDouble(targetXPos)
netTableTargetYPos.setDouble(targetYPos)
smallerBinaryTargetImage = cv2.resize(binaryTargetImage, (imageResolutionSend[0], imageResolutionSend[1]))
smallerTargetImage = cv2.resize(targetImage, (imageResolutionSend[0], imageResolutionSend[1]))
outputStreamTargetBinary.putFrame(smallerBinaryTargetImage)
outputStreamTarget.putFrame(smallerTargetImage)
textOutput += "Distance: {} horizontalAngle: {} launcherAngle: {} targetYPos: {}\n".format(distance, horizontalAngle, launcherAngle, targetYPos)
else:
textOutput += "Target Camera disabled\n"
if availableBallCamera:
t, ballImage = cvSinkBall.grabFrame(ballImage)
# ballImage, binaryBallImage, ballXPos, ballYPos = findBall(ballImage)
# angle = getHorizontalAngle(ballXPos, imageResolutionRasp)
# netTableBallXPos.setDouble(ballXPos)
# netTableBallYPos.setDouble(ballYPos)
# netTableBallAngle.setDouble(angle)
# smallerBinaryBallImage = cv2.resize(binaryBallImage, (imageResolutionSend[0], imageResolutionSend[1]))
smallerBallImage = cv2.resize(ballImage, (imageResolutionSend[0], imageResolutionSend[1]))
# outputStreamBallBinary.putFrame(smallerBinaryBallImage)
outputStreamBall.putFrame(smallerBallImage)
# textOutput += "ballXPos: {} ballYPos: {}\n".format(ballXPos, ballYPos)
else:
textOutput += "Ball Camera disabled\n"
if counter % 10 == 0:
print(textOutput)
def mainMain():
# network tables initialization
NetworkTables.initialize(server=__IP)
print("nt initialized")
smartdash = NetworkTables.getTable("SmartDashboard")
# table = NetworkTables.getTable(__SENT_TABLE_NAME)
# subtable = smartdash.getSubTable(__ORIENTATION_TABLE_NAME)
# initialize cameras
# CAMERA 1
cam = UsbCamera('Cam 1 Front', 1)
cam.setResolution(160, 120)
cam.setExposureManual(0)
cam.setBrightness(0)
cam.setFPS(30)
print("cam1 initialized")
# CAMERA 2
cam2 = UsbCamera('Cam 2 Back', 0)
cam2.setResolution(160, 120)
cam2.setExposureManual(0)
cam2.setBrightness(0)
cam2.setFPS(30)
print("cam2 initialized")
# EACH CAMERA STARTS CAPTURING VIDEO
cs.startAutomaticCapture(camera=cam)
cs.startAutomaticCapture(camera=cam2)
# GETTING THE VIDEO STREAM OF EACH CAMERA
vid = cs.getVideo(camera=cam)
vid2 = cs.getVideo(camera=cam2)
# initialize outputstream, this is the place where we send frames to shuffleboard.
output_stream = cs.putVideo('TOP CAMERAS', 160, 120)
print("Running while loop")
counter = 0
while True:
# get front or back value
camera_value = subtable.getString("value", "FRONT")
img = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
# get video
time, frame = video.grabFrame(img, 0.5)
# This condition sends video from different cameras based on the robots orientation.
if camera_value == 'BACK':
print("BACK")
video = vid
elif camera_value == 'FRONT':
print("FRONT")
video = vid2
else:
print('error, no camera value detected')
print("sending frame")
# send frame to shuffleboard
output_stream.putFrame(frame)
print('Done.')
sys.stdout.flush()
# just counting how fast it is running and if it is running at all
counter += 1
print(counter)
