From 69b5482a6241e29d5d2d08c8d5a8d33499114e5a Mon Sep 17 00:00:00 2001 From: Julio Vega Date: Wed, 23 May 2018 18:41:41 +0200 Subject: [PATCH] [Issue #1166] Added library PiBot as a new driver of JdeRobot --- src/drivers/PiBot/real/JdeRobotKids.cfg | 18 + src/drivers/PiBot/real/JdeRobotKids.py | 153 +++++ .../PiBot/real/piBot/filters/__init__.py | 0 .../PiBot/real/piBot/filters/hsvFilter.py | 71 +++ .../PiBot/real/piBot/filters/rgbFilter.py | 71 +++ .../PiBot/real/piBot/filters/yuvFilter.py | 76 +++ src/drivers/PiBot/real/piBot/piBot.py | 449 +++++++++++++++ src/drivers/PiBot/real/piBot/progeo.py | 533 ++++++++++++++++++ .../PiBot/real/piBot/sensors/__init__.py | 0 .../PiBot/real/piBot/sensors/cameraFilter.py | 120 ++++ 10 files changed, 1491 insertions(+) create mode 100644 src/drivers/PiBot/real/JdeRobotKids.cfg create mode 100644 src/drivers/PiBot/real/JdeRobotKids.py create mode 100644 src/drivers/PiBot/real/piBot/filters/__init__.py create mode 100644 src/drivers/PiBot/real/piBot/filters/hsvFilter.py create mode 100644 src/drivers/PiBot/real/piBot/filters/rgbFilter.py create mode 100644 src/drivers/PiBot/real/piBot/filters/yuvFilter.py create mode 100644 src/drivers/PiBot/real/piBot/piBot.py create mode 100644 src/drivers/PiBot/real/piBot/progeo.py create mode 100644 src/drivers/PiBot/real/piBot/sensors/__init__.py create mode 100644 src/drivers/PiBot/real/piBot/sensors/cameraFilter.py diff --git a/src/drivers/PiBot/real/JdeRobotKids.cfg b/src/drivers/PiBot/real/JdeRobotKids.cfg new file mode 100644 index 000000000..6fb29dac7 --- /dev/null +++ b/src/drivers/PiBot/real/JdeRobotKids.cfg @@ -0,0 +1,18 @@ +# Opciones -> "mBotReal", "mBotGazebo", "piBot" +JdeRobotKids.Robot=piBot +JdeRobotKids.Puerto=/dev/ttyUSB0 +JdeRobotKids.Camara=PiCam # opciones: "PiCam", "WebCam" + +# Sin registro: +#JdeRobotKids.Motors=default -h 0.0.0.0 -p 9999:ws -h 0.0.0.0 -p 11000 + +# Con registro: +JdeRobotKids.Motors.Proxy=Motors:default -h 0.0.0.0 -p 9999 +#JdeRobotKids.PiCam.Proxy=Camera:default -h 0.0.0.0 -p 8995 +#JdeRobotKids.Sonar.Proxy=Sonar:default -h 0.0.0.0 -p 8993 + +#Configuración usada por Gazebo: +#Motors Endpoints > default -h 0.0.0.0 -p 9999:ws -h 0.0.0.0 -p 11000 +#Sonar Endpoints > default -h 0.0.0.0 -p 8993:ws -h 0.0.0.0 -p 11005 +#CameraGazebo picam Endpoints > default -h 0.0.0.0 -p 8995:ws -h 0.0.0.0 -p 11002 + diff --git a/src/drivers/PiBot/real/JdeRobotKids.py b/src/drivers/PiBot/real/JdeRobotKids.py new file mode 100644 index 000000000..793957fa5 --- /dev/null +++ b/src/drivers/PiBot/real/JdeRobotKids.py @@ -0,0 +1,153 @@ +#(c) JdeRobot-kids 2018 + +# -*- coding: utf-8 -*- +import abc +from abc import ABCMeta + +class JdeRobotKids(object): + __metaclass__ = ABCMeta + + # METODOS DE VISIONLIB + @abc.abstractmethod + def damePosicionDeObjetoDeColor(image, color): + print ("damePosicionDeObjetoDeColor abstracto") + + # METODOS COMUNES A LAS TRES PLATAFORMAS + @abc.abstractmethod + def quienSoy(self): + print ("Soy un tipo de robot abstracto") + + @abc.abstractmethod + def avanzar(self, vel): + print ("Avanzar abstracto") + + @abc.abstractmethod + def retroceder(self, vel): + print ("Retroceder abstracto") + + @abc.abstractmethod + def parar(self): + print ("Parar abstracto") + + @abc.abstractmethod + def girarIzquierda(self, vel): + print ("Girar izquierda abstracto") + + @abc.abstractmethod + def girarDerecha(self, vel): + print ("Girar derecha abstracto") + + @abc.abstractmethod + def move(self, velV, velW): + print ("Move abstracto") + + # METODOS COMUNES A PIBOT Y MBOTREAL + @abc.abstractmethod + def leerIntensidadLuz(self): + print ("leerIntensidadLuz abstracto") + + @abc.abstractmethod + def leerUltrasonido(self): + print ("leerUltrasonido abstracto") + + @abc.abstractmethod + def leerIRSigueLineas(self): + print ("leerIRSigueLineas abstracto") + + @abc.abstractmethod + def leerIntensidadSonido(self): + print ("leerIntensidadSonido abstracto") + + @abc.abstractmethod + def leerPotenciometro(self): + print ("leerPotenciometro abstracto") + + @abc.abstractmethod + def moverServo(self, *args): + print ("moverServo abstracto") + + def get_tipo(self): + pass + + def set_tipo(self, valor): + pass + + tipo = abc.abstractproperty(get_tipo, set_tipo) + + # METODOS PROPIOS DEL MBOTREAL + @abc.abstractmethod + def encenderLedPlaca(self, indice, r, g, b): + print ("encenderLedPlaca abstracto") + + @abc.abstractmethod + def apagarLedPlaca(self, indice): + print ("apagarLedPlaca abstracto") + + @abc.abstractmethod + def encenderLed(self, puerto, indice, r, g, b): + print ("encenderLed abstracto") + + @abc.abstractmethod + def apagarLed(self, puerto, indice): + print ("apagarLed abstracto") + + @abc.abstractmethod + def escribirTexto(self, puerto, texto, dx, dy, brillo): + print ("escribirTexto abstracto") + + @abc.abstractmethod + def escribirFrase(self, puerto, texto, brillo = 3): + print ("escribirFrase abstracto") + + @abc.abstractmethod + def dibujosPosibles(self): + print ("dibujosPosibles abstracto") + + @abc.abstractmethod + def pintarDibujo(self, puerto, nombreDibujo, brillo): + print ("pintarDibujo abstracto") + + @abc.abstractmethod + def dibujar (self, puerto, dibujo, posicionX, posicionY, brillo, ancho): + print ("dibujar abstracto") + + @abc.abstractmethod + def escribirReloj(self, puerto, hora, minuto, brillo): + print ("escribirReloj abstracto") + + @abc.abstractmethod + def borrarMatriz(self, puerto): + print ("borrarMatriz abstracto") + + @abc.abstractmethod + def leerBoton(self): + print ("leerBoton abstracto") + + @abc.abstractmethod + def playTono(self, tono, tiempo): + print ("playTono abstracto") + + @abc.abstractmethod + def valoresMandoPosibles(self): + print ("valoresMandoPosibles abstracto") + + @abc.abstractmethod + def leerMandoIR(self): + print ("leerMandoIR abstracto") + + @abc.abstractmethod + def dameImagen(self): + print ("dameImagen abstracto") + + @abc.abstractmethod + def mostrarImagen(self): + print ("mostrarImagen abstracto") + + @abc.abstractmethod + def dameSonarVisual (self, image, cameraModel): + print ("dameSonarVisual abstracto") + + @abc.abstractmethod + def damePosicionDeObjetoDeColor(self, image, color): + print ("damePosicionDeObjetoDeColor abstracto") + diff --git a/src/drivers/PiBot/real/piBot/filters/__init__.py b/src/drivers/PiBot/real/piBot/filters/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/drivers/PiBot/real/piBot/filters/hsvFilter.py b/src/drivers/PiBot/real/piBot/filters/hsvFilter.py new file mode 100644 index 000000000..a332601d7 --- /dev/null +++ b/src/drivers/PiBot/real/piBot/filters/hsvFilter.py @@ -0,0 +1,71 @@ + +from threading import Lock +import cv2 +import numpy as np + +'''Max Values supported by OpenCV''' +HSVMAX = (179,255,255) +HSVMIN = (0,0,0) + + +class HsvFilter: + + def __init__(self): + + self.lock = Lock() + + self.MAX = HSVMAX + self.MIN = HSVMIN + + self.uLimit = list(HSVMAX) + self.dLimit = list(HSVMIN) + + def getName(self): + return 'HSV' + + def setUpLimit (self, h, s, v): + self.lock.acquire() + self.uLimit = [h,s,v] + self.lock.release() + + + def getUpLimit (self): + self.lock.acquire() + lim = self.uLimit + self.lock.release() + return lim + + def setDownLimit(self, h, s, v): + self.lock.acquire() + self.dLimit = [h,s,v] + self.lock.release() + + def getDownLimit(self): + self.lock.acquire() + lim = self.dLimit + self.lock.release() + return lim + + def getMAX(self): + return self.MAX + + def getMIN(self): + return self.MIN + + def apply (self, img): + + hup,sup,vup = self.getUpLimit() + hdwn,sdwn,vdwn = self.getDownLimit() + + hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) + + # http://docs.opencv.org/3.0-beta/doc/py_tutorials/py_imgproc/py_colorspaces/py_colorspaces.html + minValues = np.array([hdwn,sdwn,vdwn],dtype=np.uint8) + maxValues = np.array([hup,sup,vup], dtype=np.uint8) + + mask = cv2.inRange(hsv, minValues, maxValues) + + res = cv2.bitwise_and(img,img, mask= mask) + + + return res \ No newline at end of file diff --git a/src/drivers/PiBot/real/piBot/filters/rgbFilter.py b/src/drivers/PiBot/real/piBot/filters/rgbFilter.py new file mode 100644 index 000000000..66d8e85e2 --- /dev/null +++ b/src/drivers/PiBot/real/piBot/filters/rgbFilter.py @@ -0,0 +1,71 @@ + +from threading import Lock +import cv2 +import numpy as np + + +'''Max Values supported by OpenCV''' +RGBMAX = (255,255,255) +RGBMIN = (0,0,0) + + +class RgbFilter: + + def __init__(self): + + self.lock = Lock() + + self.MAX = RGBMAX + self.MIN = RGBMIN + + self.uLimit = list(RGBMAX) + self.dLimit = list(RGBMIN) + + def getName(self): + return 'RGB' + + def setUpLimit (self, r, g, b): + self.lock.acquire() + self.uLimit = [r,g,b] + self.lock.release() + + + def getUpLimit (self): + self.lock.acquire() + lim = self.uLimit + self.lock.release() + return lim + + def setDownLimit(self, r, g, b): + self.lock.acquire() + self.dLimit = [r,g,b] + self.lock.release() + + def getDownLimit(self): + self.lock.acquire() + lim = self.dLimit + self.lock.release() + return lim + + def getMAX(self): + return self.MAX + + def getMIN(self): + return self.MIN + + def apply (self, img): + + rup,gup,bup = self.getUpLimit() + rdwn,gdwn,bdwn = self.getDownLimit() + + + minValues = np.array([rdwn,gdwn,bdwn],dtype=np.uint8) + maxValues = np.array([rup,gup,bup], dtype=np.uint8) + + mask = cv2.inRange(img, minValues, maxValues) + + res = cv2.bitwise_and(img,img, mask= mask) + + + return res + return img \ No newline at end of file diff --git a/src/drivers/PiBot/real/piBot/filters/yuvFilter.py b/src/drivers/PiBot/real/piBot/filters/yuvFilter.py new file mode 100644 index 000000000..b3c0ab999 --- /dev/null +++ b/src/drivers/PiBot/real/piBot/filters/yuvFilter.py @@ -0,0 +1,76 @@ + +from threading import Lock + +import cv2 +import numpy as np + +'''Max Values supported by OpenCV''' +YUVMAX = (255,255,255) +YUVMIN = (0,0,0) + + + +class YuvFilter: + + def __init__(self): + + self.lock = Lock() + + self.MAX = YUVMAX + self.MIN = YUVMIN + + self.uLimit = list(YUVMAX) + self.dLimit = list(YUVMIN) + + def getName(self): + return 'YUV' + + def setUpLimit (self, y, u, v): + self.lock.acquire() + self.uLimit = [y,u,v] + self.lock.release() + + + def getUpLimit (self): + self.lock.acquire() + lim = self.uLimit + self.lock.release() + return lim + + def setDownLimit(self, y, u, v): + self.lock.acquire() + self.dLimit = [y,u,v] + self.lock.release() + + def getDownLimit(self): + self.lock.acquire() + lim = self.dLimit + self.lock.release() + return lim + + def getMAX(self): + return self.MAX + + def getMIN(self): + return self.MIN + + def apply (self, img): + + + yup,uup,vup = self.getUpLimit() + ydwn,udwn,vdwn = self.getDownLimit() + + ''' We convert RGB as BGR because OpenCV + with RGB pass to YVU instead of YUV''' + + yuv = cv2.cvtColor(img, cv2.COLOR_BGR2YUV) + + minValues = np.array([ydwn,udwn,vdwn],dtype=np.uint8) + maxValues = np.array([yup,uup,vup], dtype=np.uint8) + + mask = cv2.inRange(yuv, minValues, maxValues) + + res = cv2.bitwise_and(img,img, mask= mask) + + + return res \ No newline at end of file diff --git a/src/drivers/PiBot/real/piBot/piBot.py b/src/drivers/PiBot/real/piBot/piBot.py new file mode 100644 index 000000000..fc4d95e28 --- /dev/null +++ b/src/drivers/PiBot/real/piBot/piBot.py @@ -0,0 +1,449 @@ +# -*- coding: utf-8 -*- +from JdeRobotKids import JdeRobotKids +import Ice +import numpy +import threading +import sys +import comm +import config +import progeo + +class PiBot: +<<<<<<< HEAD + ''' + Controlador para el Robot PiBot de JdeRobot-Kids + Actualmente incluye dos funciones de procesamiento visual: + - damePosicionDeObjetoDeColor + - dameSonarVisual + ''' + def __init__(self, camara): + # Libreria RPi.GPIO + import RPi.GPIO as GPIO + import pigpio # Libreria para manejar los servos + JdeRobotKids.__init__(self) + self._GPIO = GPIO + self._tipo = "PiBot" + self._dit = pigpio.pi() + self._frame = None + if camara == "PiCam" + self._videostream = VideoStream(usePiCamera=True).start() + else + self._videostream = VideoStream(usePiCamera=False).start() + ''' + props = Ice.createProperties() + props.setProperty("JdeRobotKids.Motors.Proxy", "Motors:default -h localhost -p 9999") + props.setProperty("JdeRobotKids.Motors.maxV", "1") + props.setProperty("JdeRobotKids.Motors.maxW", "0.5") + id = Ice.InitializationData() + id.properties = props + ic = Ice.initialize(id) + self.motors = Motors (ic, "JdeRobotKids.Motors") + ''' + + def moverServo(self, *args): + ''' + Función que hace girar al servo motor a un angulo dado como parámetro. + @type args: lista + @param args: lista de argumentos: + args[0]: puerto al que esta conectado el controlador del servo + args[1]: banco al que esta conectado el servo en el controlador + args[2]: angulo de giro del servo. 0-180 grados. ¡PROBAR GIRO ANTES DE MONTAR EL SERVO! + ''' + puerto = args[0] + banco = args[1] + angulo = args[2]/10 + + self._GPIO.setmode(GPIO.BOARD) #Ponemos la Raspberry en modo BOARD + self._GPIO.setup(puerto,GPIO.OUT) #Ponemos el pin args[0] como salida + p = self._GPIO.PWM(puerto,50) #Ponemos el pin args[0] en modo PWM y enviamos 50 pulsos por segundo + p.start(7.5) #Enviamos un pulso del 7.5% para centrar el servo + p.ChangeDutyCycle(angulo) + time.sleep(0.5) + + def avanzar(self, vel): + ''' + Función que hace avanzar al robot en línea recta a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de avance del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1720) + self._dit.set_servo_pulsewidth(puertoR, 1280) + + def retroceder(self, vel): + ''' + Función que hace retroceder al robot en línea recta a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de retroceso del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1280) + self._dit.set_servo_pulsewidth(puertoR, 1720) + + def parar(self): + ''' + Función que hace detenerse al robot. + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1500) + self._dit.set_servo_pulsewidth(puertoR, 1480) + + def girarIzquierda(self, vel): + ''' + Función que hace rotar al robot sobre sí mismo hacia la izquierda a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de giro del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1530) + self._dit.set_servo_pulsewidth(puertoR, 1380) + + def girarDerecha(self, vel): + ''' + Función que hace rotar al robot sobre sí mismo hacia la derecha a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de giro del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1620) + self._dit.set_servo_pulsewidth(puertoR, 1460) + + def move(self, velV, velW): + ''' + Función que hace avanzar y girar al robot al mismo tiempo, según las velocidades V,W dadas como parámetro. + + @type velV, velW: enteros de [1 a 5, a mas, se corta en 5] + @param velV, velW: velocidades de avance de motores lineal y angular, en m/s y rad/s respectivamente + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + if ((velV == 0) and (velW != 0)): # pivotar + if (velW > 0): # hacia derecha + if (velW < 3): # despacio + self._dit.set_servo_pulsewidth(puertoL, 1550) + self._dit.set_servo_pulsewidth(puertoR, 1520) + + else: # a tope + self._dit.set_servo_pulsewidth(puertoL, 1700) + self._dit.set_servo_pulsewidth(puertoR, 1700) + + else: # hacia izquierda + if (velW > -3): # despacio + self._dit.set_servo_pulsewidth(puertoL, 1520) + self._dit.set_servo_pulsewidth(puertoR, 1490) + + else: # a tope + self._dit.set_servo_pulsewidth(puertoL, 1300) + self._dit.set_servo_pulsewidth(puertoR, 1300) + + elif ((velV != 0) and (velW == 0)): # linea recta + if (velV > 0): # adelante + if (velV < 2): # minimo + self._dit.set_servo_pulsewidth(puertoL, 1530) + self._dit.set_servo_pulsewidth(puertoR, 1509) + elif (velV <= 4): # despacio + self._dit.set_servo_pulsewidth(puertoL, 1540) + self._dit.set_servo_pulsewidth(puertoR, 1500) + else: # a tope + self._dit.set_servo_pulsewidth(puertoL, 1700) + self._dit.set_servo_pulsewidth(puertoR, 1300) + + else: # atras + if (velV > -3): # despacio + self._dit.set_servo_pulsewidth(puertoL, 1520) + self._dit.set_servo_pulsewidth(puertoR, 1520) + + else: # a tope + self._dit.set_servo_pulsewidth(puertoL, 1300) + self._dit.set_servo_pulsewidth(puertoR, 1700) + + elif ((velV == 0) and (velW == 0)): # PARADO + self._dit.set_servo_pulsewidth(puertoL, 1525) + self._dit.set_servo_pulsewidth(puertoR, 1510) + + elif ((velV > 0) and (velV <= 3)): # COMBINACION DE VELOCIDADES, AVANZANDO DESPACIO... + if ((velW > 0) and (velW <= 3)): # ...mientras gira despacio a derecha + self._dit.set_servo_pulsewidth(puertoL, 1540) + self._dit.set_servo_pulsewidth(puertoR, 1509) + elif (velW > 3): # ...mientras gira rapido a derecha + self._dit.set_servo_pulsewidth(puertoL, 1700) + self._dit.set_servo_pulsewidth(puertoR, 1509) + elif ((velW < 0) and (velW >= -3)): # ...mientras gira despacio a izquierda + self._dit.set_servo_pulsewidth(puertoL, 1530) + self._dit.set_servo_pulsewidth(puertoR, 1500) + elif (velW < -3): # ...mientras gira rapido a izquierda + self._dit.set_servo_pulsewidth(puertoL, 1530) + self._dit.set_servo_pulsewidth(puertoR, 1300) + + elif (velV > 3): # COMBINACION DE VELOCIDADES, AVANZANDO RAPIDO... + if ((velW > 0) and (velW <= 3)): # ...mientras gira despacio a derecha + self._dit.set_servo_pulsewidth(puertoL, 1700) + self._dit.set_servo_pulsewidth(puertoR, 1500) + elif (velW > 3): # ...mientras gira rapido a derecha + self._dit.set_servo_pulsewidth(puertoL, 1700) + self._dit.set_servo_pulsewidth(puertoR, 1509) + elif ((velW < 0) and (velW >= -3)): # ...mientras gira despacio a izquierda + self._dit.set_servo_pulsewidth(puertoL, 1540) + self._dit.set_servo_pulsewidth(puertoR, 1300) + elif (velW < -3): # ...mientras gira rapido a izquierda + self._dit.set_servo_pulsewidth(puertoL, 1530) + self._dit.set_servo_pulsewidth(puertoR, 1300) + + def dameImagen (self): + self._frame = self._videostream.read() + self._frame = imutils.resize(self._frame, width=400) + + return self._frame + + def mostrarImagen (self) + cv2.imshow("Imagen", self._frame) + + + def damePosicionDeObjetoDeColor(): + ''' + Función que devuelve el centro del objeto que tiene un color verde en el rango [GREEN_MIN, GREEN_MAX] para ser detectado + ''' + # resize the image + image = dameImagen() + + # convert to the HSV color space + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + + # construct a mask for the color specified + # then perform a series of dilations and erosions + # to remove any small blobs left in the mask + mask = cv2.inRange(hsv, GREEN_MIN, GREEN_MAX) + mask = cv2.erode(mask, None, iterations=2) + mask = cv2.dilate(mask, None, iterations=2) + + # find contours in the mask and + # initialize the current center + cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] + center = None + + # only proceed if at least one contour was found + if len(cnts) > 0: + # find the largest contour in the mask, then use + # it to compute the minimum enclosing circle and + # centroid + c = max(cnts, key=cv2.contourArea) + ((x, y), radius) = cv2.minEnclosingCircle(c) + M = cv2.moments(c) + center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) + + # only proceed if the radius meets a minimum size + if radius > 10: + # draw the circle border + cv2.circle(image, (int(x), int(y)), int(radius), (0, 255, 0), 2) + + # and the centroid + cv2.circle(image, center, 5, (0, 255, 255), -1) + + return center + + def dameSonarVisual (): + ''' + Función que devuelve el array de puntos [X,Y] (Z=0) correspondiente al obstáculo detectado + ''' + cameraModel = loadPiCamCameraModel () + puntosFrontera = 0 + fronteraArray = numpy.zeros((ANCHO_IMAGEN,2), dtype = "float64") + + image = dameImagen () + hsvImg = cv2.cvtColor(image,cv2.COLOR_BGR2HSV) + bnImg = cv2.inRange(hsvImg, GREEN_MIN, GREEN_MAX) + + pixel = Punto2D() + pixelOnGround3D = Punto3D() + tmp2d = Punto2D() + puntosFrontera = 0 + j = 0 + + # Ground image: recorremos la imagen de abajo a arriba (i=filas) y de izquierda a derecha (j=columnas) + while (j < ANCHO_IMAGEN): # recorrido en columnas + i = LARGO_IMAGEN-1 + esFrontera = None + while ((i>=0) and (esFrontera == None)): # recorrido en filas + pos = i*ANCHO_IMAGEN+j # posicion actual + + pix = bnImg[i, j] # value 0 or 255 (frontera) + + if (pix != 0): # si no soy negro + esFrontera = True # lo damos por frontera en un principio, luego veremos + # Calculo los demás vecinos, para ver de qué color son... + c = j - 1 + row = i + v1 = row*ANCHO_IMAGEN+c + + if (not((c >= 0) and (c < ANCHO_IMAGEN) and + (row >= 0) and (row < LARGO_IMAGEN))): # si no es válido ponemos sus valores a 0 + pix = 0 + else: + pix = bnImg[row, c] + + if (esFrontera == True): # si NO SOY COLOR CAMPO y alguno de los vecinitos ES color campo... + pixel.x = j + pixel.y = i + pixel.h = 1 + + # obtenemos su backproject e intersección con plano Z en 3D + pixelOnGround3D = getIntersectionZ (pixel, cameraModel) + + # vamos guardando estos puntos frontera 3D para luego dibujarlos con PyGame + fronteraArray[puntosFrontera][0] = pixelOnGround3D.x + fronteraArray[puntosFrontera][1] = pixelOnGround3D.y + puntosFrontera = puntosFrontera + 1 + #print "Hay frontera en pixel [",i,",",j,"] que intersecta al suelo en [",pixelOnGround3D.x,",",pixelOnGround3D.y,",",pixelOnGround3D.z,"]" + + i = i - 1 + j = j + 5 + + return fronteraArray +======= + ''' + Controlador para el Robot PiBot de JdeRobot-Kids + ''' + def __init__(self): + # Libreria RPi.GPIO + import RPi.GPIO as GPIO + import pigpio # Libreria para manejar los servos + JdeRobotKids.__init__(self) + self._GPIO = GPIO + self._tipo = "PiBot" + self._dit = pigpio.pi() + ''' + props = Ice.createProperties() + props.setProperty("JdeRobotKids.Motors.Proxy", "Motors:default -h localhost -p 9999") + props.setProperty("JdeRobotKids.Motors.maxV", "1") + props.setProperty("JdeRobotKids.Motors.maxW", "0.5") + id = Ice.InitializationData() + id.properties = props + ic = Ice.initialize(id) + self.motors = Motors (ic, "JdeRobotKids.Motors") + ''' + + def moverServo(self, *args): + ''' + Función que hace girar al servo motor a un angulo dado como parámetro. + @type args: lista + @param args: lista de argumentos: + args[0]: puerto al que esta conectado el controlador del servo + args[1]: banco al que esta conectado el servo en el controlador + args[2]: angulo de giro del servo. 0-180 grados. ¡PROBAR GIRO ANTES DE MONTAR EL SERVO! + ''' + puerto = args[0] + banco = args[1] + angulo = args[2]/10 + + self._GPIO.setmode(GPIO.BOARD) #Ponemos la Raspberry en modo BOARD + self._GPIO.setup(puerto,GPIO.OUT) #Ponemos el pin args[0] como salida + p = self._GPIO.PWM(puerto,50) #Ponemos el pin args[0] en modo PWM y enviamos 50 pulsos por segundo + p.start(7.5) #Enviamos un pulso del 7.5% para centrar el servo + p.ChangeDutyCycle(angulo) + time.sleep(0.5) + + def avanzar(self, vel): + ''' + Función que hace avanzar al robot en línea recta a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de avance del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1700) + self._dit.set_servo_pulsewidth(puertoR, 1300) + + def retroceder(self, vel): + ''' + Función que hace retroceder al robot en línea recta a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de retroceso del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1300) + self._dit.set_servo_pulsewidth(puertoR, 1700) + + def parar(self): + ''' + Función que hace detenerse al robot. + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1550) + self._dit.set_servo_pulsewidth(puertoR, 1490) + + def girarIzquierda(self, vel): + ''' + Función que hace rotar al robot sobre sí mismo hacia la izquierda a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de giro del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1400) + self._dit.set_servo_pulsewidth(puertoR, 1490) + + def girarDerecha(self, vel): + ''' + Función que hace rotar al robot sobre sí mismo hacia la derecha a una velocidad dada como parámetro. + @type vel: entero + @param vel: velocidad de giro del robot (máximo 255) + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, 1600) + self._dit.set_servo_pulsewidth(puertoR, 1550) + + def move(self, velV, velW): + ''' + Función que hace avanzar y girar al robot al mismo tiempo, según las velocidades V,W dadas como parámetro. + + @type velV, velW: entero + @param velV, velW: velocidades de avance de motores izquierdo y derecho + ''' + # Puertos de datos para servos izquierdo y derecho + puertoL = 4 + puertoR = 18 + #servos = pigpio.pi() + self._dit.set_servo_pulsewidth(puertoL, velV) + self._dit.set_servo_pulsewidth(puertoR, velW) +>>>>>>> 9055aa8edfb064a945b84ff01e3ba62c7cccb222 + + def quienSoy(self): + print ("Yo soy un robot real PiBot") + + @property + def tipo(self): + return self._tipo + + @tipo.setter + def tipo(self, valor): + self._tipo = valor diff --git a/src/drivers/PiBot/real/piBot/progeo.py b/src/drivers/PiBot/real/piBot/progeo.py new file mode 100644 index 000000000..7e8b5d830 --- /dev/null +++ b/src/drivers/PiBot/real/piBot/progeo.py @@ -0,0 +1,533 @@ +import numpy + +DEBUG = 1 +ANCHO_IMAGEN = 320 +LARGO_IMAGEN = 240 +DEGTORAD = 3.1415926535897932 / 180 +GREEN_MIN = numpy.array([20, 50, 100],numpy.uint8)#numpy.array([48, 138, 138],numpy.uint8) +GREEN_MAX = numpy.array([90, 235, 210],numpy.uint8)#numpy.array([67, 177, 192],numpy.uint8) + +def pixel2optical (p2d): + aux = p2d.x + p2d.x = LARGO_IMAGEN-1-p2d.y + p2d.y = aux + p2d.h = 1 + + return p2d + +def getIntersectionZ (p2d, mycamera): + p3d = Punto3D () + res = Punto3D () + p2d_ = Punto2D () + + x = myCamera.position.x + y = myCamera.position.y + z = myCamera.position.z + + p2d_ = pixel2optical(p2d) + result, p3d = backproject(p2d_, myCamera) + + # Check division by zero + if((p3d.z-z) == 0.0): + res.h = 0.0 + return + + zfinal = 0. # Quiero que intersecte con el Plano Z = 0 + + # Linear equation (X-x)/(p3d.X-x) = (Y-y)/(p3d.Y-y) = (Z-z)/(p3d.Z-z) + xfinal = x + (p3d.x - x)*(zfinal - z)/(p3d.z-z) + yfinal = y + (p3d.y - y)*(zfinal - z)/(p3d.z-z) + + res.x = xfinal + res.y = yfinal + res.z = zfinal + res.h = 1.0 + + return res + +def loadPiCamCameraModel (): + myCamera = PinholeCamera () + # ------------------------------------------------------------- + # LOADING MATRICES: + # ------------------------------------------------------------- + thetaY = 59*DEGTORAD # considerando que la camara (en vertical) está rotada 90º sobre eje Y + thetaZ = 0*DEGTORAD # considerando que la camara (en vertical) está rotada 90º sobre eje Y + thetaX = 0*DEGTORAD # considerando que la camara (en vertical) está rotada 90º sobre eje Y + + R_y = numpy.array ([(numpy.cos(thetaY),0,-numpy.sin(thetaY)),(0,1,0),(numpy.sin(thetaY),0,numpy.cos(thetaY))]) # R is a 3x3 rotation matrix + R_z = numpy.array ([(numpy.cos(thetaZ),-numpy.sin(thetaZ),0),(numpy.sin(thetaZ),numpy.cos(thetaZ),0),(0,0,1)]) # R is a 3x3 rotation matrix + R_x = numpy.array ([(1,0,0),(0,numpy.cos(thetaX),numpy.sin(thetaX)),(0, -numpy.sin(thetaX),numpy.cos(thetaX))]) # R is a 3x3 rotation matrix + + R_subt = numpy.dot (R_y, R_z) + R_tot = numpy.dot (R_subt, R_x) + + T = numpy.array ([(1,0,0,0),(0,1,0,0),(0,0,1,-110)]) # T is a 3x4 traslation matrix + Res = numpy.dot (R_tot,T) + RT = numpy.append(Res, [[0,0,0,1]], axis=0) # RT is a 4x4 matrix + K = numpy.array ([(313.89382026,0,117.5728043,0),(0,316.64906146,158.04145907,0),(0,0,1,0)]) # K is a 3x4 matrix + # ------------------------------------------------------------- + + # ------------------------------------------------------------- + # LOADING BOTH CAMERA MODELS JUST TO TEST THEM + # ------------------------------------------------------------- + # A) PROGEO CAMERA + # ------------------------------------------------------------- + myCamera.position.x = 0 + myCamera.position.y = 0 + myCamera.position.z = -110 + myCamera.position.h = 1 + + # K intrinsec parameters matrix (values got from the PiCamCalibration.py) + myCamera.k11 = K[0,0] + myCamera.k12 = K[0,1] + myCamera.k13 = K[0,2] + myCamera.k14 = K[0,3] + + myCamera.k21 = K[1,0] + myCamera.k22 = K[1,1] + myCamera.k23 = K[1,2] + myCamera.k24 = K[1,3] + + myCamera.k31 = K[2,0] + myCamera.k32 = K[2,1] + myCamera.k33 = K[2,2] + myCamera.k34 = K[2,3] + + # RT rotation-traslation matrix + myCamera.rt11 = RT[0,0] + myCamera.rt12 = RT[0,1] + myCamera.rt13 = RT[0,2] + myCamera.rt14 = RT[0,3] + + myCamera.rt21 = RT[1,0] + myCamera.rt22 = RT[1,1] + myCamera.rt23 = RT[1,2] + myCamera.rt24 = RT[1,3] + + myCamera.rt31 = RT[2,0] + myCamera.rt32 = RT[2,1] + myCamera.rt33 = RT[2,2] + myCamera.rt34 = RT[2,3] + + myCamera.rt41 = RT[3,0] + myCamera.rt42 = RT[3,1] + myCamera.rt43 = RT[3,2] + myCamera.rt44 = RT[3,3] + + myCamera.fdistx = K[0,0] #myCamera.k11 + myCamera.fdisty = K[1,1] #myCamera.k22 + myCamera.u0 = K[0,2] #myCamera.k13 + myCamera.v0 = K[1,2] #myCamera.k23 + myCamera.rows = LARGO_IMAGEN + myCamera.columns = ANCHO_IMAGEN + +class Punto2D: + x = 0 + y = 0 + h = 0 + +class Punto3D: + x = 0 + y = 0 + z = 0 + h = 0 + +class PinholeCamera: + position = Punto3D() # camera 3d position in mm + foa = Punto3D() # camera 3d focus of attention in mm + roll = None # camera roll position angle in rads + fdistx = None # focus x distance in mm + fdisty = None # focus y distance in mm + u0 = None # pixels + v0 = None + skew = None #angle between the x and y pixel axes in rads + rows = None # image height in pixels + columns = None # image width in pixels + # camera K matrix + k11 = None + k12 = None + k13 = None + k14 = None + k21 = None + k22 = None + k23 = None + k24 = None + k31 = None + k32 = None + k33 = None + k34 = None + # camera rotation + translation matrix + rt11 = None + rt12 = None + rt13 = None + rt14 = None + rt21 = None + rt22 = None + rt23 = None + rt24 = None + rt31 = None + rt32 = None + rt33 = None + rt34 = None + rt41 = None + rt42 = None + rt43 = None + rt44 = None + # distortion parameters + d1 = None + d2 = None + d3 = None + d4 = None + d5 = None + d6 = None + dx = None + dy = None + + def printCameraInfo (self): + print "------------------------------------------------------" + print "PROGEO-MODEL CAMERA INFO" + print " Position: (X,Y,Z,H)= ",self.position.x,self.position.y,self.position.z,self.position.h + print " Focus of Attention: (x,y,z,h)= ",self.foa.x,self.foa.y,self.foa.z,self.foa.h + print " Focus DistanceX(vertical): ",self.fdistx + print " Focus DistanceY(horizontal): ",self.fdisty + print " Skew: ",self.skew + print " Optical Center: (u0,v0)= ",self.u0,self.v0 + print " K Matrix: ",self.k11,self.k12,self.k13,self.k14 + print " ",self.k21,self.k22,self.k23,self.k24 + print " ",self.k31,self.k32,self.k33,self.k34 + print " RT Matrix: ",self.rt11,self.rt12,self.rt13,self.rt14 + print " ",self.rt21,self.rt22,self.rt23,self.rt24 + print " ",self.rt31,self.rt32,self.rt33,self.rt34 + print " ",self.rt41,self.rt42,self.rt43,self.rt44 + print "------------------------------------------------------\n" + +def project (punto3D, camera): + punto2D = Punto2D() + + # returns -1 if the point lies behind the camera, returns 1 if "punto3D" 3Dpoint projects into a 2D finite point, 0 otherwise + a1 = camera.rt11*punto3D.x+camera.rt12*punto3D.y+camera.rt13*punto3D.z+camera.rt14*punto3D.h + a2 = camera.rt21*punto3D.x+camera.rt22*punto3D.y+camera.rt23*punto3D.z+camera.rt24*punto3D.h + a3 = camera.rt31*punto3D.x+camera.rt32*punto3D.y+camera.rt33*punto3D.z+camera.rt34*punto3D.h + a4 = camera.rt41*punto3D.x+camera.rt42*punto3D.y+camera.rt43*punto3D.z+camera.rt44*punto3D.h + + punto2D.x=camera.k11*a1+camera.k12*a2+camera.k13*a3 + punto2D.y=camera.k21*a1+camera.k22*a2+camera.k23*a3 + punto2D.h=camera.k31*a1+camera.k32*a2+camera.k33*a3 + + if (punto2D.h!=0.): + punto2D.x=punto2D.x/punto2D.h + punto2D.y=punto2D.y/punto2D.h + punto2D.h=1. + + # a3/a4 is the number whose sign is interesting, but it has the same sign as a3*a4 and avoids to divide + if (a3*a4<=0.): + punto2D.h=-1. + output=-1 # point behind the focal plane + else: + output=1 + else: + output=0 + + return (output, punto2D) + +def backproject (punto2D, camera): + output = -1 + temp2D = Punto2D() + punto3D = Punto3D() + + if (punto2D.h>0.): + temp2D.h=camera.k11 + temp2D.x=punto2D.x*camera.k11/punto2D.h + temp2D.y=punto2D.y*camera.k11/punto2D.h + + ik11=(1./camera.k11) + ik12=-camera.k12/(camera.k11*camera.k22) + ik13=(camera.k12*camera.k23-camera.k13*camera.k22)/(camera.k22*camera.k11) + ik21=0. + ik22=(1./camera.k22) + ik23=-(camera.k23/camera.k22) + ik31=0. + ik32=0. + ik33=1. + + a1=ik11*temp2D.x+ik12*temp2D.y+ik13*temp2D.h + a2=ik21*temp2D.x+ik22*temp2D.y+ik23*temp2D.h + a3=ik31*temp2D.x+ik32*temp2D.y+ik33*temp2D.h + a4=1. + + ir11=camera.rt11 + ir12=camera.rt21 + ir13=camera.rt31 + ir14=0. + ir21=camera.rt12 + ir22=camera.rt22 + ir23=camera.rt32 + ir24=0. + ir31=camera.rt13 + ir32=camera.rt23 + ir33=camera.rt33 + ir34=0. + ir41=0. + ir42=0. + ir43=0. + ir44=1. + + b1=ir11*a1+ir12*a2+ir13*a3+ir14*a4 + b2=ir21*a1+ir22*a2+ir23*a3+ir24*a4 + b3=ir31*a1+ir32*a2+ir33*a3+ir34*a4 + b4=ir41*a1+ir42*a2+ir43*a3+ir44*a4 + + it11=1. + it12=0. + it13=0. + it14=camera.position.x + it21=0. + it22=1. + it23=0. + it24=camera.position.y + it31=0. + it32=0. + it33=1. + it34=camera.position.z + it41=0. + it42=0. + it43=0. + it44=1. + + punto3D.x=it11*b1+it12*b2+it13*b3+it14*b4 + punto3D.y=it21*b1+it22*b2+it23*b3+it24*b4 + punto3D.z=it31*b1+it32*b2+it33*b3+it34*b4 + punto3D.h=it41*b1+it42*b2+it43*b3+it44*b4 + + if (punto3D.h!=0.): + punto3D.x=punto3D.x/punto3D.h + punto3D.y=punto3D.y/punto3D.h + punto3D.z=punto3D.z/punto3D.h + punto3D.h=1. + output=1 + else: + output=0 + + return(output, punto3D) + +# if p1 and p2 can't be drawed in a camera.cols X camera.rows image, then it will return 0. Otherwise it will return 1 and gooda & goodb will be the correct points in the image to be drawn +# p1 and p2 are both 2dpoints, output are a and b, 2dpoints also +def displayline (p1, p2, camera): +# it takes care of important features: before/behind the focal plane, inside/outside the image frame + mycase=0 + papb=0. + + Xmin=0. + Xmax=camera.rows-1. + Ymin=0. + Ymax=camera.columns-1. + # Watchout!: they can't reach camera.rows or camera.columns, their are not valid values for the pixels + + l0.x=0. + l0.y=1. + l0.h=-Ymin + l1.x=0. + l1.y=1. + l1.h=-Ymax + l2.x=1. + l2.y=0. + l2.h=-Xmax + l3.x=1. + l3.y=0. + l3.h=-Xmin + + if ((p1.h<0.)and(p2.h<0.)): + # both points behind the focal plane: don't display anything + mycase=10 + else: + if ((p1.h>0.)and(p2.h<0.)): + # p1 before the focal plane, p2 behind + #Calculates p2 = p1 + -inf(p2-p1) + p2.x = p1.x + (-BIGNUM)*(p2.x-p1.x) + p2.y = p1.y + (-BIGNUM)*(p2.y-p1.y) + p2.h=-p2.h # undo the "project" trick to get the right value + elif ((p1.h<0.)and(p2.h>0.)): + # p2 before the focal plane, p1 behind + #Calculates p1 = p2 + -inf(p1-p2) + p1.x = p2.x + (-BIGNUM)*(p1.x-p2.x) + p1.y = p2.y + (-BIGNUM)*(p1.y-p2.y) + p1.h=-p1.h # undo the "project" trick to get the right value + + # both points before the focal plane + if ((p1.x>=Xmin) and (p1.x=Ymin) and (p1.y=Xmin) and (p2.x=Ymin) and (p2.y=Xmin) and (p1.x=Ymin) and (p1.y=Xmax+1) or (p2.y=Ymax+1))): + # p1 inside, p2 outside + gooda.x=p1.x + gooda.y=p1.y + gooda.h=p1.h + goodb.x=p1.x + goodb.y=p1.y + goodb.h=p1.h + pa.x=p1.x + pa.y=p1.y + pa.h=p1.h + pb.x=p2.x + pb.y=p2.y + pb.h=p2.h + mycase=3 + elif ((p2.x>=Xmin) and (p2.x=Ymin) and (p2.y=Xmax+1) or (p1.y=Ymax+1))): + # p2 inside, p1 outside + gooda.x=p2.x + gooda.y=p2.y + gooda.h=p2.h + goodb.x=p2.x + goodb.y=p2.y + goodb.h=p2.h + pa.x=p2.x + pa.y=p2.y + pa.h=p2.h + pb.x=p1.x + pb.y=p1.y + pb.h=p1.h + mycase=4 + else: + # both outside + pa.x=p2.x + pa.y=p2.y + pa.h=p2.h + pb.x=p1.x + pb.y=p1.y + pb.h=p1.h + mycase=5 + + l.x=pa.y*pb.h-pb.y*pa.h + l.y=pb.x*pa.h-pa.x*pb.h + l.h=pa.x*pb.y-pb.x*pa.y + i0.x=l.y*l0.h-l.h*l0.y + i0.y=l.h*l0.x-l.x*l0.h + i0.h=l.x*l0.y-l.y*l0.x + i1.x=l.y*l1.h-l.h*l1.y + i1.y=l.h*l1.x-l.x*l1.h + i1.h=l.x*l1.y-l.y*l1.x + i2.x=l.y*l2.h-l.h*l2.y + i2.y=l.h*l2.x-l.x*l2.h + i2.h=l.x*l2.y-l.y*l2.x + i3.x=l.y*l3.h-l.h*l3.y + i3.y=l.h*l3.x-l.x*l3.h + i3.h=l.x*l3.y-l.y*l3.x + + if (i0.h!=0.): + i0.x=i0.x/i0.h + i0.y=i0.y/i0.h + i0.h=1. + if (i1.h!=0.): + i1.x=i1.x/i1.h + i1.y=i1.y/i1.h + i1.h=1. + if (i2.h!=0.): + i2.x=i2.x/i2.h + i2.y=i2.y/i2.h + i2.h=1. + if (i3.h!=0.): + i3.x=i3.x/i3.h + i3.y=i3.y/i3.h + i3.h=1. + + papb=(pb.x-pa.x)*(pb.x-pa.x)+(pb.y-pa.y)*(pb.y-pa.y) + maxdot = -1 + + if (i0.h!=0.): + if ((i0.x>=Xmin) and (i0.x=Ymin) and (i0.y=0.) and (((pb.x-pa.x)*(i0.x-pa.x)+(pb.y-pa.y)*(i0.y-pa.y))maxdot)): + if ((mycase==3)or(mycase==4)or(mycase==6)): + goodb.x=i0.x + goodb.y=i0.y + goodb.h=i0.h + maxdot = (pb.x-pa.x)*(i0.x-pa.x)+(pb.y-pa.y)*(i0.y-pa.y) + elif (mycase==5): + gooda.x=i0.x + gooda.y=i0.y + gooda.h=i0.h + goodb.x=i0.x + goodb.y=i0.y + goodb.h=i0.h + mycase=6 + # else i0 at infinite, parallel lines + + if (i1.h!=0.): + if ((i1.x>=Xmin) and (i1.x=Ymin) and (i1.y=0.)and (((pb.x-pa.x)*(i1.x-pa.x)+(pb.y-pa.y)*(i1.y-pa.y))maxdot)): + if ((mycase==3)or(mycase==4)or(mycase==6)): + goodb.x=i1.x + goodb.y=i1.y + goodb.h=i1.h + maxdot = (pb.x-pa.x)*(i1.x-pa.x)+(pb.y-pa.y)*(i1.y-pa.y) + elif (mycase==5): + gooda.x=i1.x + gooda.y=i1.y + gooda.h=i1.h + goodb.x=i1.x + goodb.y=i1.y + goodb.h=i1.h + mycase=6 + # else i0 at infinite, parallel lines # i1 at infinite, parallel lines + + if (i2.h!=0.): + if ((i2.x>=Xmin) and (i2.x=Ymin) and (i2.y=0.)and (((pb.x-pa.x)*(i2.x-pa.x)+(pb.y-pa.y)*(i2.y-pa.y))maxdot)): + if ((mycase==3)or(mycase==4)or(mycase==6)): + goodb.x=i2.x + goodb.y=i2.y + goodb.h=i2.h + maxdot = (pb.x-pa.x)*(i2.x-pa.x)+(pb.y-pa.y)*(i2.y-pa.y) + elif (mycase==5): + gooda.x=i2.x + gooda.y=i2.y + gooda.h=i2.h + goodb.x=i2.x + goodb.y=i2.y + goodb.h=i2.h + mycase=6 + # else i0 at infinite, parallel lines # i2 at infinite, parallel lines + + if (i3.h!=0.): + if ((i3.x>=Xmin) and (i3.x=Ymin) and (i3.y=0.) and (((pb.x-pa.x)*(i3.x-pa.x)+(pb.y-pa.y)*(i3.y-pa.y))maxdot)): + if ((mycase==3)or(mycase==4)or(mycase==6)): + goodb.x=i3.x + goodb.y=i3.y + goodb.h=i3.h + maxdot = (pb.x-pa.x)*(i3.x-pa.x)+(pb.y-pa.y)*(i3.y-pa.y) + elif (mycase==5): + gooda.x=i3.x + gooda.y=i3.y + gooda.h=i3.h + goodb.x=i3.x + goodb.y=i3.y + goodb.h=i3.h + mycase=6 + # else i0 at infinite, parallel lines # i3 at infinite, parallel lines + + if (DEBUG==1): + print("p3: x=",p1.x," y=",p1.y," h=",p1.h,"\np2: x=",p2.x,", y=",p2.y," h=",p2.h,"\n") + print("case: ",mycase,"\n i0: x=",i0.x," y=",i0.y," h=",i0.h," dot=",((pb.x-pa.x)*(i0.x-pa.x)+(pb.y-pa.y)*(i0.y-pa.y)),"\n i1: x=",i1.x," y=",i1.y," h=",i1.h," dot=",((pb.x-pa.x)*(i1.x-pa.x)+(pb.y-pa.y)*(i1.y-pa.y)),"\n i2: x=",i2.x," y=",i2.y," h=",i2.h," dot=",((pb.x-pa.x)*(i2.x-pa.x)+(pb.y-pa.y)*(i2.y-pa.y)),"\n i3: x=",i3.x," y=",i3.y," z=",i3.h," dot=",((pb.x-pa.x)*(i3.x-pa.x)+(pb.y-pa.y)*(i3.y-pa.y)),"\n") + print("gooda: x=",gooda.x," y=",gooda.y," z=",gooda.h,"\n") + print("goodb: x=",goodb.x," y=",goodb.y," z=",goodb.h,"\n") + + a.x=gooda.x + b.x=goodb.x + a.y=gooda.y + b.y=goodb.y + a.h=gooda.h + b.h=goodb.h + + if((mycase!=2)and(mycase!=3)and(mycase!=4)and(mycase!=6)): + output = 0 + else: + output = 1 + + return (output, a, b) + diff --git a/src/drivers/PiBot/real/piBot/sensors/__init__.py b/src/drivers/PiBot/real/piBot/sensors/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/drivers/PiBot/real/piBot/sensors/cameraFilter.py b/src/drivers/PiBot/real/piBot/sensors/cameraFilter.py new file mode 100644 index 000000000..018ab3e92 --- /dev/null +++ b/src/drivers/PiBot/real/piBot/sensors/cameraFilter.py @@ -0,0 +1,120 @@ +# +# Copyright (C) 1997-2016 JDE Developers Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see http://www.gnu.org/licenses/. +# Authors : +# Alberto Martin Florido +# Aitor Martinez Fernandez +# +import numpy as np +import threading +import sys +sys.path.insert(0, '/usr/local/lib/python2.7/parallelIce') +from threadSensor import ThreadSensor +from cameraClient import CameraClient +import cv2 +from filters.rgbFilter import RgbFilter +from filters.yuvFilter import YuvFilter +from filters.hsvFilter import HsvFilter + +RGB = 'RGB' +HSV = 'HSV' +YUV = 'YUV' +ORIG = 'Orig' + + +class CameraFilter: + + def __init__(self, camera): + self.lock = threading.Lock() + self.client = camera + + img = self.client.getImage() + + self.height= img.height + self.width = img.width + + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self, self.kill_event) + self.thread.daemon = True + + rgbfilter = RgbFilter() + hsvfilter = HsvFilter() + yuvfilter = YuvFilter() + + self.filters = {RGB:rgbfilter, YUV:yuvfilter, HSV: hsvfilter} + + + if self.client.hasproxy(): + + trackImage = np.zeros((self.height, self.width,3), np.uint8) + trackImage.shape = self.height, self.width, 3 + + self.images = {ORIG: trackImage, RGB: trackImage, HSV: trackImage, YUV: trackImage} + + self.kill_event.clear() + self.thread.start() + + + # if client is stopped you can not start again, Threading.Thread raised error + def stop(self): + self.kill_event.set() + self.client.stop() + + + def getImage(self): + return getOrigImage() + + + def getFilteredImage(self, filt): + if self.client.hasproxy(): + img = np.zeros((self.height, self.width,3), np.uint8) + self.lock.acquire() + img = self.images[filt] + img.shape = self.images[filt].shape + self.lock.release() + return img + return None + + def getOrigImage(self): + return self.getFilteredImage(ORIG) + + def getHSVImage (self): + return self.getFilteredImage(HSV) + + def getRGBImage (self): + return self.getFilteredImage(RGB) + + def getYUVImage (self): + return self.getFilteredImage(YUV) + + + def update(self): + img = self.client.getImage().data + rgb = self.getFilter(RGB).apply(img) + hsv = self.getFilter(HSV).apply(img) + yuv = self.getFilter(YUV).apply(img) + + self.lock.acquire() + self.images[ORIG] = img + self.images[RGB] = rgb + self.images[HSV] = hsv + self.images[YUV] = yuv + self.lock.release() + + + + def getFilter (self, name): + return self.filters[name]