From 73f1b52ad583a1ae0c30c8f63d4a7bb4f1383f0c Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Thu, 24 Jan 2019 12:25:44 +0100 Subject: [PATCH 1/3] refactored pibot driver --- .../PiBot/{JdeRobotKids.yml => Kibotics.yml} | 6 +- src/drivers/PiBot/{PiBotHAL.py => PiBot.py} | 11 +- src/drivers/PiBot/__init__.py | 2 +- src/drivers/PiBot/gazebo/piBot.py | 25 +- src/drivers/PiBot/real/piBot.py | 500 ++++++++++-------- src/drivers/PiBot/test.py | 2 +- 6 files changed, 299 insertions(+), 247 deletions(-) rename src/drivers/PiBot/{JdeRobotKids.yml => Kibotics.yml} (89%) rename src/drivers/PiBot/{PiBotHAL.py => PiBot.py} (65%) diff --git a/src/drivers/PiBot/JdeRobotKids.yml b/src/drivers/PiBot/Kibotics.yml similarity index 89% rename from src/drivers/PiBot/JdeRobotKids.yml rename to src/drivers/PiBot/Kibotics.yml index f95886514..95456969a 100644 --- a/src/drivers/PiBot/JdeRobotKids.yml +++ b/src/drivers/PiBot/Kibotics.yml @@ -1,9 +1,11 @@ -JdeRobotKids: - Robot: pibot #pibot, mbot or gazebo +Kibotics: + Robot: gazebo #pibot, mbot or gazebo Real: Port: /dev/ttyUSB0 Camera: PiCam + Dist_ruedas: 1 + Radio_ruedas: 1 Sim: Motors: diff --git a/src/drivers/PiBot/PiBotHAL.py b/src/drivers/PiBot/PiBot.py similarity index 65% rename from src/drivers/PiBot/PiBotHAL.py rename to src/drivers/PiBot/PiBot.py index 916acae3a..eb3339d6d 100644 --- a/src/drivers/PiBot/PiBotHAL.py +++ b/src/drivers/PiBot/PiBot.py @@ -1,4 +1,4 @@ -import config +from jderobot_config import config def dameRobot(): @@ -9,10 +9,9 @@ def dameRobot(): if bot == "pibot": # Import real Pibot wrapper - from .real import PiBot as pireal + from real import PiBot as pireal - cam = cfg.getProperty('JdeRobotKids.Real.Camera') - robot = pireal(cam) + robot = pireal(cfg) if bot == "mbot": port = cfg.getProperty('JdeRobotKids.Real.Port') @@ -20,9 +19,9 @@ def dameRobot(): elif bot == "gazebo": # Import simulated Pibot wrapper - from .gazebo import PiBot as pisim + from gazebo import PiBot as pisim - robot = pisim(cfg_file) + robot = pisim(cfg) return robot diff --git a/src/drivers/PiBot/__init__.py b/src/drivers/PiBot/__init__.py index 12440c994..48e08b508 100644 --- a/src/drivers/PiBot/__init__.py +++ b/src/drivers/PiBot/__init__.py @@ -1 +1 @@ -from .PiBotHAL import dameRobot +from .PiBot import dameRobot diff --git a/src/drivers/PiBot/gazebo/piBot.py b/src/drivers/PiBot/gazebo/piBot.py index 3a69e2911..04065bc92 100644 --- a/src/drivers/PiBot/gazebo/piBot.py +++ b/src/drivers/PiBot/gazebo/piBot.py @@ -1,5 +1,5 @@ # -*- coding: utf-8 -*- -from JdeRobotKids import JdeRobotKids +from jderobot_interfaces import Kibotics import numpy import threading import sys @@ -21,7 +21,7 @@ ORANGE_MAX = numpy.array([179, 255, 255],numpy.uint8)#numpy.array([67, 177, 192],numpy.uint8) -class PiBot: +class PiBot(Kibotics): ''' Controlador para el Robot PiBot de JdeRobot-Kids @@ -29,15 +29,16 @@ class PiBot: def __init__(self, cfg): print("En constructor") - cfg = config.load(cfg) + Kibotics.__init__(self) + #cfg = config.load(cfg) #starting comm - jdrc= comm.init(cfg, 'JdeRobotKids.Sim') - self.camera = jdrc.getCameraClient("JdeRobotKids.Sim.Camera") - self.motors = jdrc.getMotorsClient("JdeRobotKids.Sim.Motors") - self.irLeft = jdrc.getIRClient("JdeRobotKids.Sim.IRLeft") - self.irRight = jdrc.getIRClient("JdeRobotKids.Sim.IRRight") - self.us = jdrc.getSonarClient("JdeRobotKids.Sim.Sonar") + jdrc= comm.init(cfg, 'Kibotics.Sim') + self.camera = jdrc.getCameraClient("Kibotics.Sim.Camera") + self.motors = jdrc.getMotorsClient("Kibotics.Sim.Motors") + self.irLeft = jdrc.getIRClient("Kibotics.Sim.IRLeft") + self.irRight = jdrc.getIRClient("Kibotics.Sim.IRRight") + self.us = jdrc.getSonarClient("Kibotics.Sim.Sonar") def moverServo(self, *args): @@ -188,6 +189,12 @@ def dameObjeto(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False return center, area + def mostrarImagen(self): + None + + def dameSonarVisual(self): + None + def quienSoy(self): print ("Yo soy un robot simulado PiBot") diff --git a/src/drivers/PiBot/real/piBot.py b/src/drivers/PiBot/real/piBot.py index 32edd5524..99b0d90f8 100644 --- a/src/drivers/PiBot/real/piBot.py +++ b/src/drivers/PiBot/real/piBot.py @@ -1,5 +1,5 @@ # -*- coding: utf-8 -*- -from jderobot_interfaces import JdeRobotKids +from jderobot_interfaces import Kibotics import numpy import threading import sys, time @@ -9,7 +9,7 @@ import time import math import yaml -import RPi.GPIO as GPIO +#import RPi.GPIO as GPIO from imutils.video import VideoStream import imutils @@ -17,7 +17,7 @@ ORANGE_MIN = numpy.array([0, 123, 165],numpy.uint8)#numpy.array([48, 138, 138],numpy.uint8) ORANGE_MAX = numpy.array([179, 255, 255],numpy.uint8)#numpy.array([67, 177, 192],numpy.uint8) -class PiBot: +class PiBot(Kibotics): ''' Controlador para el Robot PiBot de JdeRobot-Kids @@ -25,16 +25,22 @@ class PiBot: - damePosicionDeObjetoDeColor - dameSonarVisual ''' - def __init__(self, camara): + def __init__(self, cfg): # Libreria RPi.GPIO print("real") import pigpio # Libreria para manejar los servos - JdeRobotKids.__init__(self) + Kibotics.__init__(self) + self._GPIO = GPIO self._tipo = "PiBot" self._dit = pigpio.pi() self._frame = None self._odometry_interval = 0.032 + + cam = cfg.getProperty('Kibotics.Real.Camera') + d = cfg.getProperty('Kibotics.Real.Dist_ruedas') + r = cfg.getProperty('Kibotics.Real.Radio_ruedas') + print(d,r) if camara == "PiCam": self._videostream = VideoStream(usePiCamera=True).start() @@ -42,7 +48,6 @@ def __init__(self, camara): self._videostream = VideoStream(usePiCamera=False).start() time.sleep(2) - self.lock = threading.Lock() #Variables de la posicion relativa del PiBot self.posx = 0 #posicion en x @@ -53,25 +58,30 @@ def __init__(self, camara): self.kill_event = threading.Event() #Creo un nuevo hilo que ejecutara el metodo 'readOdometry' - self.hilo = threading.Thread(target=self.readOdometry, args=(self.kill_event,)) + self.hilo = threading.Thread(target=self.readOdometry, args=(self.kill_event,d,r)) self.hilo.start() + class Angles: def __init__(self): prev = 0 actual = 0 dif = 0 - def readOdometry(self, kill_event): + + def readOdometry(self, kill_event,d,r): ''' Esta leyendo los angulos de ambos motores llamando a la funcion 'leerangulorueda' y va modificando las variables posx, posy, postheta ''' - config = yaml.load(open('config.yml')) + #config = yaml.load(open('config.yml')) #Constantes: FullCircle = 2 * math.pi #radianes - DistRuedas = float(config['Dist_Ruedas']) #metros - Rrueda = float(config['Radio_Rueda']) #Radio de la rueda + #DistRuedas = float(config['Dist_Ruedas']) #metros + #Rrueda = float(config['Radio_Rueda']) #Radio de la rueda + + DistRuedas = d + Rrueda = r #Pines de los encoders: EncoderL = 6 @@ -96,6 +106,7 @@ def readOdometry(self, kill_event): (angleizq.prev, angledcho.prev) = (angleizq.actual, angledcho.actual) #los angulos actuales son los previos para la siguiente vuelta time.sleep(sample_time) + def getOdometry(self): self.lock.acquire() aux1 = self.posx @@ -104,10 +115,12 @@ def getOdometry(self): self.lock.release() return (aux1, aux2, aux3) + def stopOdometry(self): #print("Parando...") self.kill_event.set() + def moverServo(self, *args): ''' Función que hace girar al servo motor a un angulo dado como parámetro. @@ -128,6 +141,7 @@ def moverServo(self, *args): 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. @@ -136,6 +150,7 @@ def avanzar(self, vel): ''' self.move(vel, 0) + def retroceder(self, vel): ''' Función que hace retroceder al robot en línea recta a una velocidad dada como parámetro. @@ -144,6 +159,7 @@ def retroceder(self, vel): ''' self.move(-vel, 0) + def parar(self): ''' Función que hace detenerse al robot. @@ -153,7 +169,9 @@ def parar(self): puertoR = 18 self._dit.set_servo_pulsewidth(puertoL, 1520) #parado 1525 self._dit.set_servo_pulsewidth(puertoR, 1520) #parado 1510 - def girarIzquierda(self): + + + def girarIzquierda(self, vel=None): ''' Función que hace rotar al robot sobre sí mismo hacia la izquierda a una velocidad dada como parámetro. @type vel: entero @@ -161,7 +179,8 @@ def girarIzquierda(self): ''' self.move(0, 1) - def girarDerecha(self): + + def girarDerecha(self, vel=None): ''' Función que hace rotar al robot sobre sí mismo hacia la derecha a una velocidad dada como parámetro. @type vel: entero @@ -169,6 +188,7 @@ def girarDerecha(self): ''' self.move(0, -1) + def moverHasta(self, pos): ''' Avanza o retrocede a una posicion dada @@ -207,12 +227,13 @@ def timeout_excedido(elapsed): #self.fin() self.parar() time.sleep(0.12) + + def girarHasta(self, angle): ''' Gira el robot un angulo determinado en sentido horario o antihorario dependiendo del signo de 'angle' ''' - ErrorRange = 0.2 Timeout = 10 @@ -277,6 +298,7 @@ def alcanzarposicion(angle_aux): self.parar() time.sleep(0.12) + def arcoHasta(self, x, y, theta): ''' Mueve el robot a la posicion relativa (x, y, theta) @@ -354,6 +376,7 @@ def fin(self): self.parar() self.stopOdometry() + 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. @@ -393,17 +416,245 @@ def move(self, velV, velW): self._movermotorizq(velV) self._movermotordcho(-velmotorgiro) - #AQUI EMPIEZAN LOS METODOS PRIVADOS + + def leerIRSigueLineas(self): + ''' + devuelve el estado de los sensores IR + ''' + right_sensor_port = 22 + left_sensor_port = 27 + + self._GPIO.setmode(self._GPIO.BCM) + self._GPIO.setup(right_sensor_port, self._GPIO.IN) + self._GPIO.setup(left_sensor_port, self._GPIO.IN) + right_value = self._GPIO.input(right_sensor_port) + left_value = self._GPIO.input(left_sensor_port) + #0: ambos sobre la linea + #1: izquierdo sobre la linea + #2: derecho sobre la linea + #3: ambos fuera de la linea + + if(right_value == 1 and left_value == 1): + state = 0 + elif(right_value == 0 and left_value == 1): + state = 1 + elif(right_value == 1 and left_value == 0): + state = 2 + else: + state = 3 + + return state + + + def leerUltrasonido(self): + ''' + devuelve la distancia a un objeto en metros + ''' + + inp = 3 + out = 2 + + self._GPIO.setwarnings(False) + self._GPIO.setmode(self._GPIO.BCM) + self._GPIO.setup(out, self._GPIO.OUT) + self._GPIO.setup(inp, self._GPIO.IN) + + self._GPIO.output(out, False) + time.sleep(0.00001) + self._GPIO.output(out, True) + time.sleep(0.00001) + self._GPIO.output(out, False) + start = time.time() + while(self._GPIO.input(inp) == 0): + start = time.time() + while(self._GPIO.input(inp) == 1): + stop = time.time() + elapsed = stop - start + + return (elapsed * 343) / 2 + + + def dameImagen (self): + ''' + devuelve una matriz que se corresponde con una imagen + ''' + 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 dameObjeto(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False): + ''' + 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 = self.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, ORANGE_MIN, ORANGE_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"])) + area = 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) + if showImageFiltered: + # Control waitKey from outside, only for local executions, not jupyter. + cv2.imshow("image_filtered", image) + cv2.imshow("mask", mask) + + return center, area + + + 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 + + + def quienSoy(self): + print ("Yo soy un robot real PiBot") + + + ################## + # METODOS PRIVADOS + ################## def _esnegativo(self, n): return n < 0 + def _espositivo(self, n): return n > 0 + def _escero(self, n): return n == 0 + + def _pulse_in(self, inp, bit): + ''' + inp: pin digital de entrada + bit: Nivel que quiero testear. 1(nivel alto) o 0(nivel bajo) + + Devuelve el ancho de pulso (en microsegundos) del estado especificado de la señal (alto o bajo) + ''' + + def readuntil(inp, bit): + rec = self._GPIO.input(inp) + if(rec == bit): + #esperar hasta terminar de leer unos + while(rec == bit): + rec = self._GPIO.input(inp) + #leer ceros hasta que me llegue el primer uno + if(bit == 1): + while(rec == 0): + rec = self._GPIO.input(inp) + #ahora me acaba de llegar el primer uno despues de los ceros + else: + while(rec == 1): + rec = self._GPIO.input(inp) + #ahora me acaba de llegar el primer cero despues de los unos + + if(bit != 0 and bit != 1): + return 0 + else: + readuntil(inp, bit) #leo hasta que me llega ese bit + start = time.time() #guardo la hora actual + if(bit == 1): + readuntil(inp, 0) #leo hasta que me llega un bit contrario al anterior + else: + readuntil(inp, 1) + finish = time.time() + elapsed = (finish - start) * 1000000 #tiempo en microsegundos + + return elapsed + + def _movermotorizq(self, vel): ''' Mueve el motor izquierdo a la velicidad dada como parametro @@ -441,6 +692,7 @@ def _movermotorizq(self, vel): else: #velocidad muy rapida self._dit.set_servo_pulsewidth(puertoL, 500) + def _movermotordcho(self, vel): ''' Mueve el motor derecho a la velicidad dada como parametro @@ -478,6 +730,7 @@ def _movermotordcho(self, vel): else: #velocidad muy rapida self._dit.set_servo_pulsewidth(puertoR, 1700) + def _leerangulorueda(self, Encoder): ''' Encoder: encoder del motor cuyo angulo se quiere conocer @@ -552,6 +805,7 @@ def initangle(): return angle + def _leerangulos(self, l, r): ''' l: Encoder del motor izquierdo @@ -561,6 +815,7 @@ def _leerangulos(self, l, r): ''' return (self._leerangulorueda(l), self._leerangulorueda(r)) + def _diferenciaangulos(self, izq, dcho, fullcircle): ''' Recibe dos objetos de la clase 'Angles' y devuelve la diferencia de angulos @@ -582,6 +837,7 @@ def actualesmenor(p, a): return (diferencia(izq.prev, izq.actual), diferencia(dcho.prev, dcho.actual)) + def _calcularposicion(self, difizq, difdcho, rrueda, distruedas): ''' Recibe la diferencia de angulo (en radianes) de cada motor (difizq y difdcho) y @@ -619,224 +875,12 @@ def diferencialtheta(difizq, difdcho): y = d * math.sin(dtheta) guardarposicion(x, y, dtheta) - def _pulse_in(self, inp, bit): - ''' - inp: pin digital de entrada - bit: Nivel que quiero testear. 1(nivel alto) o 0(nivel bajo) - - Devuelve el ancho de pulso (en microsegundos) del estado especificado de la señal (alto o bajo) - ''' - - def readuntil(inp, bit): - rec = self._GPIO.input(inp) - if(rec == bit): - #esperar hasta terminar de leer unos - while(rec == bit): - rec = self._GPIO.input(inp) - #leer ceros hasta que me llegue el primer uno - if(bit == 1): - while(rec == 0): - rec = self._GPIO.input(inp) - #ahora me acaba de llegar el primer uno despues de los ceros - else: - while(rec == 1): - rec = self._GPIO.input(inp) - #ahora me acaba de llegar el primer cero despues de los unos - - if(bit != 0 and bit != 1): - return 0 - else: - readuntil(inp, bit) #leo hasta que me llega ese bit - start = time.time() #guardo la hora actual - if(bit == 1): - readuntil(inp, 0) #leo hasta que me llega un bit contrario al anterior - else: - readuntil(inp, 1) - finish = time.time() - elapsed = (finish - start) * 1000000 #tiempo en microsegundos - - return elapsed - - def leerIRSigueLineas(self): - ''' - devuelve el estado de los sensores IR - ''' - right_sensor_port = 22 - left_sensor_port = 27 - - self._GPIO.setmode(self._GPIO.BCM) - self._GPIO.setup(right_sensor_port, self._GPIO.IN) - self._GPIO.setup(left_sensor_port, self._GPIO.IN) - right_value = self._GPIO.input(right_sensor_port) - left_value = self._GPIO.input(left_sensor_port) - #0: ambos sobre la linea - #1: izquierdo sobre la linea - #2: derecho sobre la linea - #3: ambos fuera de la linea - - if(right_value == 1 and left_value == 1): - state = 0 - elif(right_value == 0 and left_value == 1): - state = 1 - elif(right_value == 1 and left_value == 0): - state = 2 - else: - state = 3 - - return state - - def leerUltrasonido(self): - ''' - devuelve la distancia a un objeto en metros - ''' - - inp = 3 - out = 2 - - self._GPIO.setwarnings(False) - self._GPIO.setmode(self._GPIO.BCM) - self._GPIO.setup(out, self._GPIO.OUT) - self._GPIO.setup(inp, self._GPIO.IN) - - self._GPIO.output(out, False) - time.sleep(0.00001) - self._GPIO.output(out, True) - time.sleep(0.00001) - self._GPIO.output(out, False) - start = time.time() - while(self._GPIO.input(inp) == 0): - start = time.time() - while(self._GPIO.input(inp) == 1): - stop = time.time() - elapsed = stop - start - - return (elapsed * 343) / 2 - - def dameImagen (self): - ''' - devuelve una matriz que se corresponde con una imagen - ''' - 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 dameObjeto(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False): - ''' - 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 = self.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, ORANGE_MIN, ORANGE_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"])) - area = 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) - if showImageFiltered: - # Control waitKey from outside, only for local executions, not jupyter. - cv2.imshow("image_filtered", image) - cv2.imshow("mask", mask) - - return center, area - - 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 - - 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/test.py b/src/drivers/PiBot/test.py index 3c1394786..76ebd6d9e 100644 --- a/src/drivers/PiBot/test.py +++ b/src/drivers/PiBot/test.py @@ -1,7 +1,7 @@ #!/bin/python2 import sys, time -import PiBot +import PiBot if __name__ == "__main__": From 01908a147b8a2730e1f8932d8168fdfbde810e07 Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Thu, 24 Jan 2019 12:54:54 +0100 Subject: [PATCH 2/3] some changes in names --- src/drivers/PiBot/Kibotics.yml | 2 +- src/drivers/PiBot/PiBot.py | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/PiBot/Kibotics.yml b/src/drivers/PiBot/Kibotics.yml index 95456969a..dbc4be46b 100644 --- a/src/drivers/PiBot/Kibotics.yml +++ b/src/drivers/PiBot/Kibotics.yml @@ -1,5 +1,5 @@ Kibotics: - Robot: gazebo #pibot, mbot or gazebo + Robot: real #pibot, mbot or gazebo Real: Port: /dev/ttyUSB0 diff --git a/src/drivers/PiBot/PiBot.py b/src/drivers/PiBot/PiBot.py index eb3339d6d..2fbfa1390 100644 --- a/src/drivers/PiBot/PiBot.py +++ b/src/drivers/PiBot/PiBot.py @@ -2,9 +2,9 @@ def dameRobot(): - cfg_file = "JdeRobotKids.yml" + cfg_file = "Kibotics.yml" cfg = config.load(cfg_file) - bot = cfg.getProperty('JdeRobotKids.Robot') + bot = cfg.getProperty('Kibotics.Robot') bot = bot.lower() if bot == "pibot": @@ -14,8 +14,9 @@ def dameRobot(): robot = pireal(cfg) if bot == "mbot": - port = cfg.getProperty('JdeRobotKids.Real.Port') + #port = cfg.getProperty('Kibotics.Real.Port') #robot = MBot() + None elif bot == "gazebo": # Import simulated Pibot wrapper From 5118130a5a432a59aeb2fdeb83518155d53d34ed Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Thu, 24 Jan 2019 13:26:52 +0100 Subject: [PATCH 3/3] some changes in names --- src/drivers/PiBot/Kibotics.yml | 2 +- src/drivers/PiBot/PiBot.py | 2 +- src/drivers/PiBot/real/piBot.py | 2 +- src/interfaces/python/Kibotics.py | 74 +++++++++++++++++++++++++++++++ 4 files changed, 77 insertions(+), 3 deletions(-) create mode 100644 src/interfaces/python/Kibotics.py diff --git a/src/drivers/PiBot/Kibotics.yml b/src/drivers/PiBot/Kibotics.yml index dbc4be46b..a6b87ae30 100644 --- a/src/drivers/PiBot/Kibotics.yml +++ b/src/drivers/PiBot/Kibotics.yml @@ -1,5 +1,5 @@ Kibotics: - Robot: real #pibot, mbot or gazebo + Robot: pibot #pibot, mbot or gazebo Real: Port: /dev/ttyUSB0 diff --git a/src/drivers/PiBot/PiBot.py b/src/drivers/PiBot/PiBot.py index 2fbfa1390..ab47e93a9 100644 --- a/src/drivers/PiBot/PiBot.py +++ b/src/drivers/PiBot/PiBot.py @@ -16,7 +16,7 @@ def dameRobot(): if bot == "mbot": #port = cfg.getProperty('Kibotics.Real.Port') #robot = MBot() - None + None elif bot == "gazebo": # Import simulated Pibot wrapper diff --git a/src/drivers/PiBot/real/piBot.py b/src/drivers/PiBot/real/piBot.py index 99b0d90f8..b8a3a6b3a 100644 --- a/src/drivers/PiBot/real/piBot.py +++ b/src/drivers/PiBot/real/piBot.py @@ -4,7 +4,7 @@ import threading import sys, time import jderobot_config -import progeo +#import progeo import cv2 import time import math diff --git a/src/interfaces/python/Kibotics.py b/src/interfaces/python/Kibotics.py new file mode 100644 index 000000000..063616994 --- /dev/null +++ b/src/interfaces/python/Kibotics.py @@ -0,0 +1,74 @@ +#(c) JdeRobot-kids 2018 + +# -*- coding: utf-8 -*- +import abc +from abc import ABCMeta + + +class Kibotics(object): + __metaclass__ = ABCMeta + + @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") + + @abc.abstractmethod + def leerUltrasonido(self): + print ("leerUltrasonido abstracto") + + @abc.abstractmethod + def leerIRSigueLineas(self): + print ("leerIRSigueLineas 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) + + @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 dameObjeto(self, lower, upper, showImageFiltered): + print ("dameObjeto abstracto") +