mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
253 lines
11 KiB
Python
253 lines
11 KiB
Python
import math
|
|
import cv2
|
|
import numpy
|
|
from cscore import CameraServer
|
|
from app.classes.SettingsManager import SettingsManager
|
|
from ..handlers.SocketHandler import send_all_async
|
|
from multiprocessing import Process
|
|
import threading
|
|
import zmq
|
|
import asyncio
|
|
import time
|
|
from networktables import NetworkTables
|
|
import networktables
|
|
from .VisionHandler import VisionHandler
|
|
|
|
|
|
class CameraHandler:
|
|
def __init__(self, cam_name, port):
|
|
#settings vars up for vision loop
|
|
self.cs = CameraServer.getInstance()
|
|
self.settings_manager = SettingsManager()
|
|
self.vision_handler = VisionHandler()
|
|
self.port = port
|
|
self.cam_name = cam_name
|
|
self.image = None
|
|
self.p_image = None
|
|
self.table = None
|
|
self.nt_data = {'valid': False}
|
|
self.time_stamp = 0
|
|
|
|
def run(self):
|
|
#starting main thread
|
|
threading.Thread(target=self.thread_proc).start()
|
|
|
|
def thread_proc(self):
|
|
self.settings_manager.cams_curr_pipeline[self.cam_name] = "pipeline0"
|
|
pipeline = self.settings_manager.cams[self.cam_name]["pipelines"][self.settings_manager.cams_curr_pipeline[self.cam_name]]
|
|
FOV = self.settings_manager.cams[self.cam_name]["FOV"]
|
|
|
|
def change_camera_values(pipeline):
|
|
self.settings_manager.usb_cameras[self.cam_name].setBrightness(pipeline['brightness'])
|
|
self.settings_manager.usb_cameras[self.cam_name].setExposureManual(pipeline['exposure'])
|
|
self.settings_manager.usb_cameras[self.cam_name].setWhiteBalanceAuto()
|
|
|
|
def pipeline_listener(table, key, value, is_new):
|
|
asyncio.set_event_loop(asyncio.new_event_loop())
|
|
if value in self.settings_manager.cams[self.cam_name]['pipelines'].keys():
|
|
self.settings_manager.cams_curr_pipeline[self.cam_name] = value
|
|
change_camera_values(pipeline)
|
|
if self.cam_name == self.settings_manager.general_settings['curr_camera']:
|
|
self.settings_manager.general_settings['curr_pipeline'] = value
|
|
update_settings = self.settings_manager.get_curr_pipeline()
|
|
update_settings['curr_pipeline'] = self.settings_manager.general_settings["curr_pipeline"]
|
|
send_all_async(update_settings)
|
|
else:
|
|
self.table.putString('Pipeline', self.settings_manager.cams_curr_pipeline[self.cam_name])
|
|
|
|
def mode_listener(table, key, value, is_new):
|
|
if value:
|
|
change_camera_values({
|
|
'brightness': 25,
|
|
'exposure': 15
|
|
})
|
|
else:
|
|
change_camera_values(pipeline)
|
|
#setting up network table
|
|
self.table = NetworkTables.getTable("/Chameleon-Vision/" + self.cam_name)
|
|
#init values for pipeline and driver mode
|
|
self.table.putString('Pipeline', self.settings_manager.cams_curr_pipeline[self.cam_name])
|
|
self.table.putBoolean('Driver_Mode', False)
|
|
self.table.addEntryListenerEx(pipeline_listener, key="Pipeline",
|
|
flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)
|
|
self.table.addEntryListenerEx(mode_listener, key="Driver_Mode",
|
|
flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)
|
|
|
|
# getting video from current camera
|
|
cv_sink = self.cs.getVideo(camera=self.settings_manager.usb_cameras[self.cam_name])
|
|
|
|
width = self.settings_manager.cams[self.cam_name]["video_mode"]["width"]
|
|
height = self.settings_manager.cams[self.cam_name]["video_mode"]["height"]
|
|
|
|
# setting up a video server for camera
|
|
cv_publish = self.cs.putVideo(name=self.cam_name, width=width, height=height)
|
|
# saving camera port in cam name dict for usage in client
|
|
self.settings_manager.cams_port[self.cam_name] = self.cs._sinks['serve_' + self.cam_name].getPort()
|
|
|
|
# setting up a zmq connection to the opencv subprocess
|
|
context = zmq.Context()
|
|
socket = context.socket(zmq.PAIR)
|
|
socket.bind('tcp://*:%s' % str(self.port))
|
|
|
|
# starting the process with initial values
|
|
p = Process(target=self.camera_process, args=(self.cam_name, self.port, FOV))
|
|
p.start()
|
|
|
|
change_camera_values(pipeline)
|
|
|
|
def _publish_thread():
|
|
#getting image values and publishing process image and data
|
|
self.image = numpy.zeros(shape=(width, height, 3), dtype=numpy.uint8)
|
|
self.p_image = self.image
|
|
while True:
|
|
try:
|
|
self.time_stamp, self.image = cv_sink.grabFrame(self.image)
|
|
cv_publish.putFrame(self.p_image)
|
|
self.table.putBoolean('valid', self.nt_data['valid'])
|
|
# check if point is valid
|
|
if self.nt_data['valid']:
|
|
# send the point using network tables
|
|
self.table.putNumber('pitch', self.nt_data['pitch'])
|
|
self.table.putNumber('yaw', self.nt_data['yaw'])
|
|
self.table.putNumber('time_stamp', self.nt_data['time_stamp'])
|
|
self.table.putNumber('fps', self.nt_data['fps'])
|
|
# if the selected camera in ui is this cam send the point to the ui
|
|
except:
|
|
pass
|
|
|
|
def _socket_thread():
|
|
#publishing to websocket at slower interval
|
|
asyncio.set_event_loop(asyncio.new_event_loop())
|
|
while True:
|
|
time.sleep(0.1)
|
|
if self.settings_manager.general_settings['curr_camera'] == self.cam_name:
|
|
try:
|
|
send_all_async({
|
|
'raw_point': self.nt_data['raw_point'],
|
|
'point': {
|
|
'pitch': self.nt_data['pitch'],
|
|
'yaw': self.nt_data['yaw'],
|
|
'fps': self.nt_data['fps']
|
|
}
|
|
})
|
|
except:
|
|
pass
|
|
|
|
threading.Thread(target=_publish_thread).start()
|
|
threading.Thread(target=_socket_thread).start()
|
|
|
|
while True:
|
|
#sending and reciving data from opencv sub process
|
|
pipeline = self.settings_manager.cams[self.cam_name]["pipelines"][
|
|
self.settings_manager.cams_curr_pipeline[self.cam_name]]
|
|
|
|
socket.send_json(dict(
|
|
pipeline=pipeline,
|
|
driver_mode=self.table.getBoolean('Driver_Mode', False)
|
|
), zmq.SNDMORE)
|
|
|
|
socket.send_pyobj((self.time_stamp,self.image))
|
|
self.p_image = socket.recv_pyobj()
|
|
self.nt_data = socket.recv_json()
|
|
|
|
def camera_process(self, cam_name, port, FOV):
|
|
from fractions import Fraction
|
|
#calc fov
|
|
diagonalView = math.radians(FOV)
|
|
|
|
width = self.settings_manager.cams[cam_name]["video_mode"]["width"]
|
|
height = self.settings_manager.cams[cam_name]["video_mode"]["height"]
|
|
centerX = (width / 2) - .5
|
|
centerY = (height / 2) - .5
|
|
cam_area = width * height
|
|
|
|
aspect_fraction = Fraction(width, height)
|
|
horizontal_ratio = aspect_fraction.numerator
|
|
vertical_ratio = aspect_fraction.denominator
|
|
|
|
horizontalView = math.atan(math.tan(diagonalView / 2) * (horizontal_ratio / diagonalView)) * 2
|
|
verticalView = math.atan(math.tan(diagonalView / 2) * (vertical_ratio / diagonalView)) * 2
|
|
|
|
H_FOCAL_LENGTH = width / (2 * math.tan((horizontalView / 2)))
|
|
V_FOCAL_LENGTH = height / (2 * math.tan((verticalView / 2)))
|
|
#setting up zmq socket
|
|
context = zmq.Context()
|
|
socket = context.socket(zmq.PAIR)
|
|
socket.connect('tcp://localhost:%s' % str(port))
|
|
#setting up filter countours class
|
|
filter_contours = self.vision_handler.Filter_Contours(center_x=centerX, center_y=centerY)
|
|
|
|
x = 1
|
|
counter = 0
|
|
start_time = time.time()
|
|
fps = 0
|
|
|
|
while True:
|
|
obj = socket.recv_json()
|
|
curr_pipeline = obj['pipeline']
|
|
driver_mode = obj['driver_mode']
|
|
time_stamp, image = socket.recv_pyobj()
|
|
if curr_pipeline['orientation'] == "Inverted":
|
|
M = cv2.getRotationMatrix2D((width / 2, height / 2), 180, 1)
|
|
image = cv2.warpAffine(image, M, (width, height))
|
|
if not driver_mode:
|
|
hsv_image = self.vision_handler._hsv_threshold(curr_pipeline["hue"],
|
|
curr_pipeline["saturation"], curr_pipeline["value"],
|
|
image, curr_pipeline["erode"], curr_pipeline["dilate"])
|
|
# if table.getBoolean("Driver_Mode", False):
|
|
contours = self.vision_handler.find_contours(hsv_image)
|
|
filtered_contours = filter_contours.filter_contours(input_contours=contours, area=curr_pipeline['area'],
|
|
ratio=curr_pipeline['ratio'],
|
|
extent=curr_pipeline['extent'],
|
|
sort_mode=curr_pipeline['sort_mode'], cam_area=cam_area,
|
|
target_grouping=curr_pipeline['target_group'],
|
|
target_intersection=
|
|
curr_pipeline['target_intersection'])
|
|
|
|
final_contour = self.vision_handler.output_contour(filtered_contours)
|
|
|
|
try:
|
|
center = final_contour[0]
|
|
if curr_pipeline["M"] == 1 and curr_pipeline["B"] == 0:
|
|
center_x = centerX
|
|
center_y = centerY
|
|
else:
|
|
center_x = (center[1] - curr_pipeline['B']) / curr_pipeline["M"]
|
|
center_y = (center[0] * curr_pipeline["M"]) + curr_pipeline["B"]
|
|
pitch = self.vision_handler.calculate_pitch(pixel_y=center[1], center_y=center_y, v_focal_length=V_FOCAL_LENGTH)
|
|
yaw = self.vision_handler.calculate_yaw(pixel_x=center[0], center_x=center_x, h_focal_length=H_FOCAL_LENGTH)
|
|
valid = True
|
|
except IndexError:
|
|
center = None
|
|
pitch = None
|
|
yaw = None
|
|
valid = False
|
|
|
|
if curr_pipeline['is_binary']:
|
|
draw_image = hsv_image
|
|
else:
|
|
draw_image = image
|
|
|
|
res = self.vision_handler.draw_image(input_image=draw_image, contour=final_contour)
|
|
else:
|
|
res = image
|
|
center = None
|
|
pitch = None
|
|
yaw = None
|
|
valid = False
|
|
|
|
socket.send_pyobj(res)
|
|
socket.send_json(dict(
|
|
pitch=pitch,
|
|
yaw=yaw,
|
|
valid=valid,
|
|
raw_point=center,
|
|
fps=fps,
|
|
time_stamp=time_stamp
|
|
))
|
|
counter += 1
|
|
if (time.time() - start_time) > x:
|
|
fps = (counter / (time.time() - start_time))
|
|
counter = 0
|
|
start_time = time.time()
|