def __init__(self):
self.ordenes = Ordenes()
self.accion_ant = None
self.accion_act = None
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.aruco_sub = rospy.Subscriber('/aruco_single/markerId',markerId,self.idCallback)
class GiraDer:
def __init__(self):
self.ordenes = Ordenes()
#Subscribers
self.giraDer_sub = rospy.Subscriber('corregirGiro', Vector3,
self.callback)
def callback(self, data):
angulo = data.x
if not self.ordenes.estaGirando():
self.ordenes.corregirYaw(angulo)
def __init__(self):
self.ordenes = Ordenes()
self.accion_ant = None
self.accion_act = None
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.ordenes = Ordenes()
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
self.pubGiro = rospy.Publisher('corregirGiro', Vector3, queue_size=1)
self.contLLamadas = 0
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.euler = None
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
#self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',Point,self.centerCallback)
#self.marker_points_sub = rospy.Subscriber('/aruco_single/markerPoints',points,self.pointsCallback)
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',
PoseStamped, self.poseCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.ReceiveNavdata)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.centered = False
self.command = Twist()
self.distAct = 1.5
self.markerDetected = False
self.center = None
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',Point,self.centerCallback)
#self.marker_points_sub = rospy.Subscriber('/aruco_single/markerPoints',points,self.pointsCallback)
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',PoseStamped,self.poseCallback)
self.marker_detected_sub = rospy.Subscriber('/aruco_single/markerDetected',Bool,self.markerDetectedCallback)
self.dist_sub = rospy.Subscriber('/getDistance/distance',Float32,self.distActCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.roll = 0
self.pitch = 0
self.yaw = 0
self.zVelocity = 0
self.cameraHeight = 0
self.cameraWidth = 0
self.command = Twist()
#Subscribers
self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw', Image,
self.imageCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.ReceiveNavdata)
self.subCameraInfo = rospy.Subscriber('/ardrone/camera_info',
CameraInfo,
self.cameraInfoCallback)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.ordenes = Ordenes()
self.accion_act = None
self.accion_ant = None
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
self.aruco_sub = rospy.Subscriber('/aruco_single/markerId', markerId,
self.idCallback)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
#self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.roll = 0
self.pitch = 0
self.yaw = 0
self.zVelocity = 0
self.centered = False
self.command = Twist()
self.distAct = 2
self.markerDetected = False
self.markerDetected_ant = False
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
#self.marker_points_sub = rospy.Subscriber('/aruco_single/markerPoints',points,self.pointsCallback)
self.ord_pub = rospy.Publisher('orden', String, 100)
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',
PoseStamped, self.distCallback)
self.marker_pose_sub = rospy.Subscriber('/aruco_single/markerDetected',
Bool,
self.markerDetectedCallback)
self.dist_sub = rospy.Subscriber('distance', Float32,
self.distActCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.ReceiveNavdata)
self.pubCommand = rospy.Publisher('/cmd_vel', Twist)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',
Point, self.centerCallback)
self.marker_detected_sub = rospy.Subscriber(
'/aruco_single/markerDetected', Bool, self.markerDetectedCallback)
self.manual_mode_sub = rospy.Subscriber(
'/joystick_controller/manualMode', Bool, self.manualModeCallback)
def __init__(self):
self.DEBUG = True
self.rate = rospy.Rate(10) #10Hz
self.ordenes = Ordenes()
self.bridge = CvBridge()
self.centrado = False
self.alineado = False
self.giros180 = 0
if self.DEBUG:
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',PoseStamped,self.distCallback)
self.dist = -1
#Subscriber para la imagen
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
#Subscribers para el centrado
self.arriba_sub = rospy.Subscriber('/arriba',Empty,self.arribaCallback)
self.abajo_sub = rospy.Subscriber('/abajo',Empty,self.abajoCallback)
self.izq_sub = rospy.Subscriber('/despIzq',Empty,self.izqCallback)
self.der_sub = rospy.Subscriber('/despDer',Empty,self.derCallback)
self.arribaIzq_sub = rospy.Subscriber('/arribaIzq',Empty,self.arribaIzqCallback)
self.arribaDer_sub = rospy.Subscriber('/arribaDer',Empty,self.arribaDerCallback)
self.abajoIzq_sub = rospy.Subscriber('/abajoIzq',Empty,self.abajoIzqCallback)
self.abajo_sub = rospy.Subscriber('/abajoDer',Empty,self.abajoDerCallback)
self.paraCentrar_sub = rospy.Subscriber('/paraCentrar',Empty,self.paraCentrarCallback)
self.centrado_sub = rospy.Subscriber('/centrado',Empty,self.centradoCallback)
#Subscribers para el avance/retroceso
self.avanza_sub = rospy.Subscriber('/avanza',Empty,self.avanzaCallback)
self.retrocede_sub = rospy.Subscriber('/retrocede',Empty,self.retrocedeCallback)
self.paraPitch_sub = rospy.Subscriber('/paraPitch',Empty,self.paraPitchCallback)
self.distCorrect_sub = rospy.Subscriber('/distCorrecta',Empty,self.distCorrectaCallback)
#Subscribers para la correccion del Yaw
self.yaw_izq_sub = rospy.Subscriber('/yawIzq',Empty,self.yawIzqCallback)
self.yaw_der_sub = rospy.Subscriber('/yawDer',Empty,self.yawDerCallback)
self.yaw_para_sub = rospy.Subscriber('/paraYaw',Empty,self.paraYawCallback)
self.yaw_correcto_sub = rospy.Subscriber('/yawCorrecto',Empty,self.yawCorrectoCallback)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',Point,self.centerCallback)
self.marker_detected_sub = rospy.Subscriber('/aruco_single/markerDetected',Bool,self.markerDetectedCallback)
self.manual_mode_sub = rospy.Subscriber('/joystick_controller/manualMode',Bool,self.manualModeCallback)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.ordenes = Ordenes()
self.accion_act = None
self.accion_ant = None
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.aruco_sub = rospy.Subscriber('/aruco_single/markerId',markerId,self.idCallback)
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.roll = 0
self.pitch = 0
self.yaw = 0
self.zVelocity = 0
self.cameraHeight = 0
self.cameraWidth = 0
self.command = Twist()
#Subscribers
self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw',Image,self.imageCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata)
self.subCameraInfo = rospy.Subscriber('/ardrone/camera_info',CameraInfo,self.cameraInfoCallback)
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.ordenes = Ordenes()
self.accion_act = None
self.accion_ant = None
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.aruco_sub = rospy.Subscriber('/aruco_single/markerId',markerId,self.idCallback)
# self.drone_image_sub = rospy.Subscriber('/ardrone/image_raw',markerId,self.detectColor)
def idCallback(self, data):
self.accion_act = data.markerId
if self.accion_ant != self.accion_act:
self.parser(data)
self.accion_ant = self.accion_act
def parser(self, data):
if data == constantes.MARGEN_IZQ:
print "ME DESPLAZO LATERALMENTE HACIA LA DERECHA"
self.ordenes.despLateralDer()
elif data == constantes.MARGEN_DER:
print "ME DESPLAZO LATERALMENTE HACIA LA IZQUIERDA"
self.ordenes.despLateralIzq()
def sonido(self, sonido):
pygame.mixer.init()
sound = pygame.mixer.Sound(sonido)
sound.play()
def angulo(self, x1, y1, x2, y2):
if x1 != x2:
return math.degrees(math.atan((y2-y1)/(x2-x1)))
else:
return 90
def maxMinXY(self, contours):
if len(contours) > 0:
i = 1
x_max = contours[0][0][0][0]
x_min = contours[0][0][0][0]
y_max = contours[0][0][0][1]
y_min = contours[0][0][0][1]
while i < len(contours[0]):
if x_max < contours[0][i][0][0]:
x_max = contours[0][i][0][0]
if x_min > contours[0][i][0][0]:
x_min = contours[0][i][0][0]
if y_max < contours[0][i][0][1]:
y_max = contours[0][i][0][1]
if y_min > contours[0][i][0][1]:
y_min = contours[0][i][0][1]
i = i + 1
return (x_max, x_min, y_max, y_min)
else:
return None
def longSegment(self, x1, x2, y1, y2):
return math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))
def maxLine(self, lines):
max_long = -1
line = None
if len(lines) > 0:
for x1,y1,x2,y2 in lines[0]:
longSe = self.longSegment(x1, x2, y1, y2)
if longSe > max_long:
max_long = longSe
line = (x1, y1, x2, y2)
return line
def calculaGiro(self, ang):
return 90 - abs(ang)
def grabarImagenes(self, img, the):
if self.contImg < 10:
cv2.imwrite('salida_000'+str(self.contImg)+'.jpeg', img)
elif self.contImg < 100:
cv2.imwrite('salida_00'+str(self.contImg)+'.jpeg', img)
elif self.contImg < 1000:
cv2.imwrite('salida_0'+str(self.contImg)+'.jpeg', img)
else:
cv2.imwrite('salida_'+str(self.contImg)+'.jpeg', img)
if self.contImg < 10:
cv2.imwrite('canny_000'+str(self.contImg)+'.jpeg', the)
elif self.contImg < 100:
cv2.imwrite('canny_00'+str(self.contImg)+'.jpeg', the)
elif self.contImg < 1000:
cv2.imwrite('canny_0'+str(self.contImg)+'.jpeg', the)
else:
cv2.imwrite('canny_'+str(self.contImg)+'.jpeg', the)
self.contImg = self.contImg + 1
def dibujaMira(self, width, height, img):
cv2.line(img,(0,height/2),(width,height/2),(0,0,255),1)
cv2.line(img,(width/2,0),(width/2,height),(0,0,255),1)
cv2.circle(img,(width/2,height/2), 4, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 10, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 20, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 30, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 40, (0,0,255), 1)
def detectaContornos(self, bordes, img_orig):
contours, hierarchy = cv2.findContours(bordes,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(img_orig,contours,-1,(0,255,0),3)
def mensajePantalla(self, img, msg, x, y):
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(img,msg,(x,y), font, 0.5,(255,0,255),2)
def detectaLinea(self, bordes, img_orig):
minLineLength = 20
maxLineGap = 4
lines = cv2.HoughLinesP(bordes,1,np.pi/180,100,minLineLength,maxLineGap)
ang = None
puntoMedio = None
if lines != None:
line = self.maxLine(lines)
ang = self.angulo(line[0],line[1],line[2],line[3])
puntoMedio = ((line[2]-line[0])/2, (line[3]-line[1])/2)
cv2.line(img_orig,(line[0],line[1]),(line[2],line[3]),(255,0,0),3)
return (ang, puntoMedio)
def imageCallback(self,data):
try:
img = self.bridge.imgmsg_to_cv2(data, "bgr8")
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
height, width, depth = img.shape
thresh = 125
ret,the = cv2.threshold(gray,thresh,220,cv2.THRESH_BINARY)
self.detectaContornos(the, img)
result = self.detectaLinea(the, img)
if result[0] != None and result[1] != None:
ang = result[0]
xMedio = result[1][0]
umbral = 5
distanciaCentro = xMedio# - width/2
if math.fabs(distanciaCentro) > umbral:
if distanciaCentro > 0:
self.mensajePantalla(the, str(distanciaCentro), width/2, height/2)
elif distanciaCentro < 0:
self.mensajePantalla(the, str(distanciaCentro), width/2, height/2)
else:
self.mensajePantalla(the, "PARA", width/2, height/2)
self.dibujaMira(width, height, img)
except CvBridgeError, e:
print e
cv2.imshow("Threshold window", the)
cv2.imshow("Image window", img)
cv2.waitKey(3)
class Piloto:
def __init__(self):
self.DEBUG = True
self.rate = rospy.Rate(10) #10Hz
self.ordenes = Ordenes()
self.bridge = CvBridge()
self.centrado = False
self.alineado = False
self.giros180 = 0
if self.DEBUG:
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',PoseStamped,self.distCallback)
self.dist = -1
#Subscriber para la imagen
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
#Subscribers para el centrado
self.arriba_sub = rospy.Subscriber('/arriba',Empty,self.arribaCallback)
self.abajo_sub = rospy.Subscriber('/abajo',Empty,self.abajoCallback)
self.izq_sub = rospy.Subscriber('/despIzq',Empty,self.izqCallback)
self.der_sub = rospy.Subscriber('/despDer',Empty,self.derCallback)
self.arribaIzq_sub = rospy.Subscriber('/arribaIzq',Empty,self.arribaIzqCallback)
self.arribaDer_sub = rospy.Subscriber('/arribaDer',Empty,self.arribaDerCallback)
self.abajoIzq_sub = rospy.Subscriber('/abajoIzq',Empty,self.abajoIzqCallback)
self.abajo_sub = rospy.Subscriber('/abajoDer',Empty,self.abajoDerCallback)
self.paraCentrar_sub = rospy.Subscriber('/paraCentrar',Empty,self.paraCentrarCallback)
self.centrado_sub = rospy.Subscriber('/centrado',Empty,self.centradoCallback)
#Subscribers para el avance/retroceso
self.avanza_sub = rospy.Subscriber('/avanza',Empty,self.avanzaCallback)
self.retrocede_sub = rospy.Subscriber('/retrocede',Empty,self.retrocedeCallback)
self.paraPitch_sub = rospy.Subscriber('/paraPitch',Empty,self.paraPitchCallback)
self.distCorrect_sub = rospy.Subscriber('/distCorrecta',Empty,self.distCorrectaCallback)
#Subscribers para la correccion del Yaw
self.yaw_izq_sub = rospy.Subscriber('/yawIzq',Empty,self.yawIzqCallback)
self.yaw_der_sub = rospy.Subscriber('/yawDer',Empty,self.yawDerCallback)
self.yaw_para_sub = rospy.Subscriber('/paraYaw',Empty,self.paraYawCallback)
self.yaw_correcto_sub = rospy.Subscriber('/yawCorrecto',Empty,self.yawCorrectoCallback)
def distCallback(self, poseS):
self.dist = poseS.pose.position.z
def yawIzqCallback(self, data):
if self.centrado:
self.alineado = False
self.ordenes.yawIzq()
def yawDerCallback(self, data):
if self.centrado:
self.alineado = False
self.ordenes.yawDer()
def paraYawCallback(self, data):
self.alineado = False
self.ordenes.paraYaw()
def yawCorrectoCallback(self, data):
self.alineado = True
self.ordenes.paraYaw()
def avanzaCallback(self, data):
if self.alineado:
self.ordenes.avanza(0.15)
self.giros180 = 0
#print 'Avanza'
def retrocedeCallback(self, data):
if self.alineado:
self.ordenes.retrocede(0.15)
self.giros180 = 0
#print 'Retrocede'
def paraPitchCallback(self, data):
self.ordenes.paraPitch()
self.giros180 = 0
#print 'Para Pitch'
def distCorrectaCallback(self, data):
if self.alineado:
self.ordenes.paraPitch()
if self.giros180 == 0:
self.ordenes.gira180()
self.giros180 = self.giros180 + 1
print 'Distancia correcta'
def centradoCallback(self, data):
self.ordenes.paraCentrar()
self.centrado = True
print 'Centrado correctamente'
def arribaCallback(self, data):
self.ordenes.sube()
self.centrado = False
print 'Arriba'
def abajoCallback(self, data):
self.ordenes.baja()
self.centrado = False
print 'Abajo'
def izqCallback(self, data):
self.ordenes.despLateralIzq()
self.centrado = False
print 'Izquierda'
def derCallback(self, data):
self.ordenes.despLateralDer()
self.centrado = False
print 'Derecha'
def arribaIzqCallback(self, data):
self.ordenes.subeIzq()
self.centrado = False
print 'Arriba Izquierda'
def arribaDerCallback(self, data):
self.ordenes.subeDer()
self.centrado = False
print 'Arriba Derecha'
def abajoIzqCallback(self, data):
self.ordenes.bajaIzq()
self.centrado = False
print 'Abajo Izquierda'
def abajoDerCallback(self, data):
self.ordenes.bajaDer()
self.centrado = False
print 'Abajo Derecha'
def paraCentrarCallback(self, data):
self.ordenes.paraCentrar()
self.centrado = False
#print 'Para de centrar'
def dibujaMira(self, width, height, img):
cv2.line(img,(0,height/2),(width,height/2),(0,0,255),1)
cv2.line(img,(width/2,0),(width/2,height),(0,0,255),1)
cv2.circle(img,(width/2,height/2), 4, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 10, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 20, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 30, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 40, (0,0,255), 1)
def imageCallback(self,data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
self.dibujaMira(width, height, self.img)
if self.DEBUG:
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.img,'Dist: ' + "%0.2f" % self.dist ,(0,25), font, 0.75,(255,255,255),1,255)
cv2.putText(self.img,'Centrado: ' + str(self.centrado) ,(0,50), font, 0.75,(255,255,255),1,255)
#cv2.putText(self.img,'Alineado: ' + str(self.alineado) ,(0,75), font, 0.75,(255,255,255),1,255)
cv2.imshow("Piloto", self.img)
cv2.waitKey(3)
except CvBridgeError, e:
print e
class Piloto:
def __init__(self):
self.DEBUG = True
self.rate = rospy.Rate(10) #10Hz
self.ordenes = Ordenes()
self.bridge = CvBridge()
self.centrado = False
self.alineado = False
self.giros180 = 0
if self.DEBUG:
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',
PoseStamped,
self.distCallback)
self.dist = -1
#Subscriber para la imagen
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
#Subscribers para el centrado
self.arriba_sub = rospy.Subscriber('/arriba', Empty,
self.arribaCallback)
self.abajo_sub = rospy.Subscriber('/abajo', Empty, self.abajoCallback)
self.izq_sub = rospy.Subscriber('/despIzq', Empty, self.izqCallback)
self.der_sub = rospy.Subscriber('/despDer', Empty, self.derCallback)
self.arribaIzq_sub = rospy.Subscriber('/arribaIzq', Empty,
self.arribaIzqCallback)
self.arribaDer_sub = rospy.Subscriber('/arribaDer', Empty,
self.arribaDerCallback)
self.abajoIzq_sub = rospy.Subscriber('/abajoIzq', Empty,
self.abajoIzqCallback)
self.abajo_sub = rospy.Subscriber('/abajoDer', Empty,
self.abajoDerCallback)
self.paraCentrar_sub = rospy.Subscriber('/paraCentrar', Empty,
self.paraCentrarCallback)
self.centrado_sub = rospy.Subscriber('/centrado', Empty,
self.centradoCallback)
#Subscribers para el avance/retroceso
self.avanza_sub = rospy.Subscriber('/avanza', Empty,
self.avanzaCallback)
self.retrocede_sub = rospy.Subscriber('/retrocede', Empty,
self.retrocedeCallback)
self.paraPitch_sub = rospy.Subscriber('/paraPitch', Empty,
self.paraPitchCallback)
self.distCorrect_sub = rospy.Subscriber('/distCorrecta', Empty,
self.distCorrectaCallback)
#Subscribers para la correccion del Yaw
self.yaw_izq_sub = rospy.Subscriber('/yawIzq', Empty,
self.yawIzqCallback)
self.yaw_der_sub = rospy.Subscriber('/yawDer', Empty,
self.yawDerCallback)
self.yaw_para_sub = rospy.Subscriber('/paraYaw', Empty,
self.paraYawCallback)
self.yaw_correcto_sub = rospy.Subscriber('/yawCorrecto', Empty,
self.yawCorrectoCallback)
def distCallback(self, poseS):
self.dist = poseS.pose.position.z
def yawIzqCallback(self, data):
if self.centrado:
self.alineado = False
self.ordenes.yawIzq()
def yawDerCallback(self, data):
if self.centrado:
self.alineado = False
self.ordenes.yawDer()
def paraYawCallback(self, data):
self.alineado = False
self.ordenes.paraYaw()
def yawCorrectoCallback(self, data):
self.alineado = True
self.ordenes.paraYaw()
def avanzaCallback(self, data):
if self.alineado:
self.ordenes.avanza(0.15)
self.giros180 = 0
#print 'Avanza'
def retrocedeCallback(self, data):
if self.alineado:
self.ordenes.retrocede(0.15)
self.giros180 = 0
#print 'Retrocede'
def paraPitchCallback(self, data):
self.ordenes.paraPitch()
self.giros180 = 0
#print 'Para Pitch'
def distCorrectaCallback(self, data):
if self.alineado:
self.ordenes.paraPitch()
if self.giros180 == 0:
self.ordenes.gira180()
self.giros180 = self.giros180 + 1
print 'Distancia correcta'
def centradoCallback(self, data):
self.ordenes.paraCentrar()
self.centrado = True
print 'Centrado correctamente'
def arribaCallback(self, data):
self.ordenes.sube()
self.centrado = False
print 'Arriba'
def abajoCallback(self, data):
self.ordenes.baja()
self.centrado = False
print 'Abajo'
def izqCallback(self, data):
self.ordenes.despLateralIzq()
self.centrado = False
print 'Izquierda'
def derCallback(self, data):
self.ordenes.despLateralDer()
self.centrado = False
print 'Derecha'
def arribaIzqCallback(self, data):
self.ordenes.subeIzq()
self.centrado = False
print 'Arriba Izquierda'
def arribaDerCallback(self, data):
self.ordenes.subeDer()
self.centrado = False
print 'Arriba Derecha'
def abajoIzqCallback(self, data):
self.ordenes.bajaIzq()
self.centrado = False
print 'Abajo Izquierda'
def abajoDerCallback(self, data):
self.ordenes.bajaDer()
self.centrado = False
print 'Abajo Derecha'
def paraCentrarCallback(self, data):
self.ordenes.paraCentrar()
self.centrado = False
#print 'Para de centrar'
def dibujaMira(self, width, height, img):
cv2.line(img, (0, height / 2), (width, height / 2), (0, 0, 255), 1)
cv2.line(img, (width / 2, 0), (width / 2, height), (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 4, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 10, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 20, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 30, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 40, (0, 0, 255), 1)
def imageCallback(self, data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
self.dibujaMira(width, height, self.img)
if self.DEBUG:
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.img, 'Dist: ' + "%0.2f" % self.dist, (0, 25),
font, 0.75, (255, 255, 255), 1, 255)
cv2.putText(self.img, 'Centrado: ' + str(self.centrado),
(0, 50), font, 0.75, (255, 255, 255), 1, 255)
#cv2.putText(self.img,'Alineado: ' + str(self.alineado) ,(0,75), font, 0.75,(255,255,255),1,255)
cv2.imshow("Piloto", self.img)
cv2.waitKey(3)
except CvBridgeError, e:
print e
def __init__(self):
super(KeyboardController,self).__init__()
self.ordenes = Ordenes()
class lector:
def __init__(self):
self.ordenes = Ordenes()
self.accion_ant = None
self.accion_act = None
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.aruco_sub = rospy.Subscriber('/aruco_single/markerId',markerId,self.idCallback)
def parser(self, data):
if data == constantes.DESPEGA:
self.ordenes.despega()
elif data == constantes.ATERRIZA:
self.ordenes.aterriza()
elif data == constantes.GIRA_IZQ:
print 'giraIzq'
self.ordenes.gira90Izq()
elif data == constantes.GIRA_DER:
self.ordenes.gira90Der()
elif data == constantes.GIRA_180:
self.ordenes.gira180()
elif data == constantes.SUBE:
self.ordenes.sube(10)
elif data == constantes.BAJA:
self.ordenes.baja(10)
elif data == constantes.AVANZA:
self.ordenes.avanza()
elif data == constantes.NONE:
self.ordenes.para()
def idCallback(self, data):
self.accion_act = data.markerId
if self.accion_ant != self.accion_act:
self.parser(self.accion_act)
self.accion_ant = self.accion_act
def imageCallback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data)
except CvBridgeError, e:
print e
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.roll = 0
self.pitch = 0
self.yaw = 0
self.zVelocity = 0
self.cameraHeight = 0
self.cameraWidth = 0
self.command = Twist()
#Subscribers
self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw', Image,
self.imageCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.ReceiveNavdata)
self.subCameraInfo = rospy.Subscriber('/ardrone/camera_info',
CameraInfo,
self.cameraInfoCallback)
def cameraInfoCallback(self, info):
self.cameraHeight = info.height
self.cameraWidth = info.width
def ReceiveNavdata(self, navdata):
self.navdata = navdata
def calculaCoords(self, x1, y1):
x = int(self.cameraWidth * x1 / 1000)
y = int(self.cameraHeight * y1 / 1000)
return (x, y)
def corregirX(self, x):
umbral = 0
if x > self.cameraWidth + umbral:
self.ordenes.despLateralIzq()
elif x < self.cameraWidth - umbral:
self.ordenes.despLateralDer()
elif x < self.cameraWidth + umbral and x > self.cameraWidth - umbral:
self.ordenes.paraDespLateral()
def corregirY(self, y):
umbral = 0
if y > self.cameraHeight + umbral:
self.ordenes.avanza(0.15)
elif y < self.cameraHeight - umbral:
self.ordenes.retrocede(0.15)
elif y < self.cameraHeight + umbral and y > self.cameraHeight - umbral:
self.ordenes.paraPitch()
def corregirAngulo(self, angulo):
umbral = 10
if (angulo < 270 + umbral) and angulo > 270 - umbral:
self.ordenes.paraYaw()
elif angulo > 270 + umbral:
self.ordenes.yawDer()
elif angulo < 270 - umbral:
self.ordenes.yawIzq()
def dibujaMira(self, width, height, img):
cv2.line(img, (0, height / 2), (width, height / 2), (0, 0, 255), 1)
cv2.line(img, (width / 2, 0), (width / 2, height), (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 4, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 10, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 20, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 30, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 40, (0, 0, 255), 1)
def imageCallback(self, data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
auxNavdata = self.navdata
# print 'height ' + str(self.cameraHeight) + ' width ' + str(self.cameraWidth)
self.dibujaMira(width, height, self.img)
font = cv2.FONT_HERSHEY_SIMPLEX
if auxNavdata != None:
cv2.putText(self.img,
'Tags Detected ' + str(auxNavdata.tags_count),
(0, 25), font, 0.5, (255, 255, 255), 1, 255)
if auxNavdata.tags_count != 0:
cv2.putText(self.img,
'Tag Type ' + str(auxNavdata.tags_type[0]),
(0, 50), font, 0.5, (255, 255, 255), 1, 255)
cv2.putText(
self.img, 'Tag xc ' + str(auxNavdata.tags_xc[0]) +
' Tag yc ' + str(auxNavdata.tags_yc[0]), (0, 75), font,
0.5, (255, 255, 255), 1, 255)
pos = self.calculaCoords(auxNavdata.tags_xc[0],
auxNavdata.tags_yc[0])
self.corregirX(pos[0])
self.corregirY(pos[1])
#self.corregirAngulo(auxNavdata.tags_orientation[0])
cv2.putText(
self.img, 'Tag x pixels ' + str(pos[0]) +
' Tag y pixels ' + str(pos[1]), (0, 100), font, 0.5,
(255, 255, 255), 1, 255)
cv2.putText(
self.img, 'Tag orientation ' +
"%0.2f" % auxNavdata.tags_orientation[0], (0, 125),
font, 0.5, (255, 255, 255), 1, 255)
cv2.putText(self.img, 'Roll ' + str(self.roll), (0, 150),
font, 0.5, (255, 255, 255), 1, 255)
else:
self.ordenes.para()
cv2.imshow("Image window", self.img)
cv2.waitKey(3)
except CvBridgeError, e:
print e
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',
Point, self.centerCallback)
self.marker_detected_sub = rospy.Subscriber(
'/aruco_single/markerDetected', Bool, self.markerDetectedCallback)
self.manual_mode_sub = rospy.Subscriber(
'/joystick_controller/manualMode', Bool, self.manualModeCallback)
def markerDetectedCallback(self, detected):
print detected
if not detected and not self.manualMode:
print 'TIENE QUE ATERRIZAR'
self.ordenes.aterriza()
def manualModeCallback(self, manualMode):
self.manualMode = manualMode
def centerCallback(self, center):
height, width, depth = self.img.shape
umbral = 20
if (center.x <= umbral + width / 2) and (
center.x > width / 2 - umbral
): #and (center.y <= umbral + height/2) and (center.y > height/2 - umbral):
self.ordenes.para()
elif center.x < width / 2 - umbral:
if not self.manualMode and self.ordenes.getStatus(
) == DroneStatus.Hovering:
self.ordenes.despLateralDer()
elif center.x <= umbral + width / 2:
if not self.manualMode and self.ordenes.getStatus(
) == DroneStatus.Hovering:
self.ordenes.despLateralIzq()
def sonido(self, sonido):
pygame.mixer.init()
sound = pygame.mixer.Sound(sonido)
sound.play()
def dibujaMira(self, width, height, img):
cv2.line(img, (0, height / 2), (width, height / 2), (0, 0, 255), 1)
cv2.line(img, (width / 2, 0), (width / 2, height), (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 4, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 10, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 20, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 30, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 40, (0, 0, 255), 1)
def imageCallback(self, data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
self.dibujaMira(width, height, self.img)
except CvBridgeError, e:
print e
cv2.imshow("Image window", self.img)
cv2.waitKey(3)
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.roll = 0
self.pitch = 0
self.yaw = 0
self.zVelocity = 0
self.cameraHeight = 0
self.cameraWidth = 0
self.command = Twist()
#Subscribers
self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw',Image,self.imageCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata)
self.subCameraInfo = rospy.Subscriber('/ardrone/camera_info',CameraInfo,self.cameraInfoCallback)
def cameraInfoCallback(self, info):
self.cameraHeight = info.height
self.cameraWidth = info.width
def ReceiveNavdata(self,navdata):
self.navdata = navdata
def calculaCoords(self, x1, y1):
x = int(self.cameraWidth*x1/1000)
y = int(self.cameraHeight*y1/1000)
return (x, y)
def corregirX(self, x):
umbral = 0
if x > self.cameraWidth + umbral:
self.ordenes.despLateralIzq()
elif x < self.cameraWidth - umbral:
self.ordenes.despLateralDer()
elif x < self.cameraWidth + umbral and x > self.cameraWidth - umbral:
self.ordenes.paraDespLateral()
def corregirY(self, y):
umbral = 0
if y > self.cameraHeight + umbral:
self.ordenes.avanza(0.15)
elif y < self.cameraHeight - umbral:
self.ordenes.retrocede(0.15)
elif y < self.cameraHeight + umbral and y > self.cameraHeight - umbral:
self.ordenes.paraPitch()
def corregirAngulo(self, angulo):
umbral = 10
if (angulo < 270 + umbral) and angulo > 270 - umbral:
self.ordenes.paraYaw()
elif angulo > 270 + umbral:
self.ordenes.yawDer()
elif angulo < 270 - umbral:
self.ordenes.yawIzq()
def dibujaMira(self, width, height, img):
cv2.line(img,(0,height/2),(width,height/2),(0,0,255),1)
cv2.line(img,(width/2,0),(width/2,height),(0,0,255),1)
cv2.circle(img,(width/2,height/2), 4, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 10, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 20, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 30, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 40, (0,0,255), 1)
def imageCallback(self,data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
auxNavdata = self.navdata
# print 'height ' + str(self.cameraHeight) + ' width ' + str(self.cameraWidth)
self.dibujaMira(width, height, self.img)
font = cv2.FONT_HERSHEY_SIMPLEX
if auxNavdata != None:
cv2.putText(self.img,'Tags Detected ' + str(auxNavdata.tags_count) ,(0,25), font, 0.5,(255,255,255),1,255)
if auxNavdata.tags_count != 0:
cv2.putText(self.img,'Tag Type ' + str(auxNavdata.tags_type[0]) ,(0,50), font, 0.5,(255,255,255),1,255)
cv2.putText(self.img,'Tag xc ' + str(auxNavdata.tags_xc[0]) + ' Tag yc ' + str(auxNavdata.tags_yc[0]),(0,75), font, 0.5,(255,255,255),1,255)
pos = self.calculaCoords(auxNavdata.tags_xc[0], auxNavdata.tags_yc[0])
self.corregirX(pos[0])
self.corregirY(pos[1])
#self.corregirAngulo(auxNavdata.tags_orientation[0])
cv2.putText(self.img,'Tag x pixels ' + str(pos[0]) + ' Tag y pixels ' + str(pos[1]),(0,100), font, 0.5,(255,255,255),1,255)
cv2.putText(self.img,'Tag orientation ' + "%0.2f" % auxNavdata.tags_orientation[0], (0,125), font, 0.5,(255,255,255),1,255)
cv2.putText(self.img,'Roll ' + str(self.roll), (0,150), font, 0.5,(255,255,255),1,255)
else:
self.ordenes.para()
cv2.imshow("Image window", self.img)
cv2.waitKey(3)
except CvBridgeError, e:
print e
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.ordenes = Ordenes()
self.accion_act = None
self.accion_ant = None
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
self.aruco_sub = rospy.Subscriber('/aruco_single/markerId', markerId,
self.idCallback)
# self.drone_image_sub = rospy.Subscriber('/ardrone/image_raw',markerId,self.detectColor)
def idCallback(self, data):
self.accion_act = data.markerId
if self.accion_ant != self.accion_act:
self.parser(data)
self.accion_ant = self.accion_act
def parser(self, data):
if data == constantes.MARGEN_IZQ:
print "ME DESPLAZO LATERALMENTE HACIA LA DERECHA"
self.ordenes.despLateralDer()
elif data == constantes.MARGEN_DER:
print "ME DESPLAZO LATERALMENTE HACIA LA IZQUIERDA"
self.ordenes.despLateralIzq()
def sonido(self, sonido):
pygame.mixer.init()
sound = pygame.mixer.Sound(sonido)
sound.play()
def angulo(self, x1, y1, x2, y2):
if x1 != x2:
return math.degrees(math.atan((y2 - y1) / (x2 - x1)))
else:
return 90
def maxMinXY(self, contours):
if len(contours) > 0:
i = 1
x_max = contours[0][0][0][0]
x_min = contours[0][0][0][0]
y_max = contours[0][0][0][1]
y_min = contours[0][0][0][1]
while i < len(contours[0]):
if x_max < contours[0][i][0][0]:
x_max = contours[0][i][0][0]
if x_min > contours[0][i][0][0]:
x_min = contours[0][i][0][0]
if y_max < contours[0][i][0][1]:
y_max = contours[0][i][0][1]
if y_min > contours[0][i][0][1]:
y_min = contours[0][i][0][1]
i = i + 1
return (x_max, x_min, y_max, y_min)
else:
return None
def longSegment(self, x1, x2, y1, y2):
return math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1))
def maxLine(self, lines):
max_long = -1
line = None
if len(lines) > 0:
for x1, y1, x2, y2 in lines[0]:
longSe = self.longSegment(x1, x2, y1, y2)
if longSe > max_long:
max_long = longSe
line = (x1, y1, x2, y2)
return line
def calculaGiro(self, ang):
return 90 - abs(ang)
def grabarImagenes(self, img, the):
if self.contImg < 10:
cv2.imwrite('salida_000' + str(self.contImg) + '.jpeg', img)
elif self.contImg < 100:
cv2.imwrite('salida_00' + str(self.contImg) + '.jpeg', img)
elif self.contImg < 1000:
cv2.imwrite('salida_0' + str(self.contImg) + '.jpeg', img)
else:
cv2.imwrite('salida_' + str(self.contImg) + '.jpeg', img)
if self.contImg < 10:
cv2.imwrite('canny_000' + str(self.contImg) + '.jpeg', the)
elif self.contImg < 100:
cv2.imwrite('canny_00' + str(self.contImg) + '.jpeg', the)
elif self.contImg < 1000:
cv2.imwrite('canny_0' + str(self.contImg) + '.jpeg', the)
else:
cv2.imwrite('canny_' + str(self.contImg) + '.jpeg', the)
self.contImg = self.contImg + 1
def dibujaMira(self, width, height, img):
cv2.line(img, (0, height / 2), (width, height / 2), (0, 0, 255), 1)
cv2.line(img, (width / 2, 0), (width / 2, height), (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 4, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 10, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 20, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 30, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 40, (0, 0, 255), 1)
def detectaContornos(self, bordes, img_orig):
contours, hierarchy = cv2.findContours(bordes, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(img_orig, contours, -1, (0, 255, 0), 3)
def mensajePantalla(self, img, msg, x, y):
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(img, msg, (x, y), font, 0.5, (255, 0, 255), 2)
def detectaLinea(self, bordes, img_orig):
minLineLength = 20
maxLineGap = 4
lines = cv2.HoughLinesP(bordes, 1, np.pi / 180, 100, minLineLength,
maxLineGap)
ang = None
puntoMedio = None
if lines != None:
line = self.maxLine(lines)
ang = self.angulo(line[0], line[1], line[2], line[3])
puntoMedio = ((line[2] - line[0]) / 2, (line[3] - line[1]) / 2)
cv2.line(img_orig, (line[0], line[1]), (line[2], line[3]),
(255, 0, 0), 3)
return (ang, puntoMedio)
def imageCallback(self, data):
try:
img = self.bridge.imgmsg_to_cv2(data, "bgr8")
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
height, width, depth = img.shape
thresh = 125
ret, the = cv2.threshold(gray, thresh, 220, cv2.THRESH_BINARY)
self.detectaContornos(the, img)
result = self.detectaLinea(the, img)
if result[0] != None and result[1] != None:
ang = result[0]
xMedio = result[1][0]
umbral = 5
distanciaCentro = xMedio # - width/2
if math.fabs(distanciaCentro) > umbral:
if distanciaCentro > 0:
self.mensajePantalla(the, str(distanciaCentro),
width / 2, height / 2)
elif distanciaCentro < 0:
self.mensajePantalla(the, str(distanciaCentro),
width / 2, height / 2)
else:
self.mensajePantalla(the, "PARA", width / 2, height / 2)
self.dibujaMira(width, height, img)
except CvBridgeError, e:
print e
cv2.imshow("Threshold window", the)
cv2.imshow("Image window", img)
cv2.waitKey(3)
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.centered = False
self.command = Twist()
self.distAct = 1.5
self.markerDetected = False
self.center = None
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',Point,self.centerCallback)
#self.marker_points_sub = rospy.Subscriber('/aruco_single/markerPoints',points,self.pointsCallback)
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',PoseStamped,self.poseCallback)
self.marker_detected_sub = rospy.Subscriber('/aruco_single/markerDetected',Bool,self.markerDetectedCallback)
self.dist_sub = rospy.Subscriber('/getDistance/distance',Float32,self.distActCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata)
def markerDetectedCallback(self, detected):
self.markerDetected = detected.data
def ReceiveNavdata(self,navdata):
self.navdata = navdata
def distActCallback(self, dist):
self.distAct = dist
def poseCallback(self, poseS):
pose = poseS.pose
position = pose.position
self.posX = position.x
self.posY = position.y
self.posZ = position.z
def moveToCenter(self, centerPos):
height, width, depth = self.img.shape
umbral = 50
font = cv2.FONT_HERSHEY_SIMPLEX
if ((centerPos.y <= umbral + height/2) and (centerPos.y > height/2 - umbral)) and ((centerPos.x <= umbral + width/2) and (centerPos.x > width/2 - umbral)):
cv2.putText(self.img,'centered',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.para()
self.centered = True
print 'centered'
elif (centerPos.y > umbral + height/2) and (centerPos.x > umbral + width/2):
cv2.putText(self.img,'Baja Derecha',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.bajaDer()
self.centered = False
elif (centerPos.y > umbral + height/2) and (centerPos.x < width/2 - umbral):
cv2.putText(self.img,'Baja Izquierda',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.bajaIzq()
self.centered = False
elif (centerPos.y < height/2 - umbral) and (centerPos.x > umbral + width/2):
cv2.putText(self.img,'Sube Derecha',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.subeDer()
self.centered = False
elif (centerPos.y < height/2 - umbral) and (centerPos.x < width/2 - umbral):
cv2.putText(self.img,'Sube Izquierda',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.subeIzq()
self.centered = False
elif (centerPos.y > umbral + height/2):
cv2.putText(self.img,'Baja',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.baja()
self.centered = False
print 'Baja'
elif (centerPos.y < height/2 - umbral):
cv2.putText(self.img,'Sube',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.sube()
self.centered = False
print 'Sube'
elif (centerPos.x > umbral + width/2):
cv2.putText(self.img,'Derecha',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.despLateralDer()
self.centered = False
print 'Derecha'
elif (centerPos.x < width/2 - umbral):
cv2.putText(self.img,'Izquierda',(0,height), font, 1,(255,255,255),1,255)
self.ordenes.despLateralIzq()
self.centered = False
print 'Izquierda'
"""
def distance(self, distance):
umbral = 0.1 #10 cm
if (distance< self.distAct - umbral):
self.pitch = -1
print 'Atras'
elif (distance > self.distAct + umbral):
self.pitch = 1
print 'Adelante'
elif (distance > self.distAct - umbral) and (distance < self.distAct + umbral):
self.pitch = 0
self.roll = 0
self.zVelocity = 0
self.SetCommand(self.roll, self.pitch, self.yaw, self.zVelocity)
self.SendCommand(self.command)
self.ordenes.gira180()
print 'Distancia correcta'
"""
def centerCallback(self, center):
self.center = center
def dibujaMira(self, width, height, img):
cv2.line(img,(0,height/2),(width,height/2),(0,0,255),1)
cv2.line(img,(width/2,0),(width/2,height),(0,0,255),1)
cv2.circle(img,(width/2,height/2), 4, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 10, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 20, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 30, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 40, (0,0,255), 1)
def imageCallback(self,data):
font = cv2.FONT_HERSHEY_SIMPLEX
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
self.dibujaMira(width, height, self.img)
cv2.putText(self.img,'Marker detected ' + str(self.markerDetected),(0,125), font, 0.75,(255,255,255),1,255)
if self.markerDetected:
self.moveToCenter(self.center)
else:
self.ordenes.para()
cv2.imshow("centered", self.img)
cv2.waitKey(3)
except CvBridgeError, e:
print e
def __init__(self):
super(KeyboardController, self).__init__()
self.ordenes = Ordenes()
class KeyboardController(DroneVideoDisplay):
def __init__(self):
super(KeyboardController,self).__init__()
self.ordenes = Ordenes()
# We add a keyboard handler to the DroneVideoDisplay to react to keypresses
def keyPressEvent(self, event):
key = event.key()
# If we have constructed the drone controller and the key is not generated from an auto-repeating key
if self.ordenes is not None and not event.isAutoRepeat():
# Handle the important cases first!
if key == KeyMapping.Emergency:
self.ordenes.emergencia()
elif key == KeyMapping.Takeoff:
self.ordenes.despega()
elif key == KeyMapping.Land:
self.ordenes.aterriza()
elif key == KeyMapping.YawLeft:
if not self.ordenes.estaGirando():
self.ordenes.gira90Izq()
elif key == KeyMapping.YawRight:
if not self.ordenes.estaGirando():
self.ordenes.corregirYaw(10)
class KeyboardController(DroneVideoDisplay):
def __init__(self):
super(KeyboardController, self).__init__()
self.ordenes = Ordenes()
# We add a keyboard handler to the DroneVideoDisplay to react to keypresses
def keyPressEvent(self, event):
key = event.key()
# If we have constructed the drone controller and the key is not generated from an auto-repeating key
if self.ordenes is not None and not event.isAutoRepeat():
# Handle the important cases first!
if key == KeyMapping.Emergency:
self.ordenes.emergencia()
elif key == KeyMapping.Takeoff:
self.ordenes.despega()
elif key == KeyMapping.Land:
self.ordenes.aterriza()
elif key == KeyMapping.YawLeft:
if not self.ordenes.estaGirando():
self.ordenes.gira90Izq()
elif key == KeyMapping.YawRight:
if not self.ordenes.estaGirando():
self.ordenes.corregirYaw(10)
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
#self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.centered = False
self.distAct = 2
self.markerDetected = False
self.markerDetected_ant = False
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
#self.marker_points_sub = rospy.Subscriber('/aruco_single/markerPoints',points,self.pointsCallback)
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',
PoseStamped, self.distCallback)
self.dist_sub = rospy.Subscriber('distance', Float32,
self.distActCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.ReceiveNavdata)
def ReceiveNavdata(self, navdata):
self.navdata = navdata
def distActCallback(self, data):
self.distAct = data.data
def distance(self, distance):
umbral = 0.4 #40 cm
height, width, depth = self.img.shape
font = cv2.FONT_HERSHEY_SIMPLEX
if self.ordenes.getStatus(
) != DroneStatus.Landed or self.ordenes.getStatus(
) != DroneStatus.TakingOff:
cv2.putText(self.img, 'Dist act: ' + str(distance),
(0, height - 20), font, 0.75, (255, 255, 255), 1, 255)
cv2.putText(self.img, 'Dist fijada: ' + str(self.distAct), (0, 25),
font, 0.75, (255, 255, 255), 1, 255)
print 'Distancia ' + str(distance)
if (distance < self.distAct - umbral):
self.ordenes.retrocede(0.25)
self.markerDetected_ant = True
print 'Atras'
elif (distance > self.distAct + umbral):
self.ordenes.avanza(0.25)
self.markerDetected_ant = True
print 'Adelante'
elif (distance > self.distAct - umbral) and (
distance < self.distAct + umbral):
self.ordenes.para()
"""
time.sleep(1)
if not self.ordenes.girando:
self.ordenes.gira180()
"""
self.markerDetected_ant = False
print 'Distancia correcta'
def distCallback(self, poseS):
pose = poseS.pose
position = pose.position
if self.img != None:
self.distance(position.z)
cv2.imshow("dist", self.img)
cv2.waitKey(3)
def dibujaMira(self, width, height, img):
cv2.line(img, (0, height / 2), (width, height / 2), (0, 0, 255), 1)
cv2.line(img, (width / 2, 0), (width / 2, height), (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 4, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 10, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 20, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 30, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 40, (0, 0, 255), 1)
def imageCallback(self, data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
self.dibujaMira(width, height, self.img)
"""
if not self.markerDetected:
#if self.ordenes.getStatus() != DroneStatus.Landed:
self.ordenes.para()
self.markerDetected_ant = False
"""
except CvBridgeError, e:
print e
def __init__(self):
self.ordenes = Ordenes()
#Subscribers
self.giraDer_sub = rospy.Subscriber('corregirGiro', Vector3,
self.callback)
def __init__(self):
self.DEBUG = True
self.rate = rospy.Rate(10) #10Hz
self.ordenes = Ordenes()
self.bridge = CvBridge()
self.centrado = False
self.alineado = False
self.giros180 = 0
if self.DEBUG:
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',
PoseStamped,
self.distCallback)
self.dist = -1
#Subscriber para la imagen
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
#Subscribers para el centrado
self.arriba_sub = rospy.Subscriber('/arriba', Empty,
self.arribaCallback)
self.abajo_sub = rospy.Subscriber('/abajo', Empty, self.abajoCallback)
self.izq_sub = rospy.Subscriber('/despIzq', Empty, self.izqCallback)
self.der_sub = rospy.Subscriber('/despDer', Empty, self.derCallback)
self.arribaIzq_sub = rospy.Subscriber('/arribaIzq', Empty,
self.arribaIzqCallback)
self.arribaDer_sub = rospy.Subscriber('/arribaDer', Empty,
self.arribaDerCallback)
self.abajoIzq_sub = rospy.Subscriber('/abajoIzq', Empty,
self.abajoIzqCallback)
self.abajo_sub = rospy.Subscriber('/abajoDer', Empty,
self.abajoDerCallback)
self.paraCentrar_sub = rospy.Subscriber('/paraCentrar', Empty,
self.paraCentrarCallback)
self.centrado_sub = rospy.Subscriber('/centrado', Empty,
self.centradoCallback)
#Subscribers para el avance/retroceso
self.avanza_sub = rospy.Subscriber('/avanza', Empty,
self.avanzaCallback)
self.retrocede_sub = rospy.Subscriber('/retrocede', Empty,
self.retrocedeCallback)
self.paraPitch_sub = rospy.Subscriber('/paraPitch', Empty,
self.paraPitchCallback)
self.distCorrect_sub = rospy.Subscriber('/distCorrecta', Empty,
self.distCorrectaCallback)
#Subscribers para la correccion del Yaw
self.yaw_izq_sub = rospy.Subscriber('/yawIzq', Empty,
self.yawIzqCallback)
self.yaw_der_sub = rospy.Subscriber('/yawDer', Empty,
self.yawDerCallback)
self.yaw_para_sub = rospy.Subscriber('/paraYaw', Empty,
self.paraYawCallback)
self.yaw_correcto_sub = rospy.Subscriber('/yawCorrecto', Empty,
self.yawCorrectoCallback)
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
"""
Para usar la camara de abajo cambiar '/ardrone/image_raw' por
'/ardrone/botton/image_raw' luego antes de ejecutar el nodo
ejecutar el comando rosservice call /ardrone/togglecam
"""
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result',Image,self.imageCallback)
self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',Point,self.centerCallback)
self.marker_detected_sub = rospy.Subscriber('/aruco_single/markerDetected',Bool,self.markerDetectedCallback)
self.manual_mode_sub = rospy.Subscriber('/joystick_controller/manualMode',Bool,self.manualModeCallback)
def markerDetectedCallback(self, detected):
print detected
if not detected and not self.manualMode:
print 'TIENE QUE ATERRIZAR'
self.ordenes.aterriza()
def manualModeCallback(self, manualMode):
self.manualMode = manualMode
def centerCallback(self, center):
height, width, depth = self.img.shape
umbral = 20
if (center.x <= umbral + width/2) and (center.x > width/2 - umbral): #and (center.y <= umbral + height/2) and (center.y > height/2 - umbral):
self.ordenes.para()
elif center.x < width/2 - umbral:
if not self.manualMode and self.ordenes.getStatus() == DroneStatus.Hovering:
self.ordenes.despLateralDer()
elif center.x <= umbral + width/2:
if not self.manualMode and self.ordenes.getStatus() == DroneStatus.Hovering:
self.ordenes.despLateralIzq()
def sonido(self, sonido):
pygame.mixer.init()
sound = pygame.mixer.Sound(sonido)
sound.play()
def dibujaMira(self, width, height, img):
cv2.line(img,(0,height/2),(width,height/2),(0,0,255),1)
cv2.line(img,(width/2,0),(width/2,height),(0,0,255),1)
cv2.circle(img,(width/2,height/2), 4, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 10, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 20, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 30, (0,0,255), 1)
cv2.circle(img,(width/2,height/2), 40, (0,0,255), 1)
def imageCallback(self,data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
height, width, depth = self.img.shape
self.dibujaMira(width, height, self.img)
except CvBridgeError, e:
print e
cv2.imshow("Image window", self.img)
cv2.waitKey(3)