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)