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