fotobox_SPRT/Hauptprogramm.py

#Hauptprogramm; Nimmt kontinuierlich Bilder auf wenn externer Trigger ausgelöst wird und speichert diese bei Bedarf

import cv2
import av
import time
import subprocess
import multiprocessing
from multiprocessing import shared_memory
import sys
import signal
import RPi.GPIO as GPIO
import numpy as np
import psutil
import os

#Region of Interest definieren:
roi_start = 0	#min: 0
roi_end = 800	#max: 720

roi_breite = roi_end - roi_start


#Warnungen von av deaktivieren (Ansonsten kommt dauerhaft eine Warnung über ein mögliches falsches Pixelformat, ist aber iO)
av.logging.set_level(av.logging.ERROR)


#GPIO Pin für Speicherimpuls einstellen
ausloeser = 17
GPIO.setmode(GPIO.BCM)
GPIO.setup(ausloeser, GPIO.IN, pull_up_down = GPIO.PUD_UP)


#Kameraeinstellungen; Soll später aus einer Textdatei ausgelesen werden, die über ein GUI eingestellt wird (Fast fertig)
camera_ids = [0, 2, 4, 6]

triggermodus_on = "v4l2-ctl -d %d -c exposure_dynamic_framerate=1"
triggermodus_off = "v4l2-ctl -d %d -c exposure_dynamic_framerate=0"
belichtung = "v4l2-ctl -d %d -c exposure_time_absolute=1"
auto_exposure = "v4l2-ctl -d %d -c auto_exposure=1"
brightness = "v4l2-ctl -d %d -c brightness=64"
contrast = "v4l2-ctl -d %d -c contrast=64"


#Funktion um die Kameraeinstellungen an das Terminal zu senden
def run_cmd(cmd):
	try:
		subprocess.run(cmd, shell=True, check=True)
	except subprocess.CalledProcessError as e:
		print("Fehler", e)
		


#Ctrl+C zum Programmabbruch definieren
def signal_handler(sig, frame):
	print('Abbruch')
	sys.exit(0)
	#Evtl. noch Kameras schließen, Python Prozesse beenden, Bildspeicher komplett leeren, usw..)

signal.signal(signal.SIGINT, signal_handler)


#Klasse definieren, um Bilder von den Kameras aufzunehmen
class CameraModule():
	def run(self, camera_id, camera_values, shm_np_block, shm_camera_id_block, shm_k_block):

		shm_np_array = np.ndarray(shm_shape, dtype='uint8', buffer=shm_np_block.buf)
		shm_camera_id_array = np.frombuffer(shm_camera_id_block.buf, dtype='int32')
		shm_k_array = np.frombuffer(shm_k_block.buf, dtype='int32')
		shm_cam_id = np.frombuffer(camera_values.buf, dtype='int32')

		ring_counter = 0

		#Cv2-Stream öffnen (Ohne den Cv2 Warmup werden die Kameras komischerweise nicht korrekt gestartet, hab da vieles probiert..)
		cap = cv2.VideoCapture(f"/dev/video{camera_id}", cv2.CAP_V4L2)
		cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
		cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
		cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 800)

		for _ in range(50):		#50 Bilder aufnehmen um Kamera zu stabilisieren
			ret, frame = cap.read()
		
		cap.release()
		cv2.destroyAllWindows()
		time.sleep(5)

		#av-Kameraeinstellungen
		options = {
			'video_size': '1280x800',
			'framerate': '60',
			'input_format': 'mjpeg'
		}

		input_stream = av.open('/dev/video{}'.format(camera_id), format='video4linux2', options=options)
		time.sleep(1)
		video_stream = next(s for s in input_stream.streams if s.type == 'video')
		framerate = video_stream.average_rate
		print(f"Kamera {camera_id} Framerate: {framerate}")	#Zur Kontrolle

		#50 Bilder aufnehmen; Dient dazu, die Kamera zu stabilisieren (Weißabgleich usw.)
		for _ in range(50):
			frame = next(input_stream.decode(video_stream))
			time.sleep(0.1)
	
		#Triggermodus der Kamera auf externen Trigger umstellen
		run_cmd(triggermodus_on % camera_id)

		k = 1
		frame = None
		ring_counter = 0

		#Buffer der Kameras (~5 Bilder) füllen und Trigger synchronisieren; Wird später auch noch automatisiert durch Verbindung mit dem Arduino
		print("Trigger auslösen bis Freigabe kommt")
		for _ in range(50):
			frame = next(input_stream.decode(video_stream))
		print("Freigabe erteilt")
		time.sleep(5)
		print("Bereit")

		#Kontinuierlich Bilder aufnehmen und in die image_queue schreiben
		while True:
				
				slot = (camera_id // 2 + ring_counter * 4) % 200
				frame = next(input_stream.decode(video_stream))
				numpy_frame = frame.to_ndarray(format='bgr24')[roi_start:roi_end, :]

				freigabe = shm_k_array[slot]
				
				if freigabe == 0:
					shm_np_array[slot] = numpy_frame
					shm_camera_id_array[slot] = camera_id
					shm_k_array[slot] = k
					shm_cam_id[camera_id//2] = k

					k += 1
					ring_counter = (ring_counter + 1)

				else:
					print("Speicher wird überschrieben")
					print(shm_k_array[slot])
					print(slot)
					print(camera_id)

	#Noch eine Fehlererkennung einbauen, falls Frame nicht ausgelesen werden kann?


#Kameraworker definieren
def camera_worker(camera_id, camera_values, shm_np_block, shm_camera_id_block, shm_k_block, cpu_core):
	p = psutil.Process(os.getpid())
	p.cpu_affinity([cpu_core])	#Jede Instanz wird auf einem seperaten CPU Kern ausgeführt
	camera_module = CameraModule()
	camera_module.run(camera_id, camera_values, shm_np_block, shm_camera_id_block, shm_k_block)


#Klasse definieren, um GPIO/Speicherimpuls zu überwachen
class SaveHandler():
	def __init__(self, camera_values, trigger_liste):
		self.camera_values = camera_values
		self.trigger_liste = trigger_liste
		GPIO.add_event_detect(ausloeser, GPIO.FALLING, callback=self.ausloeser_callback, bouncetime=200)

	def ausloeser_callback(self, channel):
		shm_cam_id = np.frombuffer(camera_values.buf, dtype='int32')

		while True:
			if not GPIO.input(channel):

				cam_value = shm_cam_id
				self.trigger_liste.append(cam_value)
				print("Trigger empfangen")
				time.sleep(1)	#Ist nur Testweise drin, damit nicht zu oft getriggert wird durch betätigen eines Tasters
			

def handler_worker(camera_values, trigger_liste):
	handler = SaveHandler(camera_values, trigger_liste)
	handler.ausloeser_callback(ausloeser)


#Klasse definieren, um Bilder von der image_queue zu holen und an den Saver zu senden, falls gewünscht
class SaveImageProcess():
	def __init__(self, shm_np_block, shm_camera_id_block, shm_k_block, trigger_liste, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t):
		self.trigger_liste = trigger_liste
		self.shm_np_block = shm_np_block
		self.shm_camera_id_block = shm_camera_id_block
		self.shm_k_block = shm_k_block
		self.shm_save_frame = shm_save_frame
		self.shm_save_k = shm_save_k
		self.shm_save_camera_id = shm_save_camera_id
		self.shm_save_ts = shm_save_ts
		self.shm_save_t = shm_save_t


	def run(self, shm_np_block, shm_camera_id_block, shm_k_block, trigger_liste, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t):

		shm_np_array = np.ndarray(shm_shape, dtype='uint8', buffer=shm_np_block.buf)
		shm_camera_id_array = np.frombuffer(shm_camera_id_block.buf, dtype='int32')
		shm_k_array = np.frombuffer(shm_k_block.buf, dtype='int32')

		shm_save_frame_array = np.ndarray(save_shape, dtype='uint8', buffer=shm_save_frame.buf)
		shm_save_k_array = np.frombuffer(shm_save_k.buf, dtype='int32')
		shm_save_camera_id_array = np.frombuffer(shm_save_camera_id.buf, dtype='int32')
		shm_save_ts_array = np.frombuffer(shm_save_ts.buf, dtype='int32')
		shm_save_t_array = np.frombuffer(shm_save_t.buf, dtype='int32')


		ring_counter = 0
		save_counter = 0

		j = 0
		l = 0
		print("SaveImageProcess gestartet")

		while True:

			non_zero_indices = [index for index, value in enumerate(shm_k_array[:]) if value != 0]

			if len(non_zero_indices) > 20:
				print(non_zero_indices)
				for index in non_zero_indices:

					camera_id = shm_camera_id_array[index]
					k = shm_k_array[index]
					shm_k_array[index] = 0

					trigger_test = np.array(trigger_liste)

					for i in range(j, trigger_test.shape[0]):
						if k >= (trigger_test[i][camera_id//2]-5+1) and k <= (trigger_test[i][camera_id//2]+5+1):
							shm_save_frame_array[save_counter] = shm_np_array[index]
							shm_save_camera_id_array[save_counter] = camera_id
							shm_save_k_array[save_counter] = k
							shm_save_ts_array[save_counter] = trigger_test[i][camera_id//2]
							shm_save_t_array[save_counter] = i

							save_counter = (save_counter + 1) % 200


						if k > (trigger_test[i][camera_id//2]+200):
							j += 1


def save_worker(shm_np_block, shm_camera_id_block, shm_k_block, trigger_liste, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t):

	save_worker = SaveImageProcess(shm_np_block, shm_camera_id_block, shm_k_block, trigger_liste, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t)
	save_worker.run(shm_np_block, shm_camera_id_block, shm_k_block, trigger_liste, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t)

#Klasse um die Bilder zu speichern (Ist aufgeteilt, da das Speichern der Bilder deutlich der Aufnahme der Bilder hinterherhängt und ohne diesen Frames verworfen werden)
class Saver():
	def __init__(self, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t):
		self.shm_save_frame = shm_save_frame
		self.shm_save_k = shm_save_k
		self.shm_save_camera_id = shm_save_camera_id
		self.shm_save_ts = shm_save_ts
		self.shm_save_t = shm_save_t

	def run(self, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t):

		shm_save_frame_array = np.ndarray(save_shape, dtype='uint8', buffer=shm_save_frame.buf)
		shm_save_k_array = np.frombuffer(shm_save_k.buf, dtype='int32')
		shm_save_camera_id_array = np.frombuffer(shm_save_camera_id.buf, dtype='int32')
		shm_save_ts_array = np.frombuffer(shm_save_ts.buf, dtype='int32')
		shm_save_t_array = np.frombuffer(shm_save_t.buf, dtype='int32')
		
		ring_counter = 0

		while True:
			if shm_save_k_array[ring_counter] != 0:

				filename = "/mnt/ramdisk/Cam{}_T{}_TS{}_B{}.jpg".format(shm_save_camera_id_array[ring_counter], shm_save_t_array[ring_counter], shm_save_ts_array[ring_counter], shm_save_k_array[ring_counter])
				cv2.imwrite(filename, shm_save_frame_array[ring_counter])

				shm_save_k_array[ring_counter] = 0
				ring_counter = (ring_counter + 1) % 200


def saver_worker(shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t):

	saver = Saver(shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t)
	saver.run(shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t)


	




		
if __name__ == "__main__":

	#Einstellungen der Shared Memory Blöcke
	shm_shape = (200, roi_breite, 1280, 3)
	save_shape = (200, roi_breite, 1280, 3)
	cv_shape = (1, 4, 1, 1)
	shm_np_block = shared_memory.SharedMemory(create=True, size=shm_shape[0]*shm_shape[1]*shm_shape[2]*shm_shape[3]*np.dtype('uint8').itemsize)
	shm_camera_id_block = shared_memory.SharedMemory(create=True, size=200 * np.dtype('int32').itemsize)
	shm_k_block = shared_memory.SharedMemory(create=True, size=200 * np.dtype('int32').itemsize)


	shm_save_frame = shared_memory.SharedMemory(create=True, size=save_shape[0]*save_shape[1]*save_shape[2]*save_shape[3]*np.dtype('uint8').itemsize)
	shm_save_k = shared_memory.SharedMemory(create=True, size=200 * np.dtype('int32').itemsize)
	shm_save_camera_id = shared_memory.SharedMemory(create=True, size=200 * np.dtype('int32').itemsize)
	shm_save_ts = shared_memory.SharedMemory(create=True, size=200 * np.dtype('int32').itemsize)
	shm_save_t = shared_memory.SharedMemory(create=True, size=200 * np.dtype('int32').itemsize)

	camera_values = shared_memory.SharedMemory(create=True, size= 4*np.dtype('int32').itemsize)



	#Kameraeinstellungen vornehmen/Wird später durch GUI ersetzt
	print("Kameras einstellen")
	for i in [0, 2, 4, 6]:
		run_cmd(triggermodus_off % i)
		time.sleep(0.5)
		run_cmd(auto_exposure % i)
		time.sleep(0.5)
		run_cmd(belichtung % i)
		time.sleep(0.5)
		run_cmd(brightness % i)
		time.sleep(0.5)
		run_cmd(contrast % i)
		time.sleep(0.5)
		print("Kamera %d eingestellt" % i)


	#Trigger_liste als Multiprocessing-Liste erstellen
	manager = multiprocessing.Manager()
	trigger_liste = manager.list()


	processes = []
	i = 0
	cpu_cores = [0, 1, 2, 3]

	for camera_id in camera_ids:
		process = multiprocessing.Process(target=camera_worker, args=(camera_id, camera_values, shm_np_block, shm_camera_id_block, shm_k_block, cpu_cores[i]))
		i += 1
		process.start()
		processes.append(process)


	handler = SaveHandler(camera_values, trigger_liste)

	save_process = multiprocessing.Process(target=save_worker, args=(shm_np_block, shm_camera_id_block, shm_k_block, trigger_liste, shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t))
	processes.append(save_process)
	save_process.start()

	saver2 = Saver(shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t)
	saver2.run(shm_save_frame, shm_save_k, shm_save_camera_id, shm_save_ts, shm_save_t)

	process.join()
	save_process.join()
	saver2.join()