Moved kids interfaces to interfaces directory

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
 

src/drivers/PiBot/CMakeLists.txt

@@ -0,0 +1,18 @@
+# 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}/real/*py)
+FOREACH(header ${HEADERS_FILES})
+	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)

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: Ice # 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: 0 # 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
+

src/drivers/PiBot/PiBot.py

@@ -0,0 +1,28 @@
+import config
+
+
+def dameRobot():
+    cfg_file = "JdeRobotKids.yml"
+    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
+

src/drivers/PiBot/__init__.py

@@ -0,0 +1 @@
+from PiBot import dameRobot

src/drivers/PiBot/gazebo/__init__.py

@@ -0,0 +1 @@
+from .piBot import PiBot

src/drivers/PiBot/gazebo/piBot.py

@@ -0,0 +1,135 @@
+# -*- coding: utf-8 -*-
+from JdeRobotKids import JdeRobotKids
+import numpy
+import threading
+import sys
+import comm
+import config
+import cv2
+
+class PiBot:
+
+    '''
+    Controlador para el Robot PiBot de JdeRobot-Kids
+    '''
+    def __init__(self, cfg):
+
+        print("En constructor")
+        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")    
+
+
+    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)
+        '''
+        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)
+        '''
+        self.motors.sendV(-vel)
+
+    def parar(self):
+        '''
+        Función que hace detenerse al robot.
+        '''
+        self.motors.sendV(0)
+        self.motors.sendW(0)
+
+    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)
+        '''
+        self.motors.sendW(vel)
+
+
+    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)
+        '''
+        self.motors.sendW(-vel)
+
+    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
+        '''
+        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 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")
+
+    @property
+    def tipo(self):
+        return self._tipo
+
+    @tipo.setter
+    def tipo(self, valor):
+        self._tipo = valor

src/drivers/PiBot/real/__init__.py

@@ -0,0 +1 @@
+from .piBot import PiBot

src/drivers/PiBot/real/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

src/drivers/PiBot/real/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