From 80d2a95875d29752a5816f98f525f42b66a7d82e Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Fri, 1 Jun 2018 14:49:11 +0200 Subject: [PATCH 1/6] Moved kids interfaces to interfaces directory --- src/drivers/PiBot/real/JdeRobotKids.cfg | 18 --- src/drivers/PiBot/real/JdeRobotKids.py | 153 ------------------------ src/drivers/PiBot/real/piBot/piBot.py | 4 +- src/drivers/PiBot/real/piBot/test.py | 1 + src/interfaces/CMakeLists.txt | 2 + 5 files changed, 5 insertions(+), 173 deletions(-) delete mode 100644 src/drivers/PiBot/real/JdeRobotKids.cfg delete mode 100644 src/drivers/PiBot/real/JdeRobotKids.py create mode 100644 src/drivers/PiBot/real/piBot/test.py diff --git a/src/drivers/PiBot/real/JdeRobotKids.cfg b/src/drivers/PiBot/real/JdeRobotKids.cfg deleted file mode 100644 index 6fb29dac7..000000000 --- a/src/drivers/PiBot/real/JdeRobotKids.cfg +++ /dev/null @@ -1,18 +0,0 @@ -# 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 deleted file mode 100644 index 793957fa5..000000000 --- a/src/drivers/PiBot/real/JdeRobotKids.py +++ /dev/null @@ -1,153 +0,0 @@ -#(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/piBot.py b/src/drivers/PiBot/real/piBot/piBot.py index fc4d95e28..666a1e1cf 100644 --- a/src/drivers/PiBot/real/piBot/piBot.py +++ b/src/drivers/PiBot/real/piBot/piBot.py @@ -9,7 +9,7 @@ import progeo class PiBot: -<<<<<<< HEAD + ''' Controlador para el Robot PiBot de JdeRobot-Kids Actualmente incluye dos funciones de procesamiento visual: @@ -435,7 +435,7 @@ def move(self, velV, velW): #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") diff --git a/src/drivers/PiBot/real/piBot/test.py b/src/drivers/PiBot/real/piBot/test.py new file mode 100644 index 000000000..a94767f1f --- /dev/null +++ b/src/drivers/PiBot/real/piBot/test.py @@ -0,0 +1 @@ +from JdeRobotKids import JdeRobotKids diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index ee3bbb0d0..940344d6d 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -132,6 +132,8 @@ endif(NOT DEFINED SLICE_NEW_STYLE) # # ################### +INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/python/JdeRobotKids.py DESTINATION ${JDEROBOT_PYTHON_MODULE_PATH} COMPONENT interfaces) + INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/libJderobotInterfaces.so DESTINATION ${CMAKE_INSTALL_PREFIX}/lib COMPONENT interfaces ) # Install python files From 031490ff78fb0e96d4faa98084b3cc4bd4a5b215 Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Fri, 1 Jun 2018 18:30:23 +0200 Subject: [PATCH 2/6] Added first verison of simulated PiBot wrapper --- scripts/install_deps.sh | 2 +- .../PiBot/gazebo/piBot/.vscode/settings.json | 3 + .../PiBot/gazebo/piBot/JdeRobotKids.cfg | 18 + .../PiBot/gazebo/piBot/JdeRobotKids.yml | 43 ++ .../PiBot/gazebo/piBot/filters/__init__.py | 0 .../PiBot/gazebo/piBot/filters/hsvFilter.py | 71 +++ .../PiBot/gazebo/piBot/filters/rgbFilter.py | 71 +++ .../PiBot/gazebo/piBot/filters/yuvFilter.py | 76 +++ src/drivers/PiBot/gazebo/piBot/piBot.py | 95 ++++ src/drivers/PiBot/gazebo/piBot/progeo.py | 533 ++++++++++++++++++ .../PiBot/gazebo/piBot/sensors/__init__.py | 0 .../gazebo/piBot/sensors/cameraFilter.py | 120 ++++ src/drivers/PiBot/gazebo/piBot/test.py | 5 + .../pibot/{mbotMotors.cfg => pibotMotors.cfg} | 0 .../pibot/{mbotPose3D.cfg => pibotPose3D.cfg} | 0 .../pibot/{mbotSonar.cfg => pibotSonar.cfg} | 0 16 files changed, 1036 insertions(+), 1 deletion(-) create mode 100644 src/drivers/PiBot/gazebo/piBot/.vscode/settings.json create mode 100644 src/drivers/PiBot/gazebo/piBot/JdeRobotKids.cfg create mode 100644 src/drivers/PiBot/gazebo/piBot/JdeRobotKids.yml create mode 100644 src/drivers/PiBot/gazebo/piBot/filters/__init__.py create mode 100644 src/drivers/PiBot/gazebo/piBot/filters/hsvFilter.py create mode 100644 src/drivers/PiBot/gazebo/piBot/filters/rgbFilter.py create mode 100644 src/drivers/PiBot/gazebo/piBot/filters/yuvFilter.py create mode 100644 src/drivers/PiBot/gazebo/piBot/piBot.py create mode 100644 src/drivers/PiBot/gazebo/piBot/progeo.py create mode 100644 src/drivers/PiBot/gazebo/piBot/sensors/__init__.py create mode 100644 src/drivers/PiBot/gazebo/piBot/sensors/cameraFilter.py create mode 100644 src/drivers/PiBot/gazebo/piBot/test.py rename src/drivers/gazebo/plugins/pibot/{mbotMotors.cfg => pibotMotors.cfg} (100%) rename src/drivers/gazebo/plugins/pibot/{mbotPose3D.cfg => pibotPose3D.cfg} (100%) rename src/drivers/gazebo/plugins/pibot/{mbotSonar.cfg => pibotSonar.cfg} (100%) diff --git a/scripts/install_deps.sh b/scripts/install_deps.sh index 9da8baae7..cf5a6be50 100755 --- a/scripts/install_deps.sh +++ b/scripts/install_deps.sh @@ -7,7 +7,7 @@ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C sudo apt-get update -sudo apt install -y ros-kinetic-roscpp ros-kinetic-std-msgs ros-kinetic-cv-bridge ros-kinetic-image-transport ros-kinetic-roscpp-core ros-kinetic-rospy ros-kinetic-nav-msgs ros-kinetic-geometry-msgs ros-kinetic-opencv3 ros-kinetic-kobuki-gazebo ros-kinetic-usb_cam +sudo apt install -y ros-kinetic-roscpp ros-kinetic-std-msgs ros-kinetic-cv-bridge ros-kinetic-image-transport ros-kinetic-roscpp-core ros-kinetic-rospy ros-kinetic-nav-msgs ros-kinetic-geometry-msgs ros-kinetic-opencv3 ros-kinetic-kobuki-gazebo sudo apt-get install -y libpcl-dev diff --git a/src/drivers/PiBot/gazebo/piBot/.vscode/settings.json b/src/drivers/PiBot/gazebo/piBot/.vscode/settings.json new file mode 100644 index 000000000..26df38bf5 --- /dev/null +++ b/src/drivers/PiBot/gazebo/piBot/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.linting.enabled": false +} \ No newline at end of file diff --git a/src/drivers/PiBot/gazebo/piBot/JdeRobotKids.cfg b/src/drivers/PiBot/gazebo/piBot/JdeRobotKids.cfg new file mode 100644 index 000000000..bc8561584 --- /dev/null +++ b/src/drivers/PiBot/gazebo/piBot/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/gazebo/piBot/JdeRobotKids.yml b/src/drivers/PiBot/gazebo/piBot/JdeRobotKids.yml new file mode 100644 index 000000000..e18e5e394 --- /dev/null +++ b/src/drivers/PiBot/gazebo/piBot/JdeRobotKids.yml @@ -0,0 +1,43 @@ +piBot: + Motors: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Motors:default -h localhost -p 9999" + Topic: "/turtlebotROS/mobile_base/commands/velocity" + Name: basic_component_pyCamera + maxW: 0.3 + maxV: 0.5 + + Camera: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "picam:default -h localhost -p 8995" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: basic_component_pyCamera + + Pose3D: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Pose3D:default -h localhost -p 9001" + Topic: "/turtlebotROS/odom" + Name: testPose3d + + Sonar: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Sonar:default -h localhost -p 8993" + Topic: "/turtlebotROS/laser/scan" + Name: testSonar + + IRLeft: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "irLeft:default -h localhost -p 8991" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: basic_component_pyCamera + + IRRight: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "irRight:default -h localhost -p 8992" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: basic_component_pyCamera + + NodeName: piBot diff --git a/src/drivers/PiBot/gazebo/piBot/filters/__init__.py b/src/drivers/PiBot/gazebo/piBot/filters/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/drivers/PiBot/gazebo/piBot/filters/hsvFilter.py b/src/drivers/PiBot/gazebo/piBot/filters/hsvFilter.py new file mode 100644 index 000000000..a332601d7 --- /dev/null +++ b/src/drivers/PiBot/gazebo/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/gazebo/piBot/filters/rgbFilter.py b/src/drivers/PiBot/gazebo/piBot/filters/rgbFilter.py new file mode 100644 index 000000000..66d8e85e2 --- /dev/null +++ b/src/drivers/PiBot/gazebo/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/gazebo/piBot/filters/yuvFilter.py b/src/drivers/PiBot/gazebo/piBot/filters/yuvFilter.py new file mode 100644 index 000000000..b3c0ab999 --- /dev/null +++ b/src/drivers/PiBot/gazebo/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/gazebo/piBot/piBot.py b/src/drivers/PiBot/gazebo/piBot/piBot.py new file mode 100644 index 000000000..b02d5df89 --- /dev/null +++ b/src/drivers/PiBot/gazebo/piBot/piBot.py @@ -0,0 +1,95 @@ +# -*- coding: utf-8 -*- +from JdeRobotKids import JdeRobotKids +import Ice +import numpy +import threading +import sys +import comm +import config +# import progeo + +class PiBot: + + ''' + Controlador para el Robot PiBot de JdeRobot-Kids + ''' + def __init__(self): + + print("En constructor") + cfg = config.load("JdeRobotKids.yml") + + #starting comm + jdrc= comm.init(cfg, 'piBot') + # self.camera = jdrc.getCameraClient("piBot.Camera") + self.motors = jdrc.getMotorsClient("piBot.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! + ''' + None + + 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) + ''' + print("En avanzar") + self.motors.sendV(vel) + + 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) + ''' + print("En retroceder") + + def parar(self): + ''' + Función que hace detenerse al robot. + ''' + print("En parar") + img = self.camera.getImage().data + cv2.imshow("img", img) + + 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) + ''' + print("En girar izquierda") + + 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) + ''' + print("En girar derecha") + + 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 + ''' + print("En move") + + def quienSoy(self): + print ("Yo soy un robot simulado PiBot") + + @property + def tipo(self): + return self._tipo + + @tipo.setter + def tipo(self, valor): + self._tipo = valor diff --git a/src/drivers/PiBot/gazebo/piBot/progeo.py b/src/drivers/PiBot/gazebo/piBot/progeo.py new file mode 100644 index 000000000..7e8b5d830 --- /dev/null +++ b/src/drivers/PiBot/gazebo/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/gazebo/piBot/sensors/__init__.py b/src/drivers/PiBot/gazebo/piBot/sensors/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/drivers/PiBot/gazebo/piBot/sensors/cameraFilter.py b/src/drivers/PiBot/gazebo/piBot/sensors/cameraFilter.py new file mode 100644 index 000000000..018ab3e92 --- /dev/null +++ b/src/drivers/PiBot/gazebo/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] diff --git a/src/drivers/PiBot/gazebo/piBot/test.py b/src/drivers/PiBot/gazebo/piBot/test.py new file mode 100644 index 000000000..f0dd9e183 --- /dev/null +++ b/src/drivers/PiBot/gazebo/piBot/test.py @@ -0,0 +1,5 @@ +from piBot import PiBot + +if __name__=="__main__": + bot = PiBot() + bot.avanzar(1) \ No newline at end of file diff --git a/src/drivers/gazebo/plugins/pibot/mbotMotors.cfg b/src/drivers/gazebo/plugins/pibot/pibotMotors.cfg similarity index 100% rename from src/drivers/gazebo/plugins/pibot/mbotMotors.cfg rename to src/drivers/gazebo/plugins/pibot/pibotMotors.cfg diff --git a/src/drivers/gazebo/plugins/pibot/mbotPose3D.cfg b/src/drivers/gazebo/plugins/pibot/pibotPose3D.cfg similarity index 100% rename from src/drivers/gazebo/plugins/pibot/mbotPose3D.cfg rename to src/drivers/gazebo/plugins/pibot/pibotPose3D.cfg diff --git a/src/drivers/gazebo/plugins/pibot/mbotSonar.cfg b/src/drivers/gazebo/plugins/pibot/pibotSonar.cfg similarity index 100% rename from src/drivers/gazebo/plugins/pibot/mbotSonar.cfg rename to src/drivers/gazebo/plugins/pibot/pibotSonar.cfg From 6dc1f0aaa4bfdd6d4ffb31357ad4e22a9602823f Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Thu, 7 Jun 2018 14:29:00 +0200 Subject: [PATCH 3/6] Reorganizing and completing wrappers --- src/drivers/PiBot/JdeRobotKids.yml | 33 ++++++++ src/drivers/PiBot/PiBot.py | 27 +++++++ .../{gazebo/piBot/filters => }/__init__.py | 0 .../PiBot/gazebo/{piBot => }/JdeRobotKids.cfg | 0 .../PiBot/gazebo/{piBot => }/JdeRobotKids.yml | 0 src/drivers/PiBot/gazebo/__init__.py | 1 + .../{piBot/sensors => filters}/__init__.py | 0 .../gazebo/{piBot => }/filters/hsvFilter.py | 0 .../gazebo/{piBot => }/filters/rgbFilter.py | 0 .../gazebo/{piBot => }/filters/yuvFilter.py | 0 src/drivers/PiBot/gazebo/{piBot => }/piBot.py | 78 +++++++++++++------ .../PiBot/gazebo/piBot/.vscode/settings.json | 3 - .../PiBot/gazebo/{piBot => }/progeo.py | 0 .../filters => gazebo/sensors}/__init__.py | 0 .../{piBot => }/sensors/cameraFilter.py | 0 src/drivers/PiBot/gazebo/{piBot => }/test.py | 0 src/drivers/PiBot/real/__init__.py | 1 + .../{piBot/sensors => filters}/__init__.py | 0 .../real/{piBot => }/filters/hsvFilter.py | 0 .../real/{piBot => }/filters/rgbFilter.py | 0 .../real/{piBot => }/filters/yuvFilter.py | 0 src/drivers/PiBot/real/{piBot => }/piBot.py | 19 +++-- src/drivers/PiBot/real/piBot/test.py | 1 - src/drivers/PiBot/real/{piBot => }/progeo.py | 0 src/drivers/PiBot/real/sensors/__init__.py | 0 .../real/{piBot => }/sensors/cameraFilter.py | 0 src/drivers/PiBot/test.py | 18 +++++ 27 files changed, 147 insertions(+), 34 deletions(-) create mode 100644 src/drivers/PiBot/JdeRobotKids.yml create mode 100644 src/drivers/PiBot/PiBot.py rename src/drivers/PiBot/{gazebo/piBot/filters => }/__init__.py (100%) rename src/drivers/PiBot/gazebo/{piBot => }/JdeRobotKids.cfg (100%) rename src/drivers/PiBot/gazebo/{piBot => }/JdeRobotKids.yml (100%) create mode 100644 src/drivers/PiBot/gazebo/__init__.py rename src/drivers/PiBot/gazebo/{piBot/sensors => filters}/__init__.py (100%) rename src/drivers/PiBot/gazebo/{piBot => }/filters/hsvFilter.py (100%) rename src/drivers/PiBot/gazebo/{piBot => }/filters/rgbFilter.py (100%) rename src/drivers/PiBot/gazebo/{piBot => }/filters/yuvFilter.py (100%) rename src/drivers/PiBot/gazebo/{piBot => }/piBot.py (57%) delete mode 100644 src/drivers/PiBot/gazebo/piBot/.vscode/settings.json rename src/drivers/PiBot/gazebo/{piBot => }/progeo.py (100%) rename src/drivers/PiBot/{real/piBot/filters => gazebo/sensors}/__init__.py (100%) rename src/drivers/PiBot/gazebo/{piBot => }/sensors/cameraFilter.py (100%) rename src/drivers/PiBot/gazebo/{piBot => }/test.py (100%) create mode 100644 src/drivers/PiBot/real/__init__.py rename src/drivers/PiBot/real/{piBot/sensors => filters}/__init__.py (100%) rename src/drivers/PiBot/real/{piBot => }/filters/hsvFilter.py (100%) rename src/drivers/PiBot/real/{piBot => }/filters/rgbFilter.py (100%) rename src/drivers/PiBot/real/{piBot => }/filters/yuvFilter.py (100%) rename src/drivers/PiBot/real/{piBot => }/piBot.py (96%) delete mode 100644 src/drivers/PiBot/real/piBot/test.py rename src/drivers/PiBot/real/{piBot => }/progeo.py (100%) create mode 100644 src/drivers/PiBot/real/sensors/__init__.py rename src/drivers/PiBot/real/{piBot => }/sensors/cameraFilter.py (100%) create mode 100644 src/drivers/PiBot/test.py diff --git a/src/drivers/PiBot/JdeRobotKids.yml b/src/drivers/PiBot/JdeRobotKids.yml new file mode 100644 index 000000000..721349c4b --- /dev/null +++ b/src/drivers/PiBot/JdeRobotKids.yml @@ -0,0 +1,33 @@ +JdeRobotKids: + Robot: gazebo #pibot, mbot or gazebo + + Real: + Port: /dev/ttyUSB0 + Camera: PiCam + + Sim: + Motors: + Server: 1 # Deactivate, Ice , ROS + Proxy: "Motors:default -h localhost -p 9999" + maxW: 0.75 + maxV: 4 + + Camera: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "picam:default -h localhost -p 8995" + Format: RGB8 + + Sonar: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Sonar:default -h localhost -p 8993" + + IRLeft: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "irLeft:default -h localhost -p 8991" + Format: RGB8 + + IRRight: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "irRight:default -h localhost -p 8992" + Format: RGB8 + \ No newline at end of file diff --git a/src/drivers/PiBot/PiBot.py b/src/drivers/PiBot/PiBot.py new file mode 100644 index 000000000..6615b98f7 --- /dev/null +++ b/src/drivers/PiBot/PiBot.py @@ -0,0 +1,27 @@ +import config + + +def dameRobot(cfg_file): + cfg = config.load(cfg_file) + bot = cfg.getProperty('JdeRobotKids.Robot') + bot = bot.lower() + + if bot == "pibot": + # Import real Pibot wrapper + from real import PiBot as pireal + + cam = cfg.getProperty('JdeRobotKids.Real.Camera') + robot = pireal(cam) + + if bot == "mbot": + port = cfg.getProperty('JdeRobotKids.Real.Port') + #robot = MBot() + + elif bot == "gazebo": + # Import simulated Pibot wrapper + from gazebo import PiBot as pisim + + robot = pisim(cfg_file) + + return robot + diff --git a/src/drivers/PiBot/gazebo/piBot/filters/__init__.py b/src/drivers/PiBot/__init__.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/filters/__init__.py rename to src/drivers/PiBot/__init__.py diff --git a/src/drivers/PiBot/gazebo/piBot/JdeRobotKids.cfg b/src/drivers/PiBot/gazebo/JdeRobotKids.cfg similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/JdeRobotKids.cfg rename to src/drivers/PiBot/gazebo/JdeRobotKids.cfg diff --git a/src/drivers/PiBot/gazebo/piBot/JdeRobotKids.yml b/src/drivers/PiBot/gazebo/JdeRobotKids.yml similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/JdeRobotKids.yml rename to src/drivers/PiBot/gazebo/JdeRobotKids.yml diff --git a/src/drivers/PiBot/gazebo/__init__.py b/src/drivers/PiBot/gazebo/__init__.py new file mode 100644 index 000000000..93a45f824 --- /dev/null +++ b/src/drivers/PiBot/gazebo/__init__.py @@ -0,0 +1 @@ +from .piBot import PiBot diff --git a/src/drivers/PiBot/gazebo/piBot/sensors/__init__.py b/src/drivers/PiBot/gazebo/filters/__init__.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/sensors/__init__.py rename to src/drivers/PiBot/gazebo/filters/__init__.py diff --git a/src/drivers/PiBot/gazebo/piBot/filters/hsvFilter.py b/src/drivers/PiBot/gazebo/filters/hsvFilter.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/filters/hsvFilter.py rename to src/drivers/PiBot/gazebo/filters/hsvFilter.py diff --git a/src/drivers/PiBot/gazebo/piBot/filters/rgbFilter.py b/src/drivers/PiBot/gazebo/filters/rgbFilter.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/filters/rgbFilter.py rename to src/drivers/PiBot/gazebo/filters/rgbFilter.py diff --git a/src/drivers/PiBot/gazebo/piBot/filters/yuvFilter.py b/src/drivers/PiBot/gazebo/filters/yuvFilter.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/filters/yuvFilter.py rename to src/drivers/PiBot/gazebo/filters/yuvFilter.py diff --git a/src/drivers/PiBot/gazebo/piBot/piBot.py b/src/drivers/PiBot/gazebo/piBot.py similarity index 57% rename from src/drivers/PiBot/gazebo/piBot/piBot.py rename to src/drivers/PiBot/gazebo/piBot.py index b02d5df89..08d03859d 100644 --- a/src/drivers/PiBot/gazebo/piBot/piBot.py +++ b/src/drivers/PiBot/gazebo/piBot.py @@ -6,22 +6,25 @@ import sys import comm import config -# import progeo +import cv2 class PiBot: ''' Controlador para el Robot PiBot de JdeRobot-Kids ''' - def __init__(self): + def __init__(self, cfg): print("En constructor") - cfg = config.load("JdeRobotKids.yml") + cfg = config.load(cfg) #starting comm - jdrc= comm.init(cfg, 'piBot') - # self.camera = jdrc.getCameraClient("piBot.Camera") - self.motors = jdrc.getMotorsClient("piBot.Motors") + 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") + def moverServo(self, *args): ''' @@ -40,7 +43,6 @@ def avanzar(self, vel): @type vel: entero @param vel: velocidad de avance del robot (máximo 255) ''' - print("En avanzar") self.motors.sendV(vel) def retroceder(self, vel): @@ -49,15 +51,14 @@ def retroceder(self, vel): @type vel: entero @param vel: velocidad de retroceso del robot (máximo 255) ''' - print("En retroceder") + self.motors.sendV(-vel) def parar(self): ''' Función que hace detenerse al robot. ''' - print("En parar") - img = self.camera.getImage().data - cv2.imshow("img", img) + self.motors.sendV(0) + self.motors.sendW(0) def girarIzquierda(self, vel): ''' @@ -65,7 +66,8 @@ def girarIzquierda(self, vel): @type vel: entero @param vel: velocidad de giro del robot (máximo 255) ''' - print("En girar izquierda") + self.motors.sendW(vel) + def girarDerecha(self, vel): ''' @@ -73,7 +75,7 @@ def girarDerecha(self, vel): @type vel: entero @param vel: velocidad de giro del robot (máximo 255) ''' - print("En girar derecha") + self.motors.sendW(-vel) def move(self, velV, velW): ''' @@ -81,15 +83,47 @@ def move(self, velV, velW): @type velV, velW: entero @param velV, velW: velocidades de avance de motores izquierdo y derecho ''' - print("En move") + self.motors.sendV(vel) + self.motors.sendW(vel) + + def dameImagen(self): + ''' + Función que muestra la imagen percibida por la camara + ''' + img = self.camera.getImage().data + cv2.imshow("img", img) + cv2.waitKey(0) + + def leerIRSigueLineas(self): + ''' + Función que retorna las lecturas del sensor siguelineas de la siguiente forma: + 0: ambos sensores sobre la linea + 1: solo sensor izquierdo sobre la linea + 2: solo sensor derecho sobre la linea + 3: ambos sensores fuera de la linea + ''' + lft = self.irLeft.getIRData().received + rgt = self.irRight.getIRData().received + value = -1 + if lft == 1 and rgt == 1: + value = 0 + elif lft == 1 and rgt == 0: + value = 1 + elif lft == 0 and rgt == 1: + value = 2 + elif lft == 0 and rgt == 0: + value = 3 + + return value + - def quienSoy(self): - print ("Yo soy un robot simulado PiBot") + def quienSoy(self): + print ("Yo soy un robot simulado PiBot") - @property - def tipo(self): - return self._tipo + @property + def tipo(self): + return self._tipo - @tipo.setter - def tipo(self, valor): - self._tipo = valor + @tipo.setter + def tipo(self, valor): + self._tipo = valor diff --git a/src/drivers/PiBot/gazebo/piBot/.vscode/settings.json b/src/drivers/PiBot/gazebo/piBot/.vscode/settings.json deleted file mode 100644 index 26df38bf5..000000000 --- a/src/drivers/PiBot/gazebo/piBot/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "python.linting.enabled": false -} \ No newline at end of file diff --git a/src/drivers/PiBot/gazebo/piBot/progeo.py b/src/drivers/PiBot/gazebo/progeo.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/progeo.py rename to src/drivers/PiBot/gazebo/progeo.py diff --git a/src/drivers/PiBot/real/piBot/filters/__init__.py b/src/drivers/PiBot/gazebo/sensors/__init__.py similarity index 100% rename from src/drivers/PiBot/real/piBot/filters/__init__.py rename to src/drivers/PiBot/gazebo/sensors/__init__.py diff --git a/src/drivers/PiBot/gazebo/piBot/sensors/cameraFilter.py b/src/drivers/PiBot/gazebo/sensors/cameraFilter.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/sensors/cameraFilter.py rename to src/drivers/PiBot/gazebo/sensors/cameraFilter.py diff --git a/src/drivers/PiBot/gazebo/piBot/test.py b/src/drivers/PiBot/gazebo/test.py similarity index 100% rename from src/drivers/PiBot/gazebo/piBot/test.py rename to src/drivers/PiBot/gazebo/test.py diff --git a/src/drivers/PiBot/real/__init__.py b/src/drivers/PiBot/real/__init__.py new file mode 100644 index 000000000..93a45f824 --- /dev/null +++ b/src/drivers/PiBot/real/__init__.py @@ -0,0 +1 @@ +from .piBot import PiBot diff --git a/src/drivers/PiBot/real/piBot/sensors/__init__.py b/src/drivers/PiBot/real/filters/__init__.py similarity index 100% rename from src/drivers/PiBot/real/piBot/sensors/__init__.py rename to src/drivers/PiBot/real/filters/__init__.py diff --git a/src/drivers/PiBot/real/piBot/filters/hsvFilter.py b/src/drivers/PiBot/real/filters/hsvFilter.py similarity index 100% rename from src/drivers/PiBot/real/piBot/filters/hsvFilter.py rename to src/drivers/PiBot/real/filters/hsvFilter.py diff --git a/src/drivers/PiBot/real/piBot/filters/rgbFilter.py b/src/drivers/PiBot/real/filters/rgbFilter.py similarity index 100% rename from src/drivers/PiBot/real/piBot/filters/rgbFilter.py rename to src/drivers/PiBot/real/filters/rgbFilter.py diff --git a/src/drivers/PiBot/real/piBot/filters/yuvFilter.py b/src/drivers/PiBot/real/filters/yuvFilter.py similarity index 100% rename from src/drivers/PiBot/real/piBot/filters/yuvFilter.py rename to src/drivers/PiBot/real/filters/yuvFilter.py diff --git a/src/drivers/PiBot/real/piBot/piBot.py b/src/drivers/PiBot/real/piBot.py similarity index 96% rename from src/drivers/PiBot/real/piBot/piBot.py rename to src/drivers/PiBot/real/piBot.py index 38533fcf7..4824affb7 100644 --- a/src/drivers/PiBot/real/piBot/piBot.py +++ b/src/drivers/PiBot/real/piBot.py @@ -17,7 +17,10 @@ class PiBot: - dameSonarVisual ''' def __init__(self, camara): + None + ''' # Libreria RPi.GPIO + print("real") import RPi.GPIO as GPIO import pigpio # Libreria para manejar los servos JdeRobotKids.__init__(self) @@ -30,6 +33,7 @@ def __init__(self, camara): 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") @@ -123,7 +127,7 @@ def girarDerecha(self, vel): self._dit.set_servo_pulsewidth(puertoL, 1620) self._dit.set_servo_pulsewidth(puertoR, 1460) - def move(self, velV, velW): + 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. @@ -206,14 +210,13 @@ def move(self, velV, velW): 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 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 mostrarImagen (self): + cv2.imshow("Imagen", self._frame) def damePosicionDeObjetoDeColor(): ''' diff --git a/src/drivers/PiBot/real/piBot/test.py b/src/drivers/PiBot/real/piBot/test.py deleted file mode 100644 index a94767f1f..000000000 --- a/src/drivers/PiBot/real/piBot/test.py +++ /dev/null @@ -1 +0,0 @@ -from JdeRobotKids import JdeRobotKids diff --git a/src/drivers/PiBot/real/piBot/progeo.py b/src/drivers/PiBot/real/progeo.py similarity index 100% rename from src/drivers/PiBot/real/piBot/progeo.py rename to src/drivers/PiBot/real/progeo.py diff --git a/src/drivers/PiBot/real/sensors/__init__.py b/src/drivers/PiBot/real/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/sensors/cameraFilter.py similarity index 100% rename from src/drivers/PiBot/real/piBot/sensors/cameraFilter.py rename to src/drivers/PiBot/real/sensors/cameraFilter.py diff --git a/src/drivers/PiBot/test.py b/src/drivers/PiBot/test.py new file mode 100644 index 000000000..1cca79263 --- /dev/null +++ b/src/drivers/PiBot/test.py @@ -0,0 +1,18 @@ +import sys, time +import PiBot + +if __name__ == "__main__": + + cfg = sys.argv[1] + + robot = PiBot.dameRobot(cfg) + + # robot.avanzar(1) + # time.sleep(2) + # robot.parar() + # time.sleep(2) + # robot.retroceder(1) + # time.sleep(2) + # robot.dameImagen() + while True: + robot.leerIRSigueLineas() From 669c1489e0f53aed7654b9464deb72f90e16e65c Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Thu, 7 Jun 2018 15:02:25 +0200 Subject: [PATCH 4/6] Cleaning directory --- src/drivers/PiBot/JdeRobotKids.yml | 4 +-- src/drivers/PiBot/PiBot.py | 3 +- src/drivers/PiBot/__init__.py | 0 src/drivers/PiBot/gazebo/JdeRobotKids.cfg | 18 ---------- src/drivers/PiBot/gazebo/JdeRobotKids.yml | 43 ----------------------- src/drivers/PiBot/gazebo/piBot.py | 10 ++++-- src/drivers/PiBot/gazebo/test.py | 5 --- src/drivers/PiBot/real/piBot.py | 3 -- src/drivers/PiBot/test.py | 6 ++-- 9 files changed, 15 insertions(+), 77 deletions(-) delete mode 100644 src/drivers/PiBot/__init__.py delete mode 100644 src/drivers/PiBot/gazebo/JdeRobotKids.cfg delete mode 100644 src/drivers/PiBot/gazebo/JdeRobotKids.yml delete mode 100644 src/drivers/PiBot/gazebo/test.py diff --git a/src/drivers/PiBot/JdeRobotKids.yml b/src/drivers/PiBot/JdeRobotKids.yml index 721349c4b..c8539ca4f 100644 --- a/src/drivers/PiBot/JdeRobotKids.yml +++ b/src/drivers/PiBot/JdeRobotKids.yml @@ -7,7 +7,7 @@ JdeRobotKids: Sim: Motors: - Server: 1 # Deactivate, Ice , ROS + Server: Ice # Deactivate, Ice , ROS Proxy: "Motors:default -h localhost -p 9999" maxW: 0.75 maxV: 4 @@ -18,7 +18,7 @@ JdeRobotKids: Format: RGB8 Sonar: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "Sonar:default -h localhost -p 8993" IRLeft: diff --git a/src/drivers/PiBot/PiBot.py b/src/drivers/PiBot/PiBot.py index 6615b98f7..c0081a2a0 100644 --- a/src/drivers/PiBot/PiBot.py +++ b/src/drivers/PiBot/PiBot.py @@ -1,7 +1,8 @@ import config -def dameRobot(cfg_file): +def dameRobot(): + cfg_file = "JdeRobotKids.yml" cfg = config.load(cfg_file) bot = cfg.getProperty('JdeRobotKids.Robot') bot = bot.lower() diff --git a/src/drivers/PiBot/__init__.py b/src/drivers/PiBot/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/drivers/PiBot/gazebo/JdeRobotKids.cfg b/src/drivers/PiBot/gazebo/JdeRobotKids.cfg deleted file mode 100644 index bc8561584..000000000 --- a/src/drivers/PiBot/gazebo/JdeRobotKids.cfg +++ /dev/null @@ -1,18 +0,0 @@ -# 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/gazebo/JdeRobotKids.yml b/src/drivers/PiBot/gazebo/JdeRobotKids.yml deleted file mode 100644 index e18e5e394..000000000 --- a/src/drivers/PiBot/gazebo/JdeRobotKids.yml +++ /dev/null @@ -1,43 +0,0 @@ -piBot: - Motors: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "Motors:default -h localhost -p 9999" - Topic: "/turtlebotROS/mobile_base/commands/velocity" - Name: basic_component_pyCamera - maxW: 0.3 - maxV: 0.5 - - Camera: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "picam:default -h localhost -p 8995" - Format: RGB8 - Topic: "/TurtlebotROS/cameraL/image_raw" - Name: basic_component_pyCamera - - Pose3D: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "Pose3D:default -h localhost -p 9001" - Topic: "/turtlebotROS/odom" - Name: testPose3d - - Sonar: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "Sonar:default -h localhost -p 8993" - Topic: "/turtlebotROS/laser/scan" - Name: testSonar - - IRLeft: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "irLeft:default -h localhost -p 8991" - Format: RGB8 - Topic: "/TurtlebotROS/cameraL/image_raw" - Name: basic_component_pyCamera - - IRRight: - Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS - Proxy: "irRight:default -h localhost -p 8992" - Format: RGB8 - Topic: "/TurtlebotROS/cameraL/image_raw" - Name: basic_component_pyCamera - - NodeName: piBot diff --git a/src/drivers/PiBot/gazebo/piBot.py b/src/drivers/PiBot/gazebo/piBot.py index 08d03859d..9263721cc 100644 --- a/src/drivers/PiBot/gazebo/piBot.py +++ b/src/drivers/PiBot/gazebo/piBot.py @@ -1,6 +1,5 @@ # -*- coding: utf-8 -*- from JdeRobotKids import JdeRobotKids -import Ice import numpy import threading import sys @@ -23,7 +22,8 @@ def __init__(self, cfg): 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.irRight = jdrc.getIRClient("JdeRobotKids.Sim.IRRight") + self.us = jdrc.getSonarClient("JdeRobotKids.Sim.Sonar") def moverServo(self, *args): @@ -116,6 +116,12 @@ def leerIRSigueLineas(self): return value + def leerUltrasonido(self): + ''' + Función que retorna las lecturas del sensor ultrasonidos + ''' + value = self.us.getSonarData().range + return value def quienSoy(self): print ("Yo soy un robot simulado PiBot") diff --git a/src/drivers/PiBot/gazebo/test.py b/src/drivers/PiBot/gazebo/test.py deleted file mode 100644 index f0dd9e183..000000000 --- a/src/drivers/PiBot/gazebo/test.py +++ /dev/null @@ -1,5 +0,0 @@ -from piBot import PiBot - -if __name__=="__main__": - bot = PiBot() - bot.avanzar(1) \ No newline at end of file diff --git a/src/drivers/PiBot/real/piBot.py b/src/drivers/PiBot/real/piBot.py index 4824affb7..8465afa79 100644 --- a/src/drivers/PiBot/real/piBot.py +++ b/src/drivers/PiBot/real/piBot.py @@ -17,8 +17,6 @@ class PiBot: - dameSonarVisual ''' def __init__(self, camara): - None - ''' # Libreria RPi.GPIO print("real") import RPi.GPIO as GPIO @@ -33,7 +31,6 @@ def __init__(self, camara): 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") diff --git a/src/drivers/PiBot/test.py b/src/drivers/PiBot/test.py index 1cca79263..3c1394786 100644 --- a/src/drivers/PiBot/test.py +++ b/src/drivers/PiBot/test.py @@ -1,11 +1,11 @@ +#!/bin/python2 + import sys, time import PiBot if __name__ == "__main__": - cfg = sys.argv[1] - - robot = PiBot.dameRobot(cfg) + robot = PiBot.dameRobot() # robot.avanzar(1) # time.sleep(2) From 31d3077542fdb2a0ed3e342741dab4dd48b482b4 Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Tue, 12 Jun 2018 11:26:45 +0200 Subject: [PATCH 5/6] Added installation directives for Pibot. First approach, needs revision --- src/drivers/PiBot/CMakeLists.txt | 16 +++ src/drivers/PiBot/__init__.py | 1 + src/interfaces/python/JdeRobotKids.py | 154 ++++++++++++++++++++++++++ 3 files changed, 171 insertions(+) create mode 100644 src/drivers/PiBot/CMakeLists.txt create mode 100644 src/drivers/PiBot/__init__.py create mode 100644 src/interfaces/python/JdeRobotKids.py diff --git a/src/drivers/PiBot/CMakeLists.txt b/src/drivers/PiBot/CMakeLists.txt new file mode 100644 index 000000000..1efe1cc54 --- /dev/null +++ b/src/drivers/PiBot/CMakeLists.txt @@ -0,0 +1,16 @@ +# install Launcher +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/PiBot + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ + DESTINATION bin + COMPONENT pibot +) + +# Install .py +FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*py) +FOREACH(header ${HEADERS_FILES}) + INSTALL(FILES ${header} DESTINATION lib/python2.7/ COMPONENT pibot) +ENDFOREACH(header) + +# Install resources +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/JdeRobotKids.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT pibot) diff --git a/src/drivers/PiBot/__init__.py b/src/drivers/PiBot/__init__.py new file mode 100644 index 000000000..aa88c1a1d --- /dev/null +++ b/src/drivers/PiBot/__init__.py @@ -0,0 +1 @@ +from PiBot import dameRobot diff --git a/src/interfaces/python/JdeRobotKids.py b/src/interfaces/python/JdeRobotKids.py new file mode 100644 index 000000000..33a70251e --- /dev/null +++ b/src/interfaces/python/JdeRobotKids.py @@ -0,0 +1,154 @@ +#(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") + From fad64eb4002d923d100106e069755d22f3bef0b3 Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Tue, 12 Jun 2018 12:57:02 +0200 Subject: [PATCH 6/6] solved installation paths --- src/drivers/PiBot/CMakeLists.txt | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/drivers/PiBot/CMakeLists.txt b/src/drivers/PiBot/CMakeLists.txt index 1efe1cc54..f00fbc391 100644 --- a/src/drivers/PiBot/CMakeLists.txt +++ b/src/drivers/PiBot/CMakeLists.txt @@ -1,16 +1,18 @@ -# install Launcher -install( - FILES ${CMAKE_CURRENT_BINARY_DIR}/PiBot - PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ - DESTINATION bin - COMPONENT pibot -) +# Install .py +FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gazebo/*py) +FOREACH(header ${HEADERS_FILES}) + INSTALL(FILES ${header} DESTINATION lib/python2.7/PiBot/gazebo COMPONENT pibot) +ENDFOREACH(header) # Install .py -FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*py) +FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/real/*py) FOREACH(header ${HEADERS_FILES}) - INSTALL(FILES ${header} DESTINATION lib/python2.7/ COMPONENT pibot) + INSTALL(FILES ${header} DESTINATION /usr/local/lib/python2.7/dist-packages/PiBot COMPONENT pibot) ENDFOREACH(header) + # Install resources INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/JdeRobotKids.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT pibot) + +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/PiBot.py DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/python2.7/PiBot COMPONENT pibot) +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/__init__.py DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/python2.7/PiBot COMPONENT pibot)