Im Vergleich zur vorherigen Version wird hier das Fahrerlose Transport System FTS mit Python + OpenCV vom PC aus über WLAN mit dem TXT im Online Modus gesteuert (Netzwerkverbindung über die Community Firmware CFW für den Fischertechnik TXT Controller). Statt der Barcodes in der vorherigen Version wird nun eine Ziffernerkennung an den Stationen genutzt. Die Anfahrt zur Station erfolgt mit einer PD Regelung der Motorleistung auf die Position der Ziffer im Kamerabild.
Der erste Teil zeigt Video-Aufnahmen mit einer Vollformat-35mm-Kamera montiert auf einem Gimbal, der mittlere Teil mit derselben Kamera montiert auf einem Stativ und der letzte Teil zeigt Mitschnitte der USB-Kamera auf dem FTS inklusive Ergebnisanzeige der Bildverarbeitung.
Code: Alles auswählen
from __future__ import print_function
import cv2 as cv
import numpy as np
import ftrobopy
import time
###########################################################################
vref1 = np.array ([[ 0, 0, 0, 0, 0, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[ 0, 0, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]])
vref2 = np.array ([[ 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 255, 255, 255],
[ 0, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 255, 255, 255]])
vref3 = np.array ([[ 0, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 0],
[255, 255, 255, 255, 0, 0, 255, 255, 255, 0, 0, 0, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
[ 0, 0, 255, 255, 255, 0, 0, 0, 255, 255, 255, 255, 255, 255, 0, 0]])
vref4 = np.array ([[ 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 0, 0, 0],
[ 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]])
###########################################################################
def detectFigures():
station = 0
position = 0
while True:
jpg = txt.getCameraFrame()
if jpg == None:
continue
else:
break
frame = cv.imdecode(np.frombuffer(bytearray(jpg)), cv.IMREAD_COLOR)
roi = frame[20:160, 80:240]
mask = cv.cvtColor(roi, cv.COLOR_BGR2GRAY)
ret, thresh = cv.threshold(mask, 70, 255,cv.THRESH_BINARY_INV)
contours,hierarchy = cv.findContours(thresh,2,cv.CHAIN_APPROX_SIMPLE)
if len(contours):
cont = max (contours, key = cv.contourArea)
cont = cont + (80, 20)
cv.drawContours(frame, [cont], 0, (0,255,0), 1)
x,y,w,h = cv.boundingRect(cont)
position = x + int(w/2)
if 100 < position < 220:
# x = x-int((h-w)/2)
x_roi = x-80
y_roi = y-20
roi = thresh[y_roi:y_roi+h, x_roi:x_roi+w]
resized = cv.resize(roi,(16,16),interpolation=cv.INTER_AREA)
ret, thresh_resized = cv.threshold(resized, 127, 255,cv.THRESH_BINARY)
resized = thresh_resized
# cv.imshow('resized',resized)
slice1 = resized[:,0:4]
slice2 = resized[:,4:8]
slice3 = resized[:,8:12]
slice4 = resized[:,12:16]
proj1 = np.amax(slice1, axis=1)
proj2 = np.amax(slice2, axis=1)
proj3 = np.amax(slice3, axis=1)
proj4 = np.amax(slice4, axis=1)
projection = np.array([proj1, proj2, proj3, proj4])
# print(projection)
error = np.array([255, 255, 255, 255])
error[0] = (projection != vref1).sum()
error[1] = (projection != vref2).sum()
error[2] = (projection != vref3).sum()
error[3] = (projection != vref4).sum()
# print ("error", error.min())
if error.min() < 10:
station = np.argmin(error)
station = station + 1
frame = cv.putText(frame, str(station), (x+w+5, y+5), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 1, cv.LINE_AA)
frame = cv.putText(frame, str(position), (x+w+5, y+h), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 255), 1, cv.LINE_AA)
# print(station)
cv.imshow('frame', frame)
cv.waitKey(1)
out.write(frame)
return position, station
#########################################
# function to detect red or blue boxes
# return values are
# 0: no Box color detected
# 2: red Box detected
# 3: blue Box detected
##########################################
def detectBoxColor():
x1, x2, y1, y2 = 170, 186, 186, 190
tnoce = int (8*(x2-x1)*(y2-y1)/10)
# noce is target number of correct elements
# > 80% of elements should be in range
while True:
jpg = txt.getCameraFrame()
if jpg == None:
continue
else:
break
frame = cv.imdecode(np.frombuffer(bytearray(jpg)), cv.IMREAD_COLOR)
roi = frame[y1:y2, x1:x2]
hsv = cv.cvtColor(roi, cv.COLOR_BGR2HSV)
hue = hsv[:, :, 0]
# print ('hue')
# print(hue)
result2 = np.count_nonzero((hue >= 160) | (hue <= 20))
# print ('result for red', result2)
result3 = np.count_nonzero((hue >= 80) & (hue <= 140))
# print ('result for blue', result3)
if result2 > tnoce:
color = 'red'
result = 2
elif result3 > tnoce:
color = 'blue'
result = 3
else:
color = 'no box'
result = 0
frame = cv.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 1)
frame = cv.putText(frame, color, (x2+5, y2), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA)
cv.imshow('frame', frame)
cv.waitKey(1)
out.write(frame)
# print (color)
return result
###########################################################################
def displayCam():
while True:
jpg = txt.getCameraFrame()
if jpg == None:
continue
else:
break
frame = cv.imdecode(np.frombuffer(bytearray(jpg)), cv.IMREAD_COLOR)
cv.imshow('frame', frame)
cv.waitKey(1)
out.write(frame)
#########################################################
# function to drive in sync to a distance
# m1 and m2 are on the front axis
# m3 and m4 are on the rear axis
# for drive to the left m2 and m3 must go forward
# for drive to the right m1 and m4 must go forward
#########################################################
def drive(distance, power_m1m4, power_m2m3):
txt.SyncDataBegin()
m1.setDistance(distance, syncto=m2)
m2.setDistance(distance, syncto=None)
m3.setDistance(distance, syncto=m4)
m4.setDistance(distance, syncto=m2)
txt.incrMotorCmdId(1)
txt.incrMotorCmdId(2)
txt.incrMotorCmdId(3)
txt.incrMotorCmdId(4)
m1.setSpeed(power_m1m4)
m2.setSpeed(power_m2m3)
m3.setSpeed(power_m2m3)
m4.setSpeed(power_m1m4)
txt.SyncDataEnd()
while not (m1.finished() and m2.finished() and m3.finished() and m4.finished()):
displayCam()
txt.updateWait()
txt.stopAll()
txt.updateWait()
d = txt.getCurrentCounterValue()
print(d)
#########################################################
# function to drive aligned to position of figure
# m1 and m2 are on the front axis
# m3 and m4 are on the rear axis
# for drive to the left m2 and m3 must go forward
# for drive to the right m1 and m4 must go forward
#########################################################
def driveFigure(distance, power_m1m4, power_m2m3):
position_old, station = detectFigures()
while station == 0:
position_old, station = detectFigures()
txt.SyncDataBegin()
m1.setDistance(distance, syncto=m4)
m2.setDistance(distance, syncto=None)
m3.setDistance(distance, syncto=m2)
m4.setDistance(distance, syncto=None)
txt.incrMotorCmdId(1)
txt.incrMotorCmdId(2)
txt.incrMotorCmdId(3)
txt.incrMotorCmdId(4)
m1.setSpeed(power_m1m4)
m2.setSpeed(power_m2m3)
m3.setSpeed(power_m2m3)
m4.setSpeed(power_m1m4)
txt.SyncDataEnd()
irStatusTaster = txt.joystick(1,0,0)
while not (m1.finished() and m2.finished() and m3.finished() and m4.finished()):
if irStatusTaster.updown():
break
position, station = detectFigures()
while station == 0:
position, station = detectFigures()
print (position, station)
# PD control for driving aligned to position of figure
p = position - 180
d = position - position_old
position_old = position
print ('p=', p, 'd=', d)
pwm = power_m1m4 + 4*p + 4*d
if pwm > -384:
pwm = -384
elif pwm < -512:
pwm = -512
print ('pwm', pwm)
m1.setSpeed(pwm)
m4.setSpeed(pwm)
txt.updateWait()
starttime = time.time()
while (time.time() < starttime + 0.5):
displayCam()
txt.stopAll()
txt.updateWait()
d = txt.getCurrentCounterValue()
print(d)
###########################################################################
# function to connect to station and load or unload boxes
# input: none
# output: color of box (only for station 1)
###########################################################################
def connect():
position, station = detectFigures()
while station == 0:
position, station = detectFigures()
print ("Station", station, "Position", position)
while ((station) and (position > 188)):
steps = int((position - 180)/8)
drive (steps, 340, 340)
position, station = detectFigures()
print ("Station", station, "Position", position)
while ((station) and (position < 172)):
steps = int((180 - position)/8)
drive (steps, -340, -340)
position, station = detectFigures()
print ("Station", station, "Position", position)
color = 0
if station == 1:
while color == 0:
color = detectBoxColor()
else:
color = 4
driveFigure(64, -448, 448)
starttime = time.time()
while (time.time() < starttime + 3):
displayCam()
drive (42, 448, -448)
return color
###########################################################################
# function to drive to target station
# input: command = target station
# output: none
###########################################################################
def driveStation(command):
position, actualStation = detectFigures()
while actualStation == 0:
position, actualStation = detectFigures()
print('actualStation=', actualStation, 'command=', command)
if command > actualStation:
distance = 60*(command - actualStation)
drive(distance, 340, 340)
elif command < actualStation:
distance = 62*(actualStation - command)
drive(distance, -340, -340)
###########################################################################
# Main Program
###########################################################################
txt=ftrobopy.ftrobopy('192.168.2.105')
txt.startCameraOnline()
time.sleep(3)
m1=txt.motor(1)
m2=txt.motor(2)
m3=txt.motor(3)
m4=txt.motor(4)
fourcc = cv.VideoWriter_fourcc(*'X264')
out = cv.VideoWriter('FT_Cam.mp4',fourcc, 10.0, (320,240))
irCommand = txt.joystick(0,0,0)
# left joystick is used
while True:
detectFigures()
command = int (-15 * irCommand.leftright())
if command:
driveStation (command)
command = connect()
print('delivery=', command)
driveStation (command)
connect()
if cv.waitKey(1) & 0xFF == ord('q'):
break
txt.updateWait()
txt.stopCameraOnline()
txt.stopOnline()
out.release()
cv.destroyAllWindows()
die Präzision der Anfahrt einfach nur Klasse 
