Private
Public Access
381 lines
16 KiB
Python
381 lines
16 KiB
Python
"""Local colour-marker preview and deliberately armed camera servo test."""
|
|
from collections import deque
|
|
from io import BytesIO
|
|
from pathlib import Path
|
|
import base64
|
|
import json
|
|
import math
|
|
import os
|
|
import secrets
|
|
import tempfile
|
|
import threading
|
|
import time
|
|
|
|
from PIL import Image
|
|
|
|
DEFAULT = dict(hue=85, tolerance=18, pan_min=1400, pan_max=1600,
|
|
tilt_min=1400, tilt_max=1600, pan_reverse=False, tilt_reverse=False)
|
|
|
|
|
|
def validate(value):
|
|
if set(value) != set(DEFAULT):
|
|
raise ValueError('Unvollständige Nachführungseinstellungen')
|
|
result = {}
|
|
for key, default in DEFAULT.items():
|
|
v = value[key]
|
|
if isinstance(default, bool):
|
|
if type(v) is not bool:
|
|
raise ValueError(f'{key}: Ja/Nein erwartet')
|
|
else:
|
|
if isinstance(v, bool) or not isinstance(v, (float, int)) or not math.isfinite(v):
|
|
raise ValueError(f'{key}: gültige Zahl erwartet')
|
|
v = int(v)
|
|
result[key] = v
|
|
if not 0 <= result['hue'] <= 255 or not 5 <= result['tolerance'] <= 35:
|
|
raise ValueError('Ungültige Farbe oder Farbtoleranz')
|
|
for axis in ('pan', 'tilt'):
|
|
if not 1100 <= result[axis+'_min'] <= 1500 <= result[axis+'_max'] <= 1900:
|
|
raise ValueError('Grenzen müssen zwischen 1100 und 1900 µs liegen und 1500 einschließen')
|
|
return result
|
|
|
|
|
|
def locate(jpeg, config):
|
|
"""Largest unambiguous saturated blob, circular hue distance (also red)."""
|
|
image = Image.open(BytesIO(jpeg)).convert('RGB').resize((160, 120))
|
|
pixels = list(image.convert('HSV').getdata())
|
|
mask = set()
|
|
for i, (h, s, v) in enumerate(pixels):
|
|
delta = abs(h-config['hue'])
|
|
if min(delta, 256-delta) <= config['tolerance'] and s >= 100 and v >= 60:
|
|
mask.add(i)
|
|
blobs = []
|
|
while mask:
|
|
seed = mask.pop()
|
|
queue, blob = deque([seed]), [seed]
|
|
while queue:
|
|
i = queue.popleft()
|
|
x, y = i % 160, i // 160
|
|
for n in ((i-1 if x else -1), (i+1 if x < 159 else -1),
|
|
(i-160 if y else -1), (i+160 if y < 119 else -1)):
|
|
if n in mask:
|
|
mask.remove(n); queue.append(n); blob.append(n)
|
|
if len(blob) >= 12:
|
|
blobs.append(blob)
|
|
blobs.sort(key=len, reverse=True)
|
|
if not blobs or len(blobs[0]) > 160*120*.35:
|
|
return None
|
|
if len(blobs) > 1 and len(blobs[1]) >= len(blobs[0])*.6:
|
|
return None
|
|
blob = blobs[0]
|
|
return {'x': sum(i % 160 for i in blob)/len(blob)/160,
|
|
'y': sum(i // 160 for i in blob)/len(blob)/120,
|
|
'radius': math.sqrt(len(blob)/math.pi)/160}
|
|
|
|
|
|
class PCA9685:
|
|
"""Dedicated camera board, bus 1/address 0x40, channels 0 and 1 only.
|
|
|
|
Register protocol: https://www.nxp.com/docs/en/data-sheet/PCA9685.pdf
|
|
No I2C access until an explicit hardware start. No bus scanning/reset broadcast.
|
|
"""
|
|
def __init__(self, bus_factory=None):
|
|
if bus_factory is None:
|
|
from smbus2 import SMBus
|
|
bus_factory = SMBus
|
|
self.bus = bus_factory(1)
|
|
try:
|
|
self.off()
|
|
# Sleep, internal oscillator, auto-increment. 25 MHz / (4096*122).
|
|
self.bus.write_byte_data(0x40, 0x00, 0x30)
|
|
self.bus.write_byte_data(0x40, 0xFE, 121)
|
|
self.bus.write_byte_data(0x40, 0x01, 0x04) # non-inverted totem-pole
|
|
self.bus.write_byte_data(0x40, 0x00, 0x20)
|
|
time.sleep(.005)
|
|
except Exception:
|
|
self.close()
|
|
raise
|
|
|
|
def move(self, pan, tilt):
|
|
for channel, pulse in enumerate((pan, tilt)):
|
|
if not 1100 <= pulse <= 1900:
|
|
raise ValueError('Puls außerhalb der absoluten Testgrenze')
|
|
count = round(pulse * 25 / 122) # microseconds -> oscillator ticks / prescale
|
|
self.bus.write_i2c_block_data(0x40, 0x06+4*channel,
|
|
[0, 0, count & 255, count >> 8])
|
|
|
|
def off(self):
|
|
failure = None
|
|
# Do not use a block write before auto-increment has been initialized.
|
|
for channel in (0, 1):
|
|
try:
|
|
self.bus.write_byte_data(0x40, 0x09+channel*4, 0x10)
|
|
except Exception as exc:
|
|
failure = exc
|
|
if failure:
|
|
raise failure
|
|
|
|
def close(self):
|
|
try:
|
|
self.off()
|
|
finally:
|
|
self.bus.close()
|
|
|
|
|
|
class FollowService:
|
|
def __init__(self, camera, directory, driver_factory=PCA9685, clock=time.monotonic):
|
|
self.camera, self.clock, self.factory = camera, clock, driver_factory
|
|
self.lock = threading.RLock()
|
|
self.path = Path(directory)/'follow.json'
|
|
self.config = dict(DEFAULT)
|
|
self.error = None
|
|
self.config_fault = False
|
|
try:
|
|
if self.path.exists():
|
|
self.config = validate(json.loads(self.path.read_text()))
|
|
except (OSError, ValueError, TypeError):
|
|
self.error = 'Nachführungseinstellungen beschädigt; neu speichern'
|
|
self.config_fault = True
|
|
self.active = False
|
|
self.operation = 'follow'
|
|
self.jog_direction = None
|
|
self.jog_until = 0.
|
|
self.jog_sequence = -1
|
|
self.driver = None
|
|
self.pan = self.tilt = 1500.
|
|
self.last_ping = self.clock()
|
|
self.last_result = self.last_seen = None
|
|
self.number = 0
|
|
self.target = None
|
|
self.jpeg = None
|
|
self.sample = None
|
|
self.sample_token = None
|
|
self.session = None
|
|
self.revision = 0
|
|
self.thread = None
|
|
self.quit = threading.Event()
|
|
|
|
def _save(self, value):
|
|
config = validate(value)
|
|
with self.lock:
|
|
self.path.parent.mkdir(parents=True, exist_ok=True)
|
|
fd, name = tempfile.mkstemp(dir=self.path.parent, prefix='.follow-')
|
|
try:
|
|
with os.fdopen(fd, 'w') as f:
|
|
json.dump(config, f)
|
|
os.replace(name, self.path)
|
|
finally:
|
|
if os.path.exists(name): os.unlink(name)
|
|
self.config, self.error, self.config_fault = config, None, False
|
|
self.revision += 1
|
|
return dict(config)
|
|
|
|
def configure(self, value):
|
|
with self.lock:
|
|
if self.active:
|
|
raise ValueError('Vor Einstellungsänderungen Nachführung stoppen')
|
|
return self._save(value)
|
|
|
|
def _direction(self, direction):
|
|
choices = {'left': ('pan', -1), 'right': ('pan', 1),
|
|
'up': ('tilt', -1), 'down': ('tilt', 1)}
|
|
if direction not in choices:
|
|
raise ValueError('Ungültige Richtung')
|
|
axis, sign = choices[direction]
|
|
return axis, -sign if self.config[axis+'_reverse'] else sign
|
|
|
|
def _require_setup(self, session):
|
|
self.watchdog()
|
|
if not self.active or self.operation != 'setup' or session != self.session:
|
|
raise ValueError('Einrichtsitzung beendet oder nicht freigegeben')
|
|
|
|
def jog(self, session, direction, sequence):
|
|
with self.lock:
|
|
self._require_setup(session)
|
|
if type(sequence) is not int or not 0 <= sequence <= 1_000_000_000:
|
|
raise ValueError('Ungültige Tastensequenz')
|
|
if direction is not None: self._direction(direction)
|
|
if sequence <= self.jog_sequence:
|
|
return {'accepted': False}
|
|
self.jog_sequence = sequence
|
|
self.jog_direction = direction
|
|
self.jog_until = self.clock()+.35 if direction else 0.
|
|
return {'accepted': True}
|
|
|
|
def remember(self, session, direction):
|
|
with self.lock:
|
|
self._require_setup(session)
|
|
if self.jog_direction and self.clock() < self.jog_until:
|
|
raise ValueError('Richtungstaste vor dem Speichern loslassen')
|
|
axis, sign = self._direction(direction)
|
|
# Store a slightly inward commanded limit, never an inferred physical stop.
|
|
value = round(getattr(self, axis)) + (10 if sign < 0 else -10)
|
|
key = axis+('_min' if sign < 0 else '_max')
|
|
if sign < 0 and value > 1500 or sign > 0 and value < 1500:
|
|
raise ValueError('Position liegt zu nahe an oder auf der anderen Seite der Mitte')
|
|
return self._save(dict(self.config, **{key:value}))
|
|
|
|
def capture(self):
|
|
with self.lock:
|
|
if self.active:
|
|
raise ValueError('Vor dem Farbfoto Nachführung stoppen')
|
|
_, jpeg, captured = self.camera.latest_timed()
|
|
if self.camera.error or jpeg is None or captured is None or self.clock()-captured > 1:
|
|
raise ValueError('Kein frisches Kamerabild')
|
|
self.sample, self.sample_token = jpeg, secrets.token_hex(16)
|
|
return {'image_base64': base64.b64encode(jpeg).decode(), 'token': self.sample_token}
|
|
|
|
def select(self, token, x, y):
|
|
with self.lock:
|
|
if not self.sample or token != self.sample_token:
|
|
raise ValueError('Beispielfoto erneut aufnehmen')
|
|
if not all(isinstance(v, (int, float)) and not isinstance(v, bool) and
|
|
math.isfinite(v) and 0 <= v <= 1 for v in (x, y)):
|
|
raise ValueError('Punkt außerhalb des Bildes')
|
|
image = Image.open(BytesIO(self.sample)).convert('HSV')
|
|
px, py = int(x*(image.width-1)), int(y*(image.height-1))
|
|
colors = list(image.crop((max(0,px-2), max(0,py-2),
|
|
min(image.width,px+3), min(image.height,py+3))).getdata())
|
|
colors = [(h,s,v) for h,s,v in colors if s >= 100 and v >= 60]
|
|
if len(colors) < 5:
|
|
raise ValueError('Bitte einen hellen, kräftig farbigen Punkt wählen')
|
|
# Circular mean avoids averaging red at both ends into cyan.
|
|
angle = math.atan2(sum(math.sin(h*2*math.pi/256) for h,_,_ in colors),
|
|
sum(math.cos(h*2*math.pi/256) for h,_,_ in colors))
|
|
config = dict(self.config, hue=round(angle*256/(2*math.pi)) % 256)
|
|
return self.configure(config)
|
|
|
|
def begin(self, hardware=False, acknowledged=False, operation='follow'):
|
|
with self.lock:
|
|
if operation not in ('follow', 'setup'):
|
|
raise ValueError('Unbekannter Kameramodus')
|
|
if self.active:
|
|
raise ValueError('Kameramodus läuft bereits')
|
|
if self.config_fault:
|
|
raise ValueError(self.error)
|
|
self.error = None
|
|
_, jpeg, captured = self.camera.latest_timed()
|
|
if self.camera.error or jpeg is None or captured is None or self.clock()-captured > 1:
|
|
raise ValueError('Kein frisches Kamerabild')
|
|
if hardware and (self.camera.demo or not acknowledged):
|
|
raise ValueError('Echte Kamera und bestätigter freier Servotest erforderlich')
|
|
self.pan = self.tilt = 1500.
|
|
try:
|
|
if hardware:
|
|
self.driver = self.factory()
|
|
self.driver.move(self.pan, self.tilt)
|
|
except Exception as exc:
|
|
self.halt(f'Servostart fehlgeschlagen: {exc}')
|
|
raise ValueError(self.error) from exc
|
|
self.active = True
|
|
self.operation = operation
|
|
self.jog_direction, self.jog_until, self.jog_sequence = None, 0., -1
|
|
self.session = secrets.token_hex(16)
|
|
self.last_ping = self.clock()
|
|
self.last_result = self.last_seen = self.clock()
|
|
self.jpeg = None
|
|
self.target = None
|
|
self.revision += 1
|
|
return self.session
|
|
|
|
def heartbeat(self, session):
|
|
with self.lock:
|
|
if not self.active or session != self.session:
|
|
raise ValueError('Nachführungssitzung beendet')
|
|
self.last_ping = self.clock()
|
|
|
|
def halt(self, error=None):
|
|
with self.lock:
|
|
self.active = False
|
|
self.session = None
|
|
self.jog_direction = None
|
|
self.jog_until = 0.
|
|
self.revision += 1
|
|
self.target = None
|
|
if self.driver:
|
|
driver, self.driver = self.driver, None
|
|
try:
|
|
driver.close()
|
|
except Exception as exc:
|
|
error = f'PWM-Abschaltung nicht bestätigt: {exc}; Servoversorgung ausschalten'
|
|
if error: self.error = error
|
|
|
|
def watchdog(self):
|
|
with self.lock:
|
|
if not self.active: return
|
|
now = self.clock()
|
|
_, _, captured = self.camera.latest_timed()
|
|
if self.camera.error or captured is None or now-captured > 1:
|
|
self.halt('Kamerabild ausgefallen; erneut starten')
|
|
elif now-self.last_ping > 1.5:
|
|
self.halt('Bedienverbindung verloren; erneut starten')
|
|
elif self.last_result is not None and now-self.last_result > 1:
|
|
self.halt('Bildauswertung ausgefallen; erneut starten')
|
|
|
|
def process(self, number, jpeg, captured, revision, target):
|
|
with self.lock:
|
|
now = self.clock()
|
|
if not self.active or revision != self.revision or captured is None or now-captured > 1:
|
|
return
|
|
self.watchdog()
|
|
if not self.active: return
|
|
dt = min(.25, max(0, now-(self.last_result or now)))
|
|
self.last_result, self.number, self.jpeg, self.target = now, number, jpeg, target
|
|
if self.operation == 'setup':
|
|
self.target = None
|
|
if self.jog_direction and now < self.jog_until:
|
|
axis, sign = self._direction(self.jog_direction)
|
|
value = max(1100, min(1900, getattr(self,axis)+sign*40*dt))
|
|
setattr(self,axis,value)
|
|
if self.driver: self.driver.move(self.pan,self.tilt)
|
|
else:
|
|
self.jog_direction = None
|
|
return
|
|
if target:
|
|
self.last_seen = now
|
|
for axis, coordinate in (('pan','x'), ('tilt','y')):
|
|
error = target[coordinate]-.5
|
|
if abs(error) < .06: continue
|
|
sign = -1 if self.config[axis+'_reverse'] else 1
|
|
value = getattr(self, axis) + sign*error*160*dt
|
|
setattr(self, axis, max(self.config[axis+'_min'], min(self.config[axis+'_max'], value)))
|
|
if self.driver: self.driver.move(self.pan, self.tilt)
|
|
# No searching or recentring when the marker disappears. End the
|
|
# hardware session after one second so old pulses do not hold forever.
|
|
elif self.driver and self.last_seen is not None and now-self.last_seen > 1:
|
|
self.halt('Farbpunkt verloren; erneut starten')
|
|
|
|
def status(self):
|
|
with self.lock:
|
|
return {'active': self.active, 'hardware': self.driver is not None,
|
|
'operation': self.operation, 'demo': self.camera.demo,
|
|
'error': self.error, 'config': dict(self.config),
|
|
'pan_us': round(self.pan), 'tilt_us': round(self.tilt),
|
|
'target': self.target, 'frame_number': self.number,
|
|
'image_base64': base64.b64encode(self.jpeg).decode() if self.active and self.jpeg else None}
|
|
|
|
def start(self):
|
|
self.thread = threading.Thread(target=self._run, daemon=True, name='colour-follow')
|
|
self.thread.start()
|
|
|
|
def _run(self):
|
|
last = 0
|
|
while not self.quit.wait(.05):
|
|
self.watchdog()
|
|
with self.lock:
|
|
if not self.active: continue
|
|
revision, config, operation = self.revision, dict(self.config), self.operation
|
|
number, jpeg, captured = self.camera.latest_timed()
|
|
if number == last or jpeg is None: continue
|
|
last = number
|
|
try:
|
|
target = locate(jpeg, config) if operation == 'follow' else None
|
|
self.process(number, jpeg, captured, revision, target)
|
|
except Exception as exc:
|
|
with self.lock:
|
|
if revision == self.revision: self.halt(f'Nachführungsfehler: {exc}')
|
|
|
|
def stop(self):
|
|
self.quit.set()
|
|
self.halt()
|
|
if self.thread: self.thread.join(timeout=2)
|