class SetCameraDirective(AbstractDroneDirective): # sets up this directive def __init__(self, camera): # camera=0 is for front camera, camera=1 is for bottom camera if camera == "FRONT": self.camera = 0 elif camera == "BOTTOM": self.camera = 1 else: raise Exception("Camera must be 0 (front) or 1 (bottom)") self.controller = BasicDroneController("Toggle Camera Directive") # given the image and navdata of the drone, returns the following in order: # # A directive status int: # 1 if algorithm is finished and the drone's camera has been set # # A tuple of (xspeed, yspeed, yawspeed, zspeed): # indicating the next instructions to fly the drone # # An image reflecting what is being done as part of the algorithm def RetrieveNextInstruction(self, image, navdata): self.controller.SwitchCamera(self.camera) if self.camera == 0: cameraName = "front camera" else: cameraName = "bottom camera" rospy.logwarn("Set Drone Camera to: " + cameraName) return 1, (0, 0, 0, 0), image, (None, None), 0, 0, None
class KeyboardController(object): def __init__(self): # initalize gui window (pygame) pygame.init() self.screen = pygame.display.set_mode((640, 480)) pygame.display.set_caption("Keyboard Controller") (self.screen).fill(GREY) background = pygame.image.load(expanduser("~")+"/drone_ws/src/ardrone_lab/src/resources/KeyboardCommands4.png") self.screen.blit(background,[0,0]) pygame.display.update() self.keyPub = rospy.Publisher('/controller/keyboard',ROSString) # setup controller + its variables self.controller = BasicDroneController("Keyboard") self.speed = 1 self.pitch = 0 self.roll = 0 self.yaw_velocity = 0 self.z_velocity = 0 def startController(self): # while gui is still running, continusly polls for keypresses gameExit = False while not gameExit: for event in pygame.event.get(): # checking when keys are pressing down if event.type == pygame.KEYDOWN and self.controller is not None: self.keyPub.publish(str(event.key)) if event.key == pygame.K_t: #switches camera to bottom when it launches self.controller.SendTakeoff() self.controller.SwitchCamera(1) print( "Takeoff") elif event.key == pygame.K_g: self.controller.SendLand() rospy.logwarn("-------- LANDING DRONE --------") print ("Land") elif event.key == pygame.K_ESCAPE: self.controller.SendEmergency() print ("Emergency Land") elif event.key == pygame.K_c: self.controller.ToggleCamera() print ("toggle camera") elif event.key == pygame.K_z: self.controller.FlatTrim() else: if event.key == pygame.K_w: self.pitch = self.speed elif event.key == pygame.K_s: self.pitch = self.speed*-1 elif event.key == pygame.K_a: self.roll = self.speed print ("Roll Left") elif event.key == pygame.K_d: self.roll = self.speed*-1 print ("Roll Right") elif event.key == pygame.K_q: self.yaw_velocity = self.speed print ("Yaw Left") elif event.key == pygame.K_e: self.yaw_velocity = self.speed*-1 print ("Yaw Right") elif event.key == pygame.K_r: self.z_velocity = self.speed*1 print ("Increase Altitude") elif event.key == pygame.K_f: self.z_velocity = self.speed*-1 print ("Decrease Altitude") self.controller.SetCommand(self.roll, self.pitch, self.yaw_velocity, self.z_velocity) if event.type == pygame.KEYUP: if (event.key == pygame.K_w or event.key == pygame.K_s or event.key == pygame.K_a or event.key == pygame.K_d or event.key == pygame.K_q or event.key == pygame.K_e or event.key == pygame.K_r or event.key == pygame.K_f): self.pitch = 0 self.roll = 0 self.z_velocity = 0 self.yaw_velocity = 0 self.controller.SetCommand(self.roll, self.pitch, self.yaw_velocity, self.z_velocity) if event.type == pygame.QUIT: gameExit = True pygame.display.quit() pygame.quit()