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