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
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
