From 7aac1a09539b7289a409d82b7e5138b1d3278b70 Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Fri, 13 Jul 2018 22:01:30 +0200 Subject: [PATCH 1/3] solving several problems with pibot api --- src/drivers/PiBot/JdeRobotKids.yml | 4 +- src/drivers/PiBot/gazebo/piBot.py | 62 +++++++++++++++++++++++++++++- src/drivers/PiBot/real/piBot.py | 33 ++++++++-------- src/drivers/PiBot/real/progeo.py | 1 + 4 files changed, 81 insertions(+), 19 deletions(-) diff --git a/src/drivers/PiBot/JdeRobotKids.yml b/src/drivers/PiBot/JdeRobotKids.yml index c8539ca4f..f95886514 100644 --- a/src/drivers/PiBot/JdeRobotKids.yml +++ b/src/drivers/PiBot/JdeRobotKids.yml @@ -1,5 +1,5 @@ JdeRobotKids: - Robot: gazebo #pibot, mbot or gazebo + Robot: pibot #pibot, mbot or gazebo Real: Port: /dev/ttyUSB0 @@ -30,4 +30,4 @@ JdeRobotKids: 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/gazebo/piBot.py b/src/drivers/PiBot/gazebo/piBot.py index b64311b3d..51399c8f0 100644 --- a/src/drivers/PiBot/gazebo/piBot.py +++ b/src/drivers/PiBot/gazebo/piBot.py @@ -7,6 +7,19 @@ import config import cv2 +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) + +BLUE_MIN = numpy.array([0, 255, 85],numpy.uint8)#numpy.array([104, 200, 42],numpy.uint8) +BLUE_MAX = numpy.array([179, 255, 255],numpy.uint8)#numpy.array([179, 255, 255],numpy.uint8) + +RED_MIN = numpy.array([163, 209, 30],numpy.uint8)#numpy.array([48, 138, 138],numpy.uint8) +RED_MAX = numpy.array([179, 255, 255],numpy.uint8)#numpy.array([67, 177, 192],numpy.uint8) + +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: ''' @@ -95,8 +108,9 @@ 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) + return img + #cv2.imshow("img", img) + #cv2.waitKey(0) def leerIRSigueLineas(self): ''' @@ -127,6 +141,50 @@ def leerUltrasonido(self): value = self.us.getSonarData().range return value + def damePosicionDeObjetoDeColor(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False): + + 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, lower, upper) + 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 + area = 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) + + return center, area + 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 e6d7ff17c..458d52743 100644 --- a/src/drivers/PiBot/real/piBot.py +++ b/src/drivers/PiBot/real/piBot.py @@ -1,13 +1,14 @@ # -*- coding: utf-8 -*- -from JdeRobotKids import JdeRobotKids -import Ice +from jderobot_interfaces import JdeRobotKids import numpy import threading import sys -import comm -import config +import jderobot_config import progeo +from imutils.video import VideoStream +import imutils + class PiBot: ''' @@ -26,9 +27,9 @@ def __init__(self, camara): self._tipo = "PiBot" self._dit = pigpio.pi() self._frame = None - if camara == "PiCam" + if camara == "PiCam": self._videostream = VideoStream(usePiCamera=True).start() - else + else: self._videostream = VideoStream(usePiCamera=False).start() ''' props = Ice.createProperties() @@ -251,14 +252,15 @@ def leerUltrasonido(self): #devuelve la distancia a un objeto en metros elapsed = stop - start return (elapsed * 343) / 2 - - 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 dameImagen (self): + self._frame = self._videostream.read() + self._frame = imutils.resize(self._frame, width=400) + + return self._frame + + def mostrarImagen (self): + cv2.imshow("Imagen", self._frame) def damePosicionDeObjetoDeColor(): ''' @@ -291,6 +293,7 @@ def damePosicionDeObjetoDeColor(): ((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: @@ -300,7 +303,7 @@ def damePosicionDeObjetoDeColor(): # and the centroid cv2.circle(image, center, 5, (0, 255, 255), -1) - return center + return center, area def dameSonarVisual (): ''' diff --git a/src/drivers/PiBot/real/progeo.py b/src/drivers/PiBot/real/progeo.py index 7e8b5d830..dead0d2a9 100644 --- a/src/drivers/PiBot/real/progeo.py +++ b/src/drivers/PiBot/real/progeo.py @@ -1,3 +1,4 @@ +# -*- coding: utf-8 -*- import numpy DEBUG = 1 From 37c9349ce8d7947718256d94c581ae086c45a6e9 Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Sat, 14 Jul 2018 15:59:42 +0200 Subject: [PATCH 2/3] Added fixed img width --- src/drivers/PiBot/gazebo/piBot.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/PiBot/gazebo/piBot.py b/src/drivers/PiBot/gazebo/piBot.py index 51399c8f0..e1bd55a07 100644 --- a/src/drivers/PiBot/gazebo/piBot.py +++ b/src/drivers/PiBot/gazebo/piBot.py @@ -6,6 +6,7 @@ import comm import config import cv2 +import imutils 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) @@ -108,6 +109,8 @@ def dameImagen(self): Función que muestra la imagen percibida por la camara ''' img = self.camera.getImage().data + img = imutils.resize(img, width=400) + return img #cv2.imshow("img", img) #cv2.waitKey(0) From 40160259fb98bfe6a1621ccba0c59e98c1b4c060 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20P=C3=A9rez?= Date: Tue, 17 Jul 2018 12:00:28 +0200 Subject: [PATCH 3/3] Update piBot.py --- src/drivers/PiBot/gazebo/piBot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/PiBot/gazebo/piBot.py b/src/drivers/PiBot/gazebo/piBot.py index e1bd55a07..3a69e2911 100644 --- a/src/drivers/PiBot/gazebo/piBot.py +++ b/src/drivers/PiBot/gazebo/piBot.py @@ -144,7 +144,7 @@ def leerUltrasonido(self): value = self.us.getSonarData().range return value - def damePosicionDeObjetoDeColor(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False): + def dameObjeto(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False): image = self.dameImagen() # convert to the HSV color space