diff --git a/poetry.lock b/poetry.lock index b26fb36..d294491 100644 --- a/poetry.lock +++ b/poetry.lock @@ -2612,6 +2612,17 @@ files = [ {file = "pytz-2025.1.tar.gz", hash = "sha256:c2db42be2a2518b28e65f9207c4d05e6ff547d1efa4086469ef855e4ab70178e"}, ] +[[package]] +name = "pyusb" +version = "1.3.1" +description = "Easy USB access for Python" +optional = false +python-versions = ">=3.9.0" +files = [ + {file = "pyusb-1.3.1-py3-none-any.whl", hash = "sha256:bf9b754557af4717fe80c2b07cc2b923a9151f5c08d17bdb5345dac09d6a0430"}, + {file = "pyusb-1.3.1.tar.gz", hash = "sha256:3af070b607467c1c164f49d5b0caabe8ac78dbed9298d703a8dbf9df4052d17e"}, +] + [[package]] name = "pywin32" version = "308" @@ -3924,4 +3935,4 @@ watchdog = ["watchdog (>=2.3)"] [metadata] lock-version = "2.0" python-versions = "^3.10,<3.12," -content-hash = "8a4b9895c94c13642a75d7b4a00f36e271674325009d24758df44bf9a023f103" +content-hash = "1327d61e8a4f6d1eee7a6bfd234edb71c7165b6c30903d49522afa52252ddcc2" diff --git a/pyproject.toml b/pyproject.toml index 751a7b2..a6add17 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -46,6 +46,7 @@ tensorboardx = "^2.6.2.2" shapely = "^1" baumer-neoapi = {path = "../../Downloads/Baumer_neoAPI_1.4.1_lin_x86_64_python/wheel/baumer_neoapi-1.4.1-cp34.cp35.cp36.cp37.cp38.cp39.cp310.cp311.cp312-none-linux_x86_64.whl"} qrcode = "^8.0" +pyusb = "^1.3.1" [build-system] requires = ["poetry-core"] diff --git a/trap/animation_renderer.py b/trap/animation_renderer.py index c968d03..614693b 100644 --- a/trap/animation_renderer.py +++ b/trap/animation_renderer.py @@ -84,7 +84,7 @@ class AnimationRenderer: config = pyglet.gl.Config(sample_buffers=1, samples=4) # , fullscreen=self.config.render_window - display = pyglet.canvas.get_display() + display = pyglet.display.get_display() idx = -1 if self.config.render_window else 0 screen = display.get_screens()[idx] print(display.get_screens()) diff --git a/trap/base.py b/trap/base.py index 710d863..fa26c42 100644 --- a/trap/base.py +++ b/trap/base.py @@ -25,6 +25,10 @@ from urllib.parse import urlparse logger = logging.getLogger('trap.base') class UrlOrPath(): + """ + Some video sources are on a path (files), others a url (some cameras). + Provide some utilities to easily deal with either. + """ def __init__(self, string): self.url = urlparse(str(string)) diff --git a/trap/config.py b/trap/config.py index ac21f2e..2ea4157 100644 --- a/trap/config.py +++ b/trap/config.py @@ -344,6 +344,9 @@ render_parser.add_argument("--render-window", render_parser.add_argument("--render-animation", help="Render animation (pyglet)", action='store_true') +render_parser.add_argument("--render-laser", + help="Render laser (Helios DAC)", + action='store_true') render_parser.add_argument("--render-debug-shapes", help="Lines and points for debugging/mapping", action='store_true') diff --git a/trap/helios.py b/trap/helios.py new file mode 100644 index 0000000..3f6e8b1 --- /dev/null +++ b/trap/helios.py @@ -0,0 +1,619 @@ +# code by phar: https://github.com/phar/heliospy + +import usb.core +import usb.util +import struct +import time +import queue +from trap.hersey import * +from threading import Thread +import matplotlib.pyplot as plt +import numpy as np + + +HELIOS_VID = 0x1209 +HELIOS_PID = 0xE500 +EP_BULK_OUT = 0x02 +EP_BULK_IN = 0x81 +EP_INT_OUT = 0x06 +EP_INT_IN = 0x83 + +INTERFACE_INT = 0 +INTERFACE_BULK = 1 +INTERFACE_ISO = 2 + +HELIOS_MAX_POINTS = 0x1000 +HELIOS_MAX_RATE = 0xFFFF +HELIOS_MIN_RATE = 7 + +HELIOS_SUCCESS = 1 + +# Functions return negative values if something went wrong +# Attempted to perform an action before calling OpenDevices() +HELIOS_ERROR_NOT_INITIALIZED =-1 +# Attempted to perform an action with an invalid device number +HELIOS_ERROR_INVALID_DEVNUM = -2 +# WriteFrame() called with null pointer to points +HELIOS_ERROR_NULL_POINTS = -3 +# WriteFrame() called with a frame containing too many points +HELIOS_ERROR_TOO_MANY_POINTS = -4 +# WriteFrame() called with pps higher than maximum allowed +HELIOS_ERROR_PPS_TOO_HIGH = -5 +# WriteFrame() called with pps lower than minimum allowed +HELIOS_ERROR_PPS_TOO_LOW = -6 + +# Errors from the HeliosDacDevice class begin at -1000 +# Attempted to perform an operation on a closed DAC device +HELIOS_ERROR_DEVICE_CLOSED = -1000 +# Attempted to send a new frame with HELIOS_FLAGS_DONT_BLOCK before previous DoFrame() completed +HELIOS_ERROR_DEVICE_FRAME_READY = -1001 +#/ Operation failed because SendControl() failed (if operation failed because of libusb_interrupt_transfer failure, the error code will be a libusb error instead) +HELIOS_ERROR_DEVICE_SEND_CONTROL = -1002 +# Received an unexpected result from a call to SendControl() +HELIOS_ERROR_DEVICE_RESULT = -1003 +# Attempted to call SendControl() with a null buffer pointer +HELIOS_ERROR_DEVICE_NULL_BUFFER = -1004 +# Attempted to call SendControl() with a control signal that is too long +HELIOS_ERROR_DEVICE_SIGNAL_TOO_LONG = -1005 + +HELIOS_ERROR_LIBUSB_BASE = -5000 + +HELIOS_FLAGS_DEFAULT = 0 +HELIOS_FLAGS_START_IMMEDIATELY = (1 << 0) +HELIOS_FLAGS_SINGLE_MODE = (1 << 1) +HELIOS_FLAGS_DONT_BLOCK = (1 << 2) + + +HELIOS_CMD_STOP =0x0001 +HELIOS_CMD_SHUTTER =0x0002 +HELIOS_CMD_GET_STATUS =0x0003 +HELIOS_GET_FWVERSION =0x0004 +HELIOS_CMD_GET_NAME =0x0005 +HELIOS_CMD_SET_NAME =0x0006 +HELIOS_SET_SDK_VERSION =0x0007 +HELIOS_CMD_ERASE_FIRMWARE =0x00de + +HELIOS_SDK_VERSION = 6 + +class HeliosPoint(): + def __init__(self,x,y,c = 0xff0000,i= 255,blank=False): + self.x = x + self.y = y + self.c = 0x010203 + self.i = i + self.blank = blank + + def __str__(self): + return "HeleiosPoint(%d, %d,0x%0x,%d,%d)" % (self.x, self.y, self.c,self.i, self.blank) + +class HeliosDAC(): + def __init__(self,queuethread=True, debug=0): + self.debug=debug + self.closed = 1 + self.frameReady = 0 + self.framebuffer = "" + self.threadqueue = queue.Queue(maxsize=20) + self.nextframebuffer = "" + self.adcbits = 12 + self.dev = usb.core.find(idVendor=HELIOS_VID, idProduct=HELIOS_PID) + self.cfg = self.dev.get_active_configuration() + self.intf = self.cfg[(0,1,2)] + self.dev.reset() + self.palette = [( 0, 0, 0 ), # Black/blanked (fixed) + ( 255, 255, 255 ), # White (fixed) + ( 255, 0, 0 ), # Red (fixed) + ( 255, 255, 0 ), # Yellow (fixed) + ( 0, 255, 0 ), # Green (fixed) + ( 0, 255, 255 ), # Cyan (fixed) + ( 0, 0, 255 ), # Blue (fixed) + ( 255, 0, 255 ), # Magenta (fixed) + ( 255, 128, 128 ), # Light red + ( 255, 140, 128 ), + ( 255, 151, 128 ), + ( 255, 163, 128 ), + ( 255, 174, 128 ), + ( 255, 186, 128 ), + ( 255, 197, 128 ), + ( 255, 209, 128 ), + ( 255, 220, 128 ), + ( 255, 232, 128 ), + ( 255, 243, 128 ), + ( 255, 255, 128 ), # Light yellow + ( 243, 255, 128 ), + ( 232, 255, 128 ), + ( 220, 255, 128 ), + ( 209, 255, 128 ), + ( 197, 255, 128 ), + ( 186, 255, 128 ), + ( 174, 255, 128 ), + ( 163, 255, 128 ), + ( 151, 255, 128 ), + ( 140, 255, 128 ), + ( 128, 255, 128 ), # Light green + ( 128, 255, 140 ), + ( 128, 255, 151 ), + ( 128, 255, 163 ), + ( 128, 255, 174 ), + ( 128, 255, 186 ), + ( 128, 255, 197 ), + ( 128, 255, 209 ), + ( 128, 255, 220 ), + ( 128, 255, 232 ), + ( 128, 255, 243 ), + ( 128, 255, 255 ), # Light cyan + ( 128, 243, 255 ), + ( 128, 232, 255 ), + ( 128, 220, 255 ), + ( 128, 209, 255 ), + ( 128, 197, 255 ), + ( 128, 186, 255 ), + ( 128, 174, 255 ), + ( 128, 163, 255 ), + ( 128, 151, 255 ), + ( 128, 140, 255 ), + ( 128, 128, 255 ), # Light blue + ( 140, 128, 255 ), + ( 151, 128, 255 ), + ( 163, 128, 255 ), + ( 174, 128, 255 ), + ( 186, 128, 255 ), + ( 197, 128, 255 ), + ( 209, 128, 255 ), + ( 220, 128, 255 ), + ( 232, 128, 255 ), + ( 243, 128, 255 ), + ( 255, 128, 255 ), # Light magenta + ( 255, 128, 243 ), + ( 255, 128, 232 ), + ( 255, 128, 220 ), + ( 255, 128, 209 ), + ( 255, 128, 197 ), + ( 255, 128, 186 ), + ( 255, 128, 174 ), + ( 255, 128, 163 ), + ( 255, 128, 151 ), + ( 255, 128, 140 ), + ( 255, 0, 0 ), # Red (cycleable) + ( 255, 23, 0 ), + ( 255, 46, 0 ), + ( 255, 70, 0 ), + ( 255, 93, 0 ), + ( 255, 116, 0 ), + ( 255, 139, 0 ), + ( 255, 162, 0 ), + ( 255, 185, 0 ), + ( 255, 209, 0 ), + ( 255, 232, 0 ), + ( 255, 255, 0 ), #Yellow (cycleable) + ( 232, 255, 0 ), + ( 209, 255, 0 ), + ( 185, 255, 0 ), + ( 162, 255, 0 ), + ( 139, 255, 0 ), + ( 116, 255, 0 ), + ( 93, 255, 0 ), + ( 70, 255, 0 ), + ( 46, 255, 0 ), + ( 23, 255, 0 ), + ( 0, 255, 0 ), # Green (cycleable) + ( 0, 255, 23 ), + ( 0, 255, 46 ), + ( 0, 255, 70 ), + ( 0, 255, 93 ), + ( 0, 255, 116 ), + ( 0, 255, 139 ), + ( 0, 255, 162 ), + ( 0, 255, 185 ), + ( 0, 255, 209 ), + ( 0, 255, 232 ), + ( 0, 255, 255 ), # Cyan (cycleable) + ( 0, 232, 255 ), + ( 0, 209, 255 ), + ( 0, 185, 255 ), + ( 0, 162, 255 ), + ( 0, 139, 255 ), + ( 0, 116, 255 ), + ( 0, 93, 255 ), + ( 0, 70, 255 ), + ( 0, 46, 255 ), + ( 0, 23, 255 ), + ( 0, 0, 255 ), # Blue (cycleable) + ( 23, 0, 255 ), + ( 46, 0, 255 ), + ( 70, 0, 255 ), + ( 93, 0, 255 ), + ( 116, 0, 255 ), + ( 139, 0, 255 ), + ( 162, 0, 255 ), + ( 185, 0, 255 ), + ( 209, 0, 255 ), + ( 232, 0, 255 ), + ( 255, 0, 255 ), # Magenta (cycleable) + ( 255, 0, 232 ), + ( 255, 0, 209 ), + ( 255, 0, 185 ), + ( 255, 0, 162 ), + ( 255, 0, 139 ), + ( 255, 0, 116 ), + ( 255, 0, 93 ), + ( 255, 0, 70 ), + ( 255, 0, 46 ), + ( 255, 0, 23 ), + ( 128, 0, 0 ), # Dark red + ( 128, 12, 0 ), + ( 128, 23, 0 ), + ( 128, 35, 0 ), + ( 128, 47, 0 ), + ( 128, 58, 0 ), + ( 128, 70, 0 ), + ( 128, 81, 0 ), + ( 128, 93, 0 ), + ( 128, 105, 0 ), + ( 128, 116, 0 ), + ( 128, 128, 0 ), # Dark yellow + ( 116, 128, 0 ), + ( 105, 128, 0 ), + ( 93, 128, 0 ), + ( 81, 128, 0 ), + ( 70, 128, 0 ), + ( 58, 128, 0 ), + ( 47, 128, 0 ), + ( 35, 128, 0 ), + ( 23, 128, 0 ), + ( 12, 128, 0 ), + ( 0, 128, 0 ), # Dark green + ( 0, 128, 12 ), + ( 0, 128, 23 ), + ( 0, 128, 35 ), + ( 0, 128, 47 ), + ( 0, 128, 58 ), + ( 0, 128, 70 ), + ( 0, 128, 81 ), + ( 0, 128, 93 ), + ( 0, 128, 105 ), + ( 0, 128, 116 ), + ( 0, 128, 128 ), # Dark cyan + ( 0, 116, 128 ), + ( 0, 105, 128 ), + ( 0, 93, 128 ), + ( 0, 81, 128 ), + ( 0, 70, 128 ), + ( 0, 58, 128 ), + ( 0, 47, 128 ), + ( 0, 35, 128 ), + ( 0, 23, 128 ), + ( 0, 12, 128 ), + ( 0, 0, 128 ), # Dark blue + ( 12, 0, 128 ), + ( 23, 0, 128 ), + ( 35, 0, 128 ), + ( 47, 0, 128 ), + ( 58, 0, 128 ), + ( 70, 0, 128 ), + ( 81, 0, 128 ), + ( 93, 0, 128 ), + ( 105, 0, 128 ), + ( 116, 0, 128 ), + ( 128, 0, 128 ), # Dark magenta + ( 128, 0, 116 ), + ( 128, 0, 105 ), + ( 128, 0, 93 ), + ( 128, 0, 81 ), + ( 128, 0, 70 ), + ( 128, 0, 58 ), + ( 128, 0, 47 ), + ( 128, 0, 35 ), + ( 128, 0, 23 ), + ( 128, 0, 12 ), + ( 255, 192, 192 ), # Very light red + ( 255, 64, 64 ), # Light-medium red + ( 192, 0, 0 ), # Medium-dark red + ( 64, 0, 0 ), # Very dark red + ( 255, 255, 192 ), # Very light yellow + ( 255, 255, 64 ), # Light-medium yellow + ( 192, 192, 0 ), # Medium-dark yellow + ( 64, 64, 0 ), # Very dark yellow + ( 192, 255, 192 ), # Very light green + ( 64, 255, 64 ), # Light-medium green + ( 0, 192, 0 ), # Medium-dark green + ( 0, 64, 0 ), # Very dark green + ( 192, 255, 255 ), # Very light cyan + ( 64, 255, 255 ), # Light-medium cyan + ( 0, 192, 192 ), # Medium-dark cyan + ( 0, 64, 64 ), # Very dark cyan + ( 192, 192, 255 ), # Very light blue + ( 64, 64, 255 ), # Light-medium blue + ( 0, 0, 192 ), # Medium-dark blue + ( 0, 0, 64 ), # Very dark blue + ( 255, 192, 255 ), # Very light magenta + ( 255, 64, 255 ), # Light-medium magenta + ( 192, 0, 192 ), # Medium-dark magenta + ( 64, 0, 64 ), # Very dark magenta + ( 255, 96, 96 ), # Medium skin tone + ( 255, 255, 255 ), # White (cycleable) + ( 245, 245, 245 ), + ( 235, 235, 235 ), + ( 224, 224, 224 ), # Very light gray (7/8 intensity) + ( 213, 213, 213 ), + ( 203, 203, 203 ), + ( 192, 192, 192 ), # Light gray (3/4 intensity) + ( 181, 181, 181 ), + ( 171, 171, 171 ), + ( 160, 160, 160 ), # Medium-light gray (5/8 int.) + ( 149, 149, 149 ), + ( 139, 139, 139 ), + ( 128, 128, 128 ), # Medium gray (1/2 intensity) + ( 117, 117, 117 ), + ( 107, 107, 107 ), + ( 96, 96, 96 ), # Medium-dark gray (3/8 int.) + ( 85, 85, 85 ), + ( 75, 75, 75 ), + ( 64, 64, 64 ), # Dark gray (1/4 intensity) + ( 53, 53, 53 ), + ( 43, 43, 43 ), + ( 32, 32, 32 ), # Very dark gray (1/8 intensity) + ( 21, 21, 21 ), + ( 11, 11, 11 )] # Black + + self.dev.set_interface_altsetting(interface = 0, alternate_setting = 1) + + if self.dev.is_kernel_driver_active(0) is True: + self.dev.detach_kernel_driver(0) + # claim the device + usb.util.claim_interface(self.dev, 0) + + if self.dev is None: + raise ValueError('Device not found') + else: + if self.debug: + print(self.dev) + + try: + transferResult = self.intf[0].read(32,1) + except: + if self.debug: + print("no lingering data") + + if self.debug: + print(self.GetName()) + print(self.getHWVersion()) + self.setSDKVersion() + self.closed = False + if queuethread: + self.runQueueThread() + + def runQueueThread(self): + worker = Thread(target=self.doframe_thread_loop) + worker.setDaemon(True) + worker.start() + + def doframe_thread_loop(self): + while self.closed == 0: + if self.closed: + return; + self.DoFrame(); + + def getHWVersion(self): + self.intf[1].write(struct.pack(" HELIOS_MAX_POINTS): + return HELIOS_ERROR_TOO_MANY_POINTS + + if (pps > HELIOS_MAX_RATE): + return HELIOS_ERROR_PPS_TOO_HIGH + + if (pps < HELIOS_MIN_RATE): + return HELIOS_ERROR_PPS_TOO_LOW + + #this is a bug workaround, the mcu won't correctly receive transfers with these sizes + ppsActual = pps; + numOfPointsActual = len(pntobjlist) + if (((len(pntobjlist)-45) % 64) == 0): + numOfPointsActual-=1 + ppsActual = int((pps * numOfPointsActual / len(pntobjlist) + 0.5)) + + pntobjlist = pntobjlist[:numOfPointsActual] + nextframebuffer = b"" + for pnt in pntobjlist: + a = (pnt.x >> 4) & 0xff + b = ((pnt.x & 0x0F) << 4) | (pnt.y >> 8) + c = pnt.y & 0xFF + if pnt.blank == False: + r = (pnt.c & 0xff0000) >> 16 + g = (pnt.c & 0xff00) >> 8 + b = (pnt.c & 0xff) + i = pnt.i + else: + r = 0 + g = 0 + b = 0 + i = 0 + nextframebuffer += struct.pack("BBBBBBB", a,b,c,r,g,b,i) + nextframebuffer += struct.pack("BBBBB", (ppsActual & 0xFF),(ppsActual >> 8) ,(len(pntobjlist) & 0xFF),(len(pntobjlist) >> 8),flags) + self.threadqueue.put(nextframebuffer) + + def DoFrame(self): + if (self.closed): + return HELIOS_ERROR_DEVICE_CLOSED; + self.nextframebuffer = self.threadqueue.get(block=True) + self.intf[3].write(self.nextframebuffer) + t = time.time() + while(self.getStatus()[1] == 0): #wait for the laser + pass + return self.getStatus() + + def GetName(self): + self.SendControl(struct.pack(" 32): + return HELIOS_ERROR_DEVICE_SIGNAL_TOO_LONG; + self.intf[1].write(buffer) + + def stop(self): + self.SendControl(struct.pack(" 0: + for i in range(rcnt): + if format in [0,1,4,5]: + if format == 0: + fmt = ">hhhBB" + (x,y,z,status,cindex) = struct.unpack(fmt,f.read(struct.calcsize(fmt))) + + elif format == 1: + fmt = ">hhBB" + (x,y,status,cindex) = struct.unpack(fmt,f.read(struct.calcsize(fmt))) + + elif format == 4: + (x,y,z,status,red,green,blue) = struct.unpack(fmt,f.read(struct.calcsize(fmt))) + + elif format == 5: + fmt = ">hhhBBBB" + (x,y,status,red,green,blue) = struct.unpack(fmt,f.read(struct.calcsize(fmt))) + + blank = (status & 0x40) > 0 + lastpoint = (status & 0x80) > 0 + lessadcbits = (16 - self.adcbits) + x = int((x >> lessadcbits) * xscale) + y = int((y >> lessadcbits) * yscale) + pointlist.append(HeliosPoint(x,y,self.palette[cindex],blank=blank)) + + elif format == 2: + fmt = ">BBB" + (r,g,b) = struct.unpack(fmt,f.read(struct.calcsize(fmt))) + palette.append((r<<16) | (g<<8) | b) + + if format == 2: + frames.append((("palette",fname,cname, num),palette)) + else: + frames.append((("frame",fname,cname,num),pointlist)) + + else: + moreframes = 0 + else: + moreframes = 0 + + return frames + + def plot(self, pntlist): + fig, ax = plt.subplots() # Create a figure containing a single axes. + xlst = [] + ylst = [] + for p in pntlist: + if p.blank == False: + xlst.append(p.x) + ylst.append(p.y) + ax.plot(xlst,ylst) + plt.show() + + + + +if __name__ == "__main__": + a = HeliosDAC() + + a.runQueueThread() + +# cal = a.generateText("hello World", 20,20,scale=10) +## print(cal) +# a.plot(cal) +# +# while(1): +# a.newFrame(2000,cal) +# a.DoFrame() + + + cal = a.loadILDfile("astroid.ild") + while(1): + for (t,n1,n2,c),f in cal: + print("playing %s,%s, %d" % (n1,n2,c)) + a.newFrame(5000,f) +# a.DoFrame() + +# a.plot(f) + + +# while(1): +## a.newFrame(1000,[HeliosPoint(16000,16000)]) +# a.newFrame(100,[HeliosPoint(16000-2500,16000),HeliosPoint(16000,16000),HeliosPoint(16000+2500,16000),HeliosPoint(16000,16000),HeliosPoint(16000,16000+2500),HeliosPoint(16000,16000),HeliosPoint(16000,16000-2500),HeliosPoint(16000,16000)]) +# a.DoFrame() + + +# while(1): +# a.newFrame(1000,[HeliosPoint(0,200), +# HeliosPoint(200,200), +# HeliosPoint(200,0), +# HeliosPoint(0,0), +# ]) +# a.DoFrame() + diff --git a/trap/hersey.py b/trap/hersey.py new file mode 100644 index 0000000..d2e979f --- /dev/null +++ b/trap/hersey.py @@ -0,0 +1,196 @@ +# part of heliospy, see helios.py + +HERSHEY_HEIGHT = 28 +HERSHEY_WIDTH = 28 +HERSHEY_FONT = [ + #Ascii 32 + [(0,16),(-1, -1)], + #Ascii 33 + [(8,10),(5, 21),(5, 7),(-1, -1),(5, 2),(4, 1),(5, 0),(6, 1),(5, 2),(-1, -1)], + #Ascii 34 + [(5,16),(4, 21),(4, 14),(-1, -1),(12, 21),(12, 14),(-1, -1)], + #Ascii 35 + [(11,21),(11, 25),(4, -7),(-1, -1),(17, 25),(10, -7),(-1, -1),(4, 12),(18, 12),(-1, -1),(3, 6),(17, 6),(-1, -1)], + #Ascii 36 + [(26,20),(8, 25),(8, -4),(-1, -1),(12, 25),(12, -4),(-1, -1),(17, 18),(15, 20),(12, 21),(8, 21),(5, 20),(3, 18),(3, 16),(4, 14),(5, 13),(7, 12),(13, 10),(15, 9),(16, 8),(17, 6),(17, 3),(15, 1),(12, 0),(8, 0),(5, 1),(3, 3),(-1, -1)], + #Ascii 37 + [(31,24),(21, 21),(3, 0),(-1, -1),(8, 21),(10, 19),(10, 17),(9, 15),(7, 14),(5, 14),(3, 16),(3, 18),(4, 20),(6, 21),(8, 21),(10, 20),(13, 19),(16, 19),(19, 20),(21, 21),(-1, -1),(17, 7),(15, 6),(14, 4),(14, 2),(16, 0),(18, 0),(20, 1),(21, 3),(21, 5),(19, 7),(17, 7),(-1, -1)], + #Ascii 38 + [(34,26),(23, 12),(23, 13),(22, 14),(21, 14),(20, 13),(19, 11),(17, 6),(15, 3),(13, 1),(11, 0),(7, 0),(5, 1),(4, 2),(3, 4),(3, 6),(4, 8),(5, 9),(12, 13),(13, 14),(14, 16),(14, 18),(13, 20),(11, 21),(9, 20),(8, 18),(8, 16),(9, 13),(11, 10),(16, 3),(18, 1),(20, 0),(22, 0),(23, 1),(23, 2),(-1, -1)], + #Ascii 39 + [(7,10),(5, 19),(4, 20),(5, 21),(6, 20),(6, 18),(5, 16),(4, 15),(-1, -1)], + #Ascii 40 + [(10,14),(11, 25),(9, 23),(7, 20),(5, 16),(4, 11),(4, 7),(5, 2),(7, -2),(9, -5),(11, -7),(-1, -1)], + #Ascii 41 + [(10,14),(3, 25),(5, 23),(7, 20),(9, 16),(10, 11),(10, 7),(9, 2),(7, -2),(5, -5),(3, -7),(-1, -1)], + #Ascii 42 + [(8,16),(8, 21),(8, 9),(-1, -1),(3, 18),(13, 12),(-1, -1),(13, 18),(3, 12),(-1, -1)], + #Ascii 43 + [(5,26),(13, 18),(13, 0),(-1, -1),(4, 9),(22, 9),(-1, -1)], + #Ascii 44 + [(8,10),(6, 1),(5, 0),(4, 1),(5, 2),(6, 1),(6, -1),(5, -3),(4, -4),(-1, -1)], + #Ascii 45 + [(2,26),(4, 9),(22, 9),(-1, -1)], + #Ascii 46 + [(5,10),(5, 2),(4, 1),(5, 0),(6, 1),(5, 2),(-1, -1)], + #Ascii 47` + [(2,22),(20, 25),(2, -7),(-1, -1)], + #Ascii 48 + [(17,20),(9, 21),(6, 20),(4, 17),(3, 12),(3, 9),(4, 4),(6, 1),(9, 0),(11, 0),(14, 1),(16, 4),(17, 9),(17, 12),(16, 17),(14, 20),(11, 21),(9, 21),(-1, -1)], + #Ascii 49 + [(4,20),(6, 17),(8, 18),(11, 21),(11, 0),(-1, -1)], + #Ascii 50 + [(14,20),(4, 16),(4, 17),(5, 19),(6, 20),(8, 21),(12, 21),(14, 20),(15, 19),(16, 17),(16, 15),(15, 13),(13, 10),(3, 0),(17, 0),(-1, -1)], + #Ascii 51 + [(15,20),(5, 21),(16, 21),(10, 13),(13, 13),(15, 12),(16, 11),(17, 8),(17, 6),(16, 3),(14, 1),(11, 0),(8, 0),(5, 1),(4, 2),(3, 4),(-1, -1)], + #Ascii 52 + [(6,20),(13, 21),(3, 7),(18, 7),(-1, -1),(13, 21),(13, 0),(-1, -1)], + #Ascii 53 + [(17,20),(15, 21),(5, 21),(4, 12),(5, 13),(8, 14),(11, 14),(14, 13),(16, 11),(17, 8),(17, 6),(16, 3),(14, 1),(11, 0),(8, 0),(5, 1),(4, 2),(3, 4),(-1, -1)], + #Ascii 54 + [(23,20),(16, 18),(15, 20),(12, 21),(10, 21),(7, 20),(5, 17),(4, 12),(4, 7),(5, 3),(7, 1),(10, 0),(11, 0),(14, 1),(16, 3),(17, 6),(17, 7),(16, 10),(14, 12),(11, 13),(10, 13),(7, 12),(5, 10),(4, 7),(-1, -1)], + #Ascii 55 + [(5,20),(17, 21),(7, 0),(-1, -1),(3, 21),(17, 21),(-1, -1)], + #Ascii 56 + [(29,20),(8, 21),(5, 20),(4, 18),(4, 16),(5, 14),(7, 13),(11, 12),(14, 11),(16, 9),(17, 7),(17, 4),(16, 2),(15, 1),(12, 0),(8, 0),(5, 1),(4, 2),(3, 4),(3, 7),(4, 9),(6, 11),(9, 12),(13, 13),(15, 14),(16, 16),(16, 18),(15, 20),(12, 21),(8, 21),(-1, -1)], + #Ascii 57 + [(23,20),(16, 14),(15, 11),(13, 9),(10, 8),(9, 8),(6, 9),(4, 11),(3, 14),(3, 15),(4, 18),(6, 20),(9, 21),(10, 21),(13, 20),(15, 18),(16, 14),(16, 9),(15, 4),(13, 1),(10, 0),(8, 0),(5, 1),(4, 3),(-1, -1)], + #Ascii 58 + [(11,10),(5, 14),(4, 13),(5, 12),(6, 13),(5, 14),(-1, -1),(5, 2),(4, 1),(5, 0),(6, 1),(5, 2),(-1, -1)], + #Ascii 59 + [(14,10),(5, 14),(4, 13),(5, 12),(6, 13),(5, 14),(-1, -1),(6, 1),(5, 0),(4, 1),(5, 2),(6, 1),(6, -1),(5, -3),(4, -4),(-1, -1)], + #Ascii 60 + [(3,24),(20, 18),(4, 9),(20, 0),(-1, -1)], + #Ascii 61 + [(5,26),(4, 12),(22, 12),(-1, -1),(4, 6),(22, 6),(-1, -1)], + #Ascii 62 + [(3,24),(4, 18),(20, 9),(4, 0),(-1, -1)], + #Ascii 63 + [(20,18),(3, 16),(3, 17),(4, 19),(5, 20),(7, 21),(11, 21),(13, 20),(14, 19),(15, 17),(15, 15),(14, 13),(13, 12),(9, 10),(9, 7),(-1, -1),(9, 2),(8, 1),(9, 0),(10, 1),(9, 2),(-1, -1)], + #Ascii 64 + [(55,27),(18, 13),(17, 15),(15, 16),(12, 16),(10, 15),(9, 14),(8, 11),(8, 8),(9, 6),(11, 5),(14, 5),(16, 6),(17, 8),(-1, -1),(12, 16),(10, 14),(9, 11),(9, 8),(10, 6),(11, 5),(-1, -1),(18, 16),(17, 8),(17, 6),(19, 5),(21, 5),(23, 7),(24, 10),(24, 12),(23, 15),(22, 17),(20, 19),(18, 20),(15, 21),(12, 21),(9, 20),(7, 19),(5, 17),(4, 15),(3, 12),(3, 9),(4, 6),(5, 4),(7, 2),(9, 1),(12, 0),(15, 0),(18, 1),(20, 2),(21, 3),(-1, -1),(19, 16),(18, 8),(18, 6),(19, 5),(8, 18),(-1,-1)], + #Ascii 65 + [(8,18), (9,21), (1, 0),(-1,-1), (9,21),(17, 0),(-1,-1),( 4, 7),(14, 7),(-1,-1)], + #Ascii 66 + [(23,21),(4, 21),(4, 0),(-1, -1),(4, 21),(13, 21),(16, 20),(17, 19),(18, 17),(18, 15),(17, 13),(16, 12),(13, 11),(-1, -1),(4, 11),(13, 11),(16, 10),(17, 9),(18, 7),(18, 4),(17, 2),(16, 1),(13, 0),(4, 0),(-1, -1)], + #Ascii 67 + [(18,21),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(-1, -1)], + #Ascii 68 + [(15,21),(4, 21),(4, 0),(-1, -1),(4, 21),(11, 21),(14, 20),(16, 18),(17, 16),(18, 13),(18, 8),(17, 5),(16, 3),(14, 1),(11, 0),(4, 0),(-1, -1)], + #Ascii 69 + [(11,19),(4, 21),(4, 0),(-1, -1),(4, 21),(17, 21),(-1, -1),(4, 11),(12, 11),(-1, -1),(4, 0),(17, 0),(-1, -1)], + #Ascii 70 + [(8,18),(4, 21),(4, 0),(-1, -1),(4, 21),(17, 21),(-1, -1),(4, 11),(12, 11),(-1, -1)], + #Ascii 71 + [(22,21),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(18, 8),(-1, -1),(13, 8),(18, 8),(-1, -1)], + #Ascii 72 + [(8,22),(4, 21),(4, 0),(-1, -1),(18, 21),(18, 0),(-1, -1),(4, 11),(18, 11),(-1, -1)], + #Ascii 73 + [(2,8),(4, 21),(4, 0),(-1, -1)], + #Ascii 74 + [(10,16),(12, 21),(12, 5),(11, 2),(10, 1),(8, 0),(6, 0),(4, 1),(3, 2),(2, 5),(2, 7),(-1, -1)], + #Ascii 75 + [(8,21),(4, 21),(4, 0),(-1, -1),(18, 21),(4, 7),(-1, -1),(9, 12),(18, 0),(-1, -1)], + #Ascii 76 + [(5,17),(4, 21),(4, 0),(-1, -1),(4, 0),(16, 0),(-1, -1)], + #Ascii 77 + [(11,24),(4, 21),(4, 0),(-1, -1),(4, 21),(12, 0),(-1, -1),(20, 21),(12, 0),(-1, -1),(20, 21),(20, 0),(-1, -1)], + #Ascii 78 + [(8,22),(4, 21),(4, 0),(-1, -1),(4, 21),(18, 0),(-1, -1),(18, 21),(18, 0),(-1, -1)], + #Ascii 79 + [(21,22),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(19, 8),(19, 13),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(-1, -1)], + #Ascii 80 + [(13,21),(4, 21),(4, 0),(-1, -1),(4, 21),(13, 21),(16, 20),(17, 19),(18, 17),(18, 14),(17, 12),(16, 11),(13, 10),(4, 10),(-1, -1)], + #Ascii 81 + [(24,22),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(19, 8),(19, 13),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(-1, -1),(12, 4),(18, -2),(-1, -1)], + #Ascii 82 + [(16,21),(4, 21),(4, 0),(-1, -1),(4, 21),(13, 21),(16, 20),(17, 19),(18, 17),(18, 15),(17, 13),(16, 12),(13, 11),(4, 11),(-1, -1),(11, 11),(18, 0),(-1, -1)], + #Ascii 83 + [(20,20),(17, 18),(15, 20),(12, 21),(8, 21),(5, 20),(3, 18),(3, 16),(4, 14),(5, 13),(7, 12),(13, 10),(15, 9),(16, 8),(17, 6),(17, 3),(15, 1),(12, 0),(8, 0),(5, 1),(3, 3),(-1, -1)], + #Ascii 8,4 + [(5,16),(8, 21),(8, 0),(-1, -1),(1, 21),(15, 21),(-1, -1)], + #Ascii 85 + [(10,22),(4, 21),(4, 6),(5, 3),(7, 1),(10, 0),(12, 0),(15, 1),(17, 3),(18, 6),(18, 21),(-1, -1)], + #Ascii 86 + [(5,18),(1, 21),(9, 0),(-1, -1),(17, 21),(9, 0),(-1, -1)], + #Ascii 87 + [(11,24),(2, 21),(7, 0),(-1, -1),(12, 21),(7, 0),(-1, -1),(12, 21),(17, 0),(-1, -1),(22, 21),(17, 0),(-1, -1)], + #Ascii 88 + [(5,20),(3, 21),(17, 0),(-1, -1),(17, 21),(3, 0),(-1, -1)], + #Ascii 89 + [(6,18),(1, 21),(9, 11),(9, 0),(-1, -1),(17, 21),(9, 11),(-1, -1)], + #Ascii 90 + [(8,20),(17, 21),(3, 0),(-1, -1),(3, 21),(17, 21),(-1, -1),(3, 0),(17, 0),(-1, -1)], + #Ascii 91 + [(11,14),(4, 25),(4, -7),(-1, -1),(5, 25),(5, -7),(-1, -1),(4, 25),(11, 25),(-1, -1),(4, -7),(11, -7),(-1, -1)], + #Ascii 92 + [(2,14),(0, 21),(14, -3),(-1, -1)], + #Ascii 93 + [(11,14),(9, 25),(9, -7),(-1, -1),(10, 25),(10, -7),(-1, -1),(3, 25),(10, 25),(-1, -1),(3, -7),(10, -7),(-1, -1)], + #Ascii 94 + [(10,16),(6, 15),(8, 18),(10, 15),(-1, -1),(3, 12),(8, 17),(13, 12),(-1, -1),(8, 17),(8, 0),(-1, -1)], + #Ascii 95 + [(2,16),(0, -2),(16, -2),(-1, -1)], + #Ascii 96 + [(7,10),(6, 21),(5, 20),(4, 18),(4, 16),(5, 15),(6, 16),(5, 17),(-1, -1)], + #Ascii 97 + [(17,19),(15, 14),(15, 0),(-1, -1),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)], + #Ascii 98 + [(17,19),(4, 21),(4, 0),(-1, -1),(4, 11),(6, 13),(8, 14),(11, 14),(13, 13),(15, 11),(16, 8),(16, 6),(15, 3),(13, 1),(11, 0),(8, 0),(6, 1),(4, 3),(-1, -1)], + #Ascii 99 + [(14,18),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)], + #Ascii 100 + [(17,19),(15, 21),(15, 0),(-1, -1),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)], + #Ascii 101 + [(17,18),(3, 8),(15, 8),(15, 10),(14, 12),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)], + #Ascii 102 + [(8,12),(10, 21),(8, 21),(6, 20),(5, 17),(5, 0),(-1, -1),(2, 14),(9, 14),(-1, -1)], + #Ascii 103 + [(22,19),(15, 14),(15, -2),(14, -5),(13, -6),(11, -7),(8, -7),(6, -6),(-1, -1),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)], + #Ascii 104 + [(10,19),(4, 21),(4, 0),(-1, -1),(4, 10),(7, 13),(9, 14),(12, 14),(14, 13),(15, 10),(15, 0),(-1, -1)], + #Ascii 105 + [(8,8),(3, 21),(4, 20),(5, 21),(4, 22),(3, 21),(-1, -1),(4, 14),(4, 0),(-1, -1)], + #Ascii 106 + [(11,10),(5, 21),(6, 20),(7, 21),(6, 22),(5, 21),(-1, -1),(6, 14),(6, -3),(5, -6),(3, -7),(1, -7),(-1, -1)], + #Ascii 107 + [(8,17),(4, 21),(4, 0),(-1, -1),(14, 14),(4, 4),(-1, -1),(8, 8),(15, 0),(-1, -1)], + #Ascii 108 + [(2,8),(4, 21),(4, 0),(-1, -1),(18, 30),(-1,-1)], + #Ascii 109 + [(18,30), (4,14),(4, 0),(-1,-1),(4,10),(7,13),(9,14),(12,14),(14,13),(15,10),(15, 0),(-1,-1),(15,10),(18,13),(20,14),(23,14),(25,13),(26,10),(26, 0),(-1,-1)], + #Ascii 110 + [(10,19),(4, 14),(4, 0),(-1, -1),(4, 10),(7, 13),(9, 14),(12, 14),(14, 13),(15, 10),(15, 0),(-1, -1),(17, 19),(-1,-1)], + #Ascii 111 */ + [(17,19),(8,14), (6,13), (4,11), (3, 8), (3, 6), (4, 3), (6, 1), (8, 0),(11, 0),(13, 1),(15, 3),(16,6),(16, 8),(15,11),(13,13),(11,14), (8,14), (-1,-1),(-1,-1)], + #Ascii 112 + [(17,19),(4, 14),(4, -7),(-1, -1),(4, 11),(6, 13),(8, 14),(11, 14),(13, 13),(15, 11),(16, 8),(16, 6),(15, 3),(13, 1),(11, 0),(8, 0),(6, 1),(4, 3),(-1, -1),(17, 19),(-1,-1)], + #Ascii 113, + [(17,19), (15,14),(15,-7),(-1,-1),(15,11),(13,13),(11,14), (8,14), (6,13), (4,11), (3, 8), (3, 6), (4,3), (6, 1), (8, 0),(11, 0),(13, 1),(15, 3), (-1,-1), (-1,-1)], + #Ascii 114 + [(8,13),(4, 14),(4, 0),(-1, -1),(4, 8),(5, 11),(7, 13),(9, 14),(12, 14),(-1, -1)], + #Ascii 115 + [(17,17),(14, 11),(13, 13),(10, 14),(7, 14),(4, 13),(3, 11),(4, 9),(6, 8),(11, 7),(13, 6),(14, 4),(14, 3),(13, 1),(10, 0),(7, 0),(4, 1),(3, 3),(-1, -1)], + #Ascii 116 + [(8,12),(5, 21),(5, 4),(6, 1),(8, 0),(10, 0),(-1, -1),(2, 14),(9, 14),(-1, -1)], + #Ascii 117 + [(10,19),(4, 14),(4, 4),(5, 1),(7, 0),(10, 0),(12, 1),(15, 4),(-1, -1),(15, 14),(15, 0),(-1, -1)], + #Ascii 118 + [(5,16),(2, 14),(8, 0),(-1, -1),(14, 14),(8, 0),(-1, -1)], + #Ascii 119 + [(11,22),(3, 14),(7, 0),(-1, -1),(11, 14),(7, 0),(-1, -1),(11, 14),(15, 0),(-1, -1),(19, 14),(15, 0),(-1, -1)], + #Ascii 120 + [(5,17),(3, 14),(14, 0),(-1, -1),(14, 14),(3, 0),(-1, -1)], + #Ascii 121 + [(9,16),(2, 14),(8, 0),(-1, -1),(14, 14),(8, 0),(6, -4),(4, -6),(2, -7),(1, -7),(-1, -1)], + #Ascii 122 + [(8,17),(14, 14),(3, 0),(-1, -1),(3, 14),(14, 14),(-1, -1),(3, 0),(14, 0),(-1, -1)], + #Ascii 123 + [(39,14),(9, 25),(7, 24),(6, 23),(5, 21),(5, 19),(6, 17),(7, 16),(8, 14),(8, 12),(6, 10),(-1, -1),(7, 24),(6, 22),(6, 20),(7, 18),(8, 17),(9, 15),(9, 13),(8, 11),(4, 9),(8, 7),(9, 5),(9, 3),(8, 1),(7, 0),(6, -2),(6, -4),(7, -6),(-1, -1),(6, 8),(8, 6),(8, 4),(7, 2),(6, 1),(5, -1),(5, -3),(6, -5),(7, -6),(9, -7),(-1, -1)], + #Ascii 124 + [(2,8),(4, 25),(4, -7),(-1, -1)], + #Ascii 125 + [(39,14),(5, 25),(7, 24),(8, 23),(9, 21),(9, 19),(8, 17),(7, 16),(6, 14),(6, 12),(8, 10),(-1, -1),(7, 24),(8, 22),(8, 20),(7, 18),(6, 17),(5, 15),(5, 13),(6, 11),(10, 9),(6, 7),(5, 5),(5, 3),(6, 1),(7, 0),(8, -2),(8, -4),(7, -6),(-1, -1),(8, 8),(6, 6),(6, 4),(7, 2),(8, 1),(9, -1),(9, -3),(8, -5),(7, -6),(5, -7),(-1, -1)], + #Ascii 126 + [(23,24),(3, 6),(3, 8),(4, 11),(6, 12),(8, 12),(10, 11),(14, 8),(16, 7),(18, 7),(20, 8),(21, 10),(-1, -1),(3, 8),(4, 10),(6, 11),(8, 11),(10, 10),(14, 7),(16, 6),(18, 6),(20, 7),(21, 10),(21, 12),(-1, -1)]] + diff --git a/trap/laser_renderer.py b/trap/laser_renderer.py new file mode 100644 index 0000000..1461239 --- /dev/null +++ b/trap/laser_renderer.py @@ -0,0 +1,285 @@ +# used for "Forward Referencing of type annotations" +from __future__ import annotations + +import time +import ffmpeg +from argparse import Namespace +import datetime +import logging +from multiprocessing import Event +from multiprocessing.synchronize import Event as BaseEvent +import cv2 +import numpy as np +import json +import pyglet +import pyglet.event +import zmq +import tempfile +from pathlib import Path +import shutil +import math +from typing import Dict, Iterable, Optional + + +from pyglet import shapes +from PIL import Image + +from trap.frame_emitter import DetectionState, Frame, Track, Camera +from trap.helios import HeliosDAC, HeliosPoint +from trap.preview_renderer import FrameWriter +from trap.tools import draw_track, draw_track_predictions, draw_track_projected, draw_trackjectron_history, to_point, track_predictions_to_lines +from trap.utils import convert_world_points_to_img_points, convert_world_space_to_img_space + + + +logger = logging.getLogger("trap.laser_renderer") + +class LaserRenderer: + def __init__(self, config: Namespace, is_running: BaseEvent): + self.config = config + self.is_running = is_running + + context = zmq.Context() + self.prediction_sock = context.socket(zmq.SUB) + self.prediction_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!! + self.prediction_sock.setsockopt(zmq.SUBSCRIBE, b'') + # self.prediction_sock.connect(config.zmq_prediction_addr if not self.config.bypass_prediction else config.zmq_trajectory_addr) + self.prediction_sock.connect(config.zmq_prediction_addr) + + self.tracker_sock = context.socket(zmq.SUB) + self.tracker_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!! + self.tracker_sock.setsockopt(zmq.SUBSCRIBE, b'') + self.tracker_sock.connect(config.zmq_trajectory_addr) + + self.H = self.config.H + + self.inv_H = np.linalg.pinv(self.H) + + # TODO: get FPS from frame_emitter + # self.out = cv2.VideoWriter(str(filename), fourcc, 23.97, (1280,720)) + self.fps = 60 + self.frame_size = (self.config.camera.w,self.config.camera.h) + + self.first_time: float|None = None + self.frame: Frame|None= None + self.tracker_frame: Frame|None = None + self.prediction_frame: Frame|None = None + + self.tracks: Dict[str, Track] = {} + self.predictions: Dict[str, Track] = {} + + + self.dac = HeliosDAC(debug=False) + logger.info(f"{self.dac.dev}") + logger.info(f"{self.dac.GetName()}") + logger.info(f"{self.dac.getHWVersion()}") + + logger.info(f"Helios version: {self.dac.getHWVersion()}") + + + # self.init_shapes() + + # self.init_labels() + + + def check_frames(self, dt): + new_tracks = False + try: + self.frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK) + if not self.first_time: + self.first_time = self.frame.time + img = cv2.GaussianBlur(self.frame.img, (15, 15), 0) + img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0) + img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes()) + # don't draw in batch, so that it is the background + self.video_sprite = pyglet.sprite.Sprite(img=img, batch=self.batch_bg) + self.video_sprite.opacity = 100 + except zmq.ZMQError as e: + # idx = frame.index if frame else "NONE" + # logger.debug(f"reuse video frame {idx}") + pass + try: + self.prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK) + new_tracks = True + except zmq.ZMQError as e: + pass + try: + self.tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK) + new_tracks = True + except zmq.ZMQError as e: + pass + + def run(self, timer_counter): + frame = None + prediction_frame = None + tracker_frame = None + + i=0 + first_time = None + + while self.is_running.is_set(): + i+=1 + with timer_counter.get_lock(): + timer_counter.value+=1 + + try: + prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK) + for track_id, track in prediction_frame.tracks.items(): + prediction_id = f"{track_id}-{track.history[-1].frame_nr}" + self.predictions[prediction_id] = track + except zmq.ZMQError as e: + logger.debug(f'reuse prediction') + + try: + tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK) + + for track_id, track in tracker_frame.tracks.items(): + self.tracks[track_id] = track + except zmq.ZMQError as e: + logger.debug(f'reuse tracks') + + if tracker_frame is None: + # might need to wait a few iterations before first frame comes available + time.sleep(.1) + continue + + if first_time is None: + first_time = tracker_frame.time + + pointlist = render_frame_to_dac(self.dac, tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.config.render_clusters) + + self.dac.newFrame(50000, pointlist) + + + # clear out old tracks & predictions: + + for track_id, track in list(self.tracks.items()): + # TODO)) Migrate to using time() instead of framenr, to detach the two + if get_animation_position(track, tracker_frame) == 1: + self.tracks.pop(track_id) + for prediction_id, track in list(self.predictions.items()): + if get_animation_position(track, tracker_frame) == 1: + self.predictions.pop(prediction_id) + + logger.info('Stopping') + + # if i>2: + + + + logger.info('stopped') +# colorset = itertools.product([0,255], repeat=3) # but remove white +# colorset = [(0, 0, 0), +# (0, 0, 255), +# (0, 255, 0), +# (0, 255, 255), +# (255, 0, 0), +# (255, 0, 255), +# (255, 255, 0) +# ] +colorset = [ + (255,255,100), + (255,100,255), + (100,255,255), + ] +# colorset = [ +# (0,0,0), +# ] + +def get_animation_position(track: Track, current_frame: Frame): + fade_duration = current_frame.camera.fps * 3 + diff = current_frame.index - track.history[-1].frame_nr + return max(0, min(1, diff / fade_duration)) + # track.history[-1].frame_nr < (current_frame.index - current_frame.camera.fps * 3) + # track.history[-1].frame_nr < (current_frame.index - current_frame.camera.fps * 3) + + +# Deprecated +def render_frame_to_dac(dac: HeliosDAC, tracker_frame: Frame, prediction_frame: Frame, first_time: float, config: Namespace, tracks: Dict[str, Track], predictions: Dict[str, Track], as_clusters = True) -> np.array: + # TODO: replace opencv with QPainter to support alpha? https://doc.qt.io/qtforpython-5/PySide2/QtGui/QPainter.html#PySide2.QtGui.PySide2.QtGui.QPainter.drawImage + # or https://github.com/pygobject/pycairo?tab=readme-ov-file + # or https://pyglet.readthedocs.io/en/latest/programming_guide/shapes.html + # and use http://code.astraw.com/projects/motmot/pygarrayimage.html or https://gist.github.com/nkymut/1cb40ea6ae4de0cf9ded7332f1ca0d55 + # or https://api.arcade.academy/en/stable/index.html (supports gradient color in line -- "Arcade is built on top of Pyglet and OpenGL.") + pointlist = [] + + # pointlist.append(HeliosPoint(x,y, dac.palette[cindex],blank=blank)) + + # all not working: + # if i == 1: + # # thanks to GpG for fixing scaling issue: https://stackoverflow.com/a/39668864 + # scale_factor = 1./20 # from 10m to 1000px + # S = np.array([[scale_factor, 0,0],[0,scale_factor,0 ],[ 0,0,1 ]]) + # new_H = S * self.H * np.linalg.inv(S) + # warpedFrame = cv2.warpPerspective(img, new_H, (1000,1000)) + # cv2.imwrite(str(self.config.output_dir / "orig.png"), warpedFrame) + # cv2.rectangle(img, (0,0), (img.shape[1],25), (0,0,0), -1) + + c = dac.palette[4] # Green + pointlist.append(HeliosPoint(10,10, c,blank=False)) + pointlist.append(HeliosPoint(10,100, c,blank=False)) + pointlist.append(HeliosPoint(100,100, c,blank=False)) + pointlist.append(HeliosPoint(100,10, c,blank=False)) + pointlist.append(HeliosPoint(10,10, c,blank=True)) + + if not tracker_frame: + c = dac.palette[3] # yellow + pointlist.append(HeliosPoint(110,10, c,blank=False)) + pointlist.append(HeliosPoint(110,100, c,blank=False)) + pointlist.append(HeliosPoint(200,100, c,blank=False)) + pointlist.append(HeliosPoint(200,10, c,blank=False)) + pointlist.append(HeliosPoint(110,10, c,blank=True)) + else: + for track_id, track in tracks.items(): + inv_H = np.linalg.pinv(tracker_frame.H) + history = track.get_projected_history(camera=config.camera) + history = convert_world_points_to_img_points(history) + # point_color = bgr_colors[color_index % len(bgr_colors)] + points = np.rint(history.reshape((-1,1,2))).astype(np.int32) + for i, point in enumerate(points): + blank = i+1 == len(points) # last point blank + pointlist.append(HeliosPoint(point[0][0], point[0][1], dac.palette[2], blank=blank)) + + # draw_track_projected(img, track, int(track_id), config.camera, convert_world_points_to_img_points) + + if not prediction_frame: + c = dac.palette[7] # magenta + pointlist.append(HeliosPoint(210,10, c,blank=False)) + pointlist.append(HeliosPoint(210,100, c,blank=False)) + pointlist.append(HeliosPoint(300,100, c,blank=False)) + pointlist.append(HeliosPoint(300,10, c,blank=False)) + pointlist.append(HeliosPoint(210,10, c,blank=True)) + # cv2.putText(img, f"Waiting for prediction...", (500,17), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1) + # continue + else: + for track_id, track in predictions.items(): + inv_H = np.linalg.pinv(prediction_frame.H) + # For debugging: + # draw_trackjectron_history(img, track, int(track.track_id), convert_world_points_to_img_points) + anim_position = get_animation_position(track, tracker_frame) + lines = track_predictions_to_lines(track, config.camera, anim_position) + + if not lines: + continue + + lines = [convert_world_points_to_img_points(points) for points in lines] + + # cv2 only draws to integer coordinates + lines = [np.rint(points).astype(int) for points in lines] + + # draw in a single pass + # line_points = line_points.reshape((1, -1,1,2)) + for line in lines: + for i, point in enumerate(line): + blank = i+1 == len(points) # last point blank + pointlist.append(HeliosPoint(point[0], point[1], dac.palette[4], blank=blank)) + # draw_track_predictions(img, track, int(track.track_id)+1, config.camera, convert_world_points_to_img_points, anim_position=anim_position, as_clusters=as_clusters) + # cv2.putText(img, f"{len(track.predictor_history) if track.predictor_history else 'none'}", to_point(track.history[0].get_foot_coords()), cv2.FONT_HERSHEY_COMPLEX, 1, (255,255,255), 1) + + + return pointlist + + +def run_laser_renderer(config: Namespace, is_running: BaseEvent, timer_counter): + renderer = LaserRenderer(config, is_running) + renderer.run(timer_counter) \ No newline at end of file diff --git a/trap/plumber.py b/trap/plumber.py index 8594f53..63b08cc 100644 --- a/trap/plumber.py +++ b/trap/plumber.py @@ -9,6 +9,7 @@ import time from trap.config import parser from trap.cv_renderer import run_cv_renderer from trap.frame_emitter import run_frame_emitter +from trap.laser_renderer import run_laser_renderer from trap.prediction_server import run_prediction_server from trap.preview_renderer import run_preview_renderer from trap.animation_renderer import run_animation_renderer @@ -106,6 +107,10 @@ def start(): procs.append( ExceptionHandlingProcess(target=run_animation_renderer, kwargs={'config': args, 'is_running': isRunning}, name='renderer') ) + if args.render_laser: + procs.append( + ExceptionHandlingProcess(target=run_laser_renderer, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_preview.iterations}, name='renderer') + ) if not args.bypass_prediction: timer_predict = timers.new('predict') diff --git a/trap/tools.py b/trap/tools.py index 315efec..d94e492 100644 --- a/trap/tools.py +++ b/trap/tools.py @@ -324,27 +324,13 @@ def cluster_predictions_by_radius(start_point, lines: Iterable[np.ndarray] | Lin # l = LineString([(0,0), (10, 10)]) # i = c.intersection(l) - - -def draw_track_predictions(img: cv2.Mat, track: Track, color_index: int, camera:Camera, convert_points: Optional[Callable], anim_position=.8, as_clusters=False): - """ - anim_position: 0-1 - """ +def track_predictions_to_lines(track: Track, camera:Camera, anim_position=.8): if not track.predictions: return - + current_point = track.get_projected_history(camera=camera)[-1] - - opacity = 1-min(1, max(0, inv_lerp(0.8, 1, anim_position))) # fade out slide_t = min(1, max(0, inv_lerp(0, 0.8, anim_position))) # slide_position - - # if convert_points: - # current_point = convert_points([current_point])[0] - - color = bgr_colors[color_index % len(bgr_colors)] - color = tuple([int(c*opacity) for c in color]) - lines = [] for pred_i, pred in enumerate(track.predictions): pred_coords = pred #cv2.perspectiveTransform(np.array([pred]), inv_H)[0].tolist() @@ -353,7 +339,27 @@ def draw_track_predictions(img: cv2.Mat, track: Track, color_index: int, camera: # print(pred_coords, current_point, line_points) line_points = transition_path_points(line_points, slide_t) lines.append(line_points) - + return lines + +def draw_track_predictions(img: cv2.Mat, track: Track, color_index: int, camera:Camera, convert_points: Optional[Callable], anim_position=.8, as_clusters=False): + """ + anim_position: 0-1 + """ + + lines = track_predictions_to_lines(track, camera, anim_position) + + if not lines: + return + + opacity = 1-min(1, max(0, inv_lerp(0.8, 1, anim_position))) # fade out + + + # if convert_points: + # current_point = convert_points([current_point])[0] + + color = bgr_colors[color_index % len(bgr_colors)] + color = tuple([int(c*opacity) for c in color]) + if as_clusters: clusters = cluster_predictions_by_radius(current_point, lines, 1.5) @@ -387,7 +393,7 @@ def draw_track_predictions(img: cv2.Mat, track: Track, color_index: int, camera: lines = [np.rint(points).astype(int) for points in lines] # draw in a single pass - line_points = line_points.reshape((1, -1,1,2)) + # line_points = line_points.reshape((1, -1,1,2)) # TODO)) SEems to do nothing.. cv2.polylines(img, lines, False, color, 2, cv2.LINE_AA) def draw_trackjectron_history(img: cv2.Mat, track: Track, color_index: int, convert_points: Optional[Callable]): diff --git a/trap/video_sources.py b/trap/video_sources.py index 0fc8dac..380d981 100644 --- a/trap/video_sources.py +++ b/trap/video_sources.py @@ -7,7 +7,7 @@ import neoapi import cv2 import numpy as np -from trap.frame_emitter import Camera, Frame, UrlOrPath +from trap.base import Camera, UrlOrPath logger = logging.getLogger('video_source') @@ -32,10 +32,10 @@ class GigE(VideoSource): self.camera.SetImageBufferCount(10) # neoAPI docs: Setting the neoapi.Cam.SetImageBufferCycleCount()to one ensures that all buffers but one are given back to the neoAPI to be re-cycled and never given to the user by the neoapi.Cam.GetImage() method. self.camera.SetImageBufferCycleCount(1) - # if self.camera.IsConnected(): - # self.camera.f.PixelFormat.Set(neoapi.PixelFormat_RGB8) - # self.camera.f.BinningHorizontal.Set(2) - # self.camera.f.BinningVertical.Set(2) + if self.camera.IsConnected(): + self.camera.f.PixelFormat.Set(neoapi.PixelFormat_RGB8) + self.camera.f.BinningHorizontal.Set(2) + self.camera.f.BinningVertical.Set(2) self.pixfmt = self.camera.f.PixelFormat.Get() def recv(self): @@ -43,7 +43,7 @@ class GigE(VideoSource): if not self.camera.IsConnected(): return - i = self.camera.GetImage(0) + i = self.camera.GetImage(0) if i.IsEmpty(): time.sleep(.01) continue