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() usb0 = UsbCamera('Camera 0', 0) usb0.setFPS(10) usb0.setResolution(176, 144) usb1 = UsbCamera('Camera 1', 1) usb1.setFPS(10) usb1.setResolution(176, 144) # We "bounce" between these two camera objects using a Command on # the robot. cams = [usb0, usb1] # Add a "Switched" camera opject to Network Tables, # and set the source to the first camera (usb0). server = cs.addSwitchedCamera('Switched') server.setSource(cams[0]) server.setFPS(10) server.setCompression(80) server.setResolution(176, 144) def _listener(source, key, value, isNew): '''Use networktables to switch the camera source. We set the camera source to the camera object in the "cams" dictionary. We use the bitwise Exclusive Or of "value" to index the array (see https://wiki.python.org/moin/BitwiseOperators for bit operators). "value" MUST be 0 (zero) or 1 (one) in order for this to work ''' # print("cams[int(value)]: value: {} cams: {}".format(int(value), cams[int(value)])) # print("usb0: {} usb1: {}".format(usb0, usb1)) server.setSource(cams[int(value)]) table = networktables.NetworkTables.getTable('/CameraPublisher') table.putNumber('selected', 0) table.addEntryListener(_listener, key='selected') cs.waitForever()
def main(): cs = CameraServer.getInstance() cs.enableLogging() outputStream = CvSource('camera', VideoMode.PixelFormat.kMJPEG, int(640), int(480), int(15)) cs.addCamera(outputStream) server = cs.addServer(name='camera', port=int(5801)) server.setSource(outputStream) camera = UsbCamera( "cam1", '/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0' ) # Capture from the first USB Camera on the system cs.startAutomaticCapture(camera=camera) camera.setResolution(640, 480) # Get a CvSink. This will capture images from the camera cvSink = cs.getVideo(camera=camera) # Allocating new images is very expensive, always try to preallocate img = np.zeros(shape=(480, 640, 3), dtype=np.uint8) while True: # Tell the CvSink to grab a frame from the camera and put it # in the source image. If there is an error notify the output. time, img = cvSink.grabFrame(img) if time == 0: # Send the output the error. outputStream.notifyError(cvSink.getError()) # skip the rest of the current iteration continue # # Insert your image processing logic here! # cv2.imshow("stream", img) # (optional) send some image back to the dashboard outputStream.putFrame(img)
# start NetworkTables to send to smartDashboard ntinst = NetworkTablesInstance.getDefault() print("Setting up NetworkTables client for team {}".format(team)) ntinst.startClientTeam(7539) SmartDashBoardValues = ntinst.getTable('SmartDashboard') #Start camera print("Connecting to camera") cs = CameraServer.getInstance() cs.enableLogging() Camera = UsbCamera('Cam 0', 0) Camera.setExposureManual(5) Camera.setResolution(160, 120) cs.addCamera(Camera) print("connected") CvSink = cs.getVideo() outputStream = cs.putVideo("Processed Frames", 160, 120) #buffer to store img data img = np.zeros(shape=(160, 120, 3), dtype=np.uint8) # loop forever while True: GotFrame, img = CvSink.grabFrame(img) if GotFrame == 0: outputStream.notifyError(CvSink.getError())
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)
lower_green = np.array([0, 255, 110]) #137 240 135 - HSV, 0,90,90 - RGB upper_green = np.array([80, 255, 200]) #143 255 148 - HSV, 86,255,255 - RGB camWidth = 320 camHeight = 240 center_x = camWidth * .5 margin = 20 cs = CameraServer.getInstance() cs.enableLogging() cam1 = UsbCamera("cam1", 0) cam2 = UsbCamera("cam2", 1) cam1.setResolution(camWidth, camHeight) cam2.setResolution(camWidth, camHeight) cam1.setExposureManual(8) cam2.setExposureManual(8) cvSink = cs.getVideo(camera=cam1) outputStream = cs.putVideo("Cam1", camWidth, camHeight) frame = np.zeros(shape=(camHeight, camWidth, 3), dtype=np.uint8) def listener(table, key, value, isNew): print("value changed: key: '%s'; value: %s; isNew: %s" % (key, value, isNew))
SmartDashBoardValues.setPersistent("HL") SmartDashBoardValues.setPersistent("HU") SmartDashBoardValues.setPersistent("SL") SmartDashBoardValues.setPersistent("SU") SmartDashBoardValues.setPersistent("VL") SmartDashBoardValues.setPersistent("VU") #Start first camera print("Connecting to camera 0") cs = CameraServer.getInstance() cs.enableLogging() Camera = UsbCamera('Cam 0', 0) #exp = 2 #Camera.setExposureManual(exp) Camera.setResolution(160,120) cs.addCamera(Camera) #SmartDashBoardValues.putNumber('ExpAuto', 0) print("connected") Camera1 = UsbCamera('Cam 1', 1) Camera1.setResolution(640, 480) Camera1.setFPS(25) mjpegServer = MjpegServer("serve_Cam 1", 1182) mjpegServer.setResolution(480, 360) mjpegServer.setSource(Camera1) CvSink = cs.getVideo() outputStream = cs.putVideo("Processed Frames", 160,120)
def main(table): camSrv = CameraServer.getInstance() camSrv.enableLogging() cam = UsbCamera('logitech', 0) cam.setResolution(CAM_WIDTH, CAM_HEIGHT) camSrv.addCamera(cam) # Get a CvSink. This will capture images from the camera cvSink = camSrv.getVideo() # (optional) Setup a CvSource. This will send images back to the Dashboard outputStream = camSrv.putVideo("Rectangle", CAM_WIDTH, CAM_HEIGHT) # Allocating new images is very expensive, always try to preallocate rawimg = np.zeros(shape=(CAM_HEIGHT, CAM_WIDTH, 3), dtype=np.uint8) # OpenCV ranges # Hue: 0 - 180 # Saturation: 0 - 255 # Vibrancy: 0 - 255 lower = np.array([0, 0, 225]) upper = np.array([10, 10, 255]) while True: # Tell the CvSink to grab a frame from the camera and put it # in the source image. If there is an error notify the output. time, rawimg = cvSink.grabFrame(rawimg, 0.5) if time == 0: # Send the output the error. print(cvSink.getError()) #outputStream.notifyError(cvSink.getError()) # skip the rest of the current iteration continue # Threshold the HSV image to get only the selected colors hsv = cv2.cvtColor(rawimg, cv2.COLOR_RGB2HSV) mask = cv2.inRange(hsv, lower, upper) mask, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) if len(contours) == 0: continue try: contours.sort(key=cv2.contourArea) target1 = contours[-1] target2 = contours[-2] target1Moment = cv2.moments(target1) target2Moment = cv2.moments(target2) except: continue try: target1Cx = int(target1Moment['m10'] / target1Moment['m00']) target1Cy = int(target1Moment['m01'] / target1Moment['m00']) target2Cx = int(target2Moment['m10'] / target2Moment['m00']) target2Cy = int(target2Moment['m01'] / target2Moment['m00']) except ZeroDivisionError: target1Cx = 0 target1Cy = 0 target2Cx = 0 target2Cy = 0 distance = int( vu.getDistance(target1Cx, target1Cy, target2Cx, target2Cy)) cv2.drawContours(rawimg, [target1], -1, (0, 0, 255), 2) cv2.drawContours(rawimg, [target2], -1, (0, 0, 255), 2) # figure the midpoint of x1,y1 and x2,y2, get the angle for midX midX, midY = vu.getMidPoint(target1Cx, target1Cy, target2Cx, target2Cy) midX = int(midX) midY = int(midY) if distance > 100: cv2.circle(rawimg, (target1Cx, target1Cy), 7, (255, 0, 0), -1) cv2.circle(rawimg, (target2Cx, target2Cy), 7, (255, 0, 0), -1) cv2.drawMarker(rawimg, (midX, midY), (0, 0, 255), cv2.MARKER_CROSS, 20, 3) cv2.line(rawimg, (336, 0), (336, 480), (0, 255, 0), 2) cv2.line(rawimg, (304, 0), (304, 480), (0, 255, 0), 2) #cv2.circle(rawimg, (midX, midY), 7, (0, 0, 255), -1) #angle = vu.getAngle(midX, CENTER, DEGREES_PER_PIXEL) print('%d\t%d' % (midX, distance)) table.putNumber('midX', midX) table.putNumber('distance', distance) # Give the output stream a new image to display outputStream.putFrame(rawimg)
def main(config): team = read_config(config) WIDTH, HEIGHT = 320, 240 FOV = 68.5 focal_length = WIDTH / (2 * math.tan(math.radians(FOV / 2))) print("Starting camera server") cs = CameraServer.getInstance() power_cell_cam = UsbCamera('power_cell_cam', 2) power_cell_cam.setResolution(WIDTH, HEIGHT) power_cell_cam.setExposureManual(50) tape_cam = UsbCamera('tape_cam', 0) tape_cam.setResolution(WIDTH, HEIGHT) tape_cam.setExposureManual(1) print("Connecting to Network Tables") ntinst = NetworkTablesInstance.getDefault() ntinst.startClientTeam(team) ntinst.addConnectionListener(connectionListener, immediateNotify=True) """Format of these entries found in WPILib documentation.""" power_cell_angle = ntinst.getTable('ML').getEntry('power_cell_angle') power_cell_pos = ntinst.getTable('ML').getEntry('power_cell_pos') power_cell_exists = ntinst.getTable('ML').getEntry('power_cell_exists') power_cell_dist = ntinst.getTable('ML').getEntry('power_cell_dist') power_cell_x = ntinst.getTable('ML').getEntry('power_cell_x') power_cell_y = ntinst.getTable('ML').getEntry('power_cell_y') tape_angle = ntinst.getTable('ML').getEntry('tape_angle') tape_dist = ntinst.getTable('ML').getEntry('tape_dist') tape_found = ntinst.getTable('ML').getEntry('tape_found') sd = ntinst.getTable('SmartDashboard') sd.addEntryListener(listener, key='cam') cvSink = cs.getVideo(camera=power_cell_cam) img = np.zeros(shape=(HEIGHT, WIDTH, 3), dtype=np.uint8) output = cs.putVideo("MLOut", WIDTH, HEIGHT) print("Initializing ML engine") engine = DetectionEngine("model.tflite") lower_yellow = np.array([20, 75, 100]) upper_yellow = np.array([40, 255, 255]) lower_green = np.array([55, 124, 81]) #137 240 135 - HSV, 0,90,90 - RGB upper_green = np.array([255, 255, 255]) #143 255 148 - HSV, 86,255,255 - RGB lower_color = np.array([0, 0, 0]) upper_color = np.array([0, 0, 0]) camera_robot_x = .3302 camera_robot_y = -.0254 print("Starting ML mainloop") while True: cam_mode = sd.getBoolean(key='cam', defaultValue=False) if cam_mode: cvSink.setSource(tape_cam) lower_color = lower_green upper_color = upper_green #print('green') else: cvSink.setSource(power_cell_cam) lower_color = lower_yellow upper_color = upper_yellow #print('yellow') t, frame = cvSink.grabFrame(img) # Run inference. if not cam_mode: ans = engine.detect_with_image(Image.fromarray(frame), threshold=0.5, keep_aspect_ratio=True, relative_coord=False, top_k=10) else: ans = False center_power_cell_x = WIDTH / 2 center_power_cell_y = HEIGHT / 2 bool_power_cell = False bool_tape = False largest_box_xmin = 0 largest_box_xmax = 0 largest_box_ymin = 0 largest_box_ymin = 0 largest_box_area = 0 minimum_box_area = 0 angle_to_turn_power_cell = 0 angle_to_turn_power_cell_robot = 0 dist_power_cell = 0 angle_to_turn_tape = 0 dist_tape = 0 new_x = 0 new_y = 0 center_contour_x = WIDTH / 2 # Display result. if ans: for obj in ans: box = [ round(i, 3) for i in obj.bounding_box.flatten().tolist() ] xmin, ymin, xmax, ymax = map(int, box) box_area = (xmax - xmin) * (ymax - ymin) if box_area > largest_box_area: largest_box_area = box_area largest_box_xmin = xmin largest_box_xmax = xmax largest_box_ymin = ymin largest_box_ymax = ymax label = '%s: %d%%' % ('power_cell', int(obj.score * 100) ) # Example: 'Cargo: 72%' label_size, base_line = cv2.getTextSize( label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2) label_ymin = max(ymin, label_size[1] + 10) cv2.rectangle( frame, (xmin, label_ymin - label_size[1] - 10), (xmin + label_size[0], label_ymin + base_line - 10), (255, 255, 255), cv2.FILLED) cv2.putText(frame, label, (xmin, label_ymin - 7), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2) cv2.rectangle(frame, (xmin, ymin), (xmax, ymax), (10, 255, 0), 4) if (largest_box_area > minimum_box_area): center_power_cell_x = 0.5 * (largest_box_xmin + largest_box_xmax) center_power_cell_y = 0.5 * (largest_box_ymin + largest_box_ymax) bool_power_cell = True angle_to_turn_power_cell = math.degrees( math.atan((center_power_cell_x - (.5 * WIDTH - .5)) / focal_length)) angle_to_right_side = math.degrees( math.atan( (largest_box_xmax - (.5 * WIDTH - .5)) / focal_length)) angle_diff_right_center = abs(angle_to_right_side - angle_to_turn_power_cell) dist_power_cell = (.5842 / (2 * math.pi)) / math.tan( math.radians(angle_diff_right_center)) x_vec = dist_power_cell / math.sqrt( 1 + math.tan(math.radians(-angle_to_turn_power_cell))**2) y_vec = x_vec * math.tan( math.radians((-angle_to_turn_power_cell))) if (angle_to_turn_power_cell < 0): x_vec *= -1 y_vec *= -1 new_x = camera_robot_x + x_vec new_y = camera_robot_y + y_vec angle_to_turn_power_cell_robot = -math.degrees( math.atan2(new_y, new_x)) cv2.putText(frame, str(angle_to_turn_power_cell_robot), (100, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) #cv2.putText(frame, str(new_y), (100, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) #cv2.putText(frame, str(dist_power_cell), (100, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) else: hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, lower_color, upper_color) contours = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[-2] ''' TODO: Determine how reflective tapes will look through eye of god TODO: Calculate angle and distance to tape (tape angle calculation same as power cell angle calculation) ''' largest_contour = np.array([0, 0]) for c in contours: area = cv2.contourArea(c) if area > largest_box_area: largest_contour = c largest_box_area = area if (largest_box_area > minimum_box_area and not cam_mode): x, y, w, h = cv2.boundingRect(largest_contour) cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2) center_power_cell_x = 0.5 * (x + x + w) center_power_cell_y = 0.5 * (y + y + h) bool_power_cell = True angle_to_turn_power_cell = math.degrees( math.atan((center_power_cell_x - (.5 * WIDTH - .5)) / focal_length)) angle_to_right_side = math.degrees( math.atan(((x + w) - (.5 * WIDTH - .5)) / focal_length)) angle_diff_right_center = angle_to_right_side - angle_to_turn_power_cell dist_power_cell = (.5842 / (2 * math.pi)) / math.tan( math.radians(angle_diff_right_center)) x_vec = dist_power_cell / math.sqrt( 1 + math.tan(math.radians(-angle_to_turn_power_cell))**2) y_vec = x_vec * math.tan( math.radians((-angle_to_turn_power_cell))) if (angle_to_turn_power_cell < 0): x_vec *= -1 y_vec *= -1 new_x = camera_robot_x + x_vec new_y = camera_robot_y + y_vec angle_to_turn_power_cell_robot = -math.degrees( math.atan2(new_y, new_x)) cv2.putText(frame, str(angle_to_turn_power_cell_robot), (100, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) #cv2.putText(frame, str(new_y), (100, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) # cv2.putText(frame, str(dist_power_cell), (100, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) elif (largest_box_area > 0 and cam_mode): x, y, w, h = cv2.boundingRect(largest_contour) M1 = cv2.moments(largest_contour) center_contour_x = int(M1['m10'] / M1['m00']) cv2.drawContours(frame, [largest_contour], 0, (0, 255, 0), 3) angle_to_turn_tape = math.degrees( math.atan( (center_contour_x - (.5 * WIDTH - .5)) / focal_length)) angle_to_right_side = math.degrees( math.atan(((x + w) - (.5 * WIDTH - .5)) / focal_length)) angle_diff_right_center = abs(angle_to_right_side - angle_to_turn_tape) dist_tape = .99695 / math.tan( math.radians(angle_diff_right_center)) cv2.putText(frame, str(dist_tape), (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) bool_tape = True output.putFrame(frame) tape_found.setBoolean(bool_tape) if cam_mode: angle_to_turn_tape = (center_contour_x / WIDTH * FOV) - FOV / 2 tape_angle.setDouble(angle_to_turn_tape) #TODO: Send distance to tape else: power_cell_angle.setDouble(angle_to_turn_power_cell_robot) power_cell_pos.setDoubleArray( [center_power_cell_x, center_power_cell_y]) power_cell_exists.setBoolean(bool_power_cell) power_cell_dist.setDouble(dist_power_cell) power_cell_x.setDouble(new_x) power_cell_y.setDouble(new_y)
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)
# This code is designed to work for the Microsoft Lifecam HD3000 and the limelight ring log.basicConfig(level=log.DEBUG) # NetworkTables set up ntinst = nti.getDefault() ntinst.startClientTeam(578) sd = ntinst.getTable('SmartDashboard') # Start the camera cs = CameraServer.getInstance() cs.enableLogging() Camera = UsbCamera('rPi Camera 0', 0) Camera.setResolution(1280, 720) cs.addCamera(Camera) CvSink = cs.getVideo() outputStream = cs.putVideo("Processed Frames", 1280, 720) img = np.zeros(shape=(1280, 720, 3), dtype=np.uint8) #cap = cv2.VideoCapture(2) while (True): GotFrame, img = CvSink.grabFrame(img) if (GotFrame == 0): outputStream.notifyError(CvSink.getError()) continue
fiveMove = False else: rightFound = True return x if __name__ == "__main__": if len(sys.argv) >= 2: configFile = sys.argv[1] # start cameras print("Connecting to camera") cs = CameraServer.getInstance() cs.enableLogging() Camera = UsbCamera('rPi Camera 0', 0) Camera.setResolution(width, height) #cs.addCamera(Camera) cs.startAutomaticCapture(camera=Camera, return_server=True) CvSink = cs.getVideo() outputStream = CvSource('Processed Frames', VideoMode.PixelFormat.kBGR, width, height, 28) img = np.zeros(shape=(width, height, 3), dtype=np.uint8) count = 0 global topX global topY global botX global botY global direction global alreadyFound while True:
def main(table): camSrv = CameraServer.getInstance() camSrv.enableLogging() cam = UsbCamera("logitech", 0) camSrv.addCamera(cam) #cam = cs.startAutomaticCapture() cam.setResolution(CAM_WIDTH, CAM_HEIGHT) # Get a CvSink. This will capture images from the camera cvSink = camSrv.getVideo() #camera=cam) # (optional) Setup a CvSource. This will send images back to the Dashboard outputStream = camSrv.putVideo("Rectangle", CAM_WIDTH, CAM_HEIGHT) # Allocating new images is very expensive, always try to preallocate rawimg = np.zeros(shape=(CAM_HEIGHT, CAM_WIDTH, 3), dtype=np.uint8) # OpenCV ranges # Hue: 0 - 180 # Saturation: 0 - 255 # Vibrancy: 0 - 255 lower = np.array([0, 0, 200]) upper = np.array([10, 100, 255]) while True: # Tell the CvSink to grab a frame from the camera and put it # in the source image. If there is an error notify the output. #print('grab a frame...') time, rawimg = cvSink.grabFrame(rawimg,0.5) if time == 0: # Send the output the error. print(cvSink.getError()) #outputStream.notifyError(cvSink.getError()) # skip the rest of the current iteration continue # convert RGB to HSV hsv = cv2.cvtColor(rawimg, cv2.COLOR_RGB2HSV) # Threshold the HSV image to get only the selected colors mask = cv2.inRange(hsv, lower, upper) mask, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) if len(contours) == 0: print('no contours') continue try: contours.sort(key=lambda x: cv2.contourArea(x)) target = max(contours, key=cv2.contourArea) boundingBox = cv2.minAreaRect(target) except: print('no bounding box') continue angle = boundingBox[2] if angle < -45: angle = angle + 90 table.putNumber('cameraAngle', angle) # draw a red outline on the output image # so that the user can see what is targeted boxPoints = cv2.boxPoints(boundingBox) boxPoints = np.int0(boxPoints) rawimg = cv2.drawContours(rawimg, [boxPoints], 0, (0,0,255), 2) # Give the output stream a new image to display outputStream.putFrame(rawimg)
except KeyError: parseError("could not read cameras") return False for camera in cameras: if not readCameraConfig(camera): return False return True readConfig() #Set up the camera camServer = CameraServer.getInstance() camServer.enableLogging() camera = UsbCamera("rPi Camera 0", 0) camera.setFPS(30) camera.setResolution(cameraWidth, cameraHeight) camServer.addCamera(camera) cvSink = camServer.getVideo() outputStream = camServer.putVideo("Processed Frames", cameraWidth, cameraHeight) ntinst = NetworkTablesInstance.getDefault() print("Setting up NetworkTables client for team {}".format(team)) ntinst.startClientTeam(team) dashboard = ntinst.getTable('SmartDashboard') #outputStream = camServer.putVideo("Vision", cameraWidth, cameraHeight) #camera = camServer.startAutomaticCapture();