class SetupDirective(AbstractDroneDirective): # sets up this directive def __init__(self): self.controller = BasicDroneController("Setup 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 is now ready to fly # # 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): state = navdata["state"][1] # Drone is currently in emergency mode, unset it if state == 0: self.controller.SendEmergency() rospy.logwarn("Drone is set up to fly") 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()
class Calibrater(object): def __init__(self): # initalizing gui window (pygame) pygame.init() self.screen = pygame.display.set_mode((353, 576)) pygame.display.set_caption("Calibrater Controller") (self.screen).fill(GREY) background = pygame.image.load( expanduser("~") + "/drone_workspace/src/ardrone_lab/src/resources/PID_Calibrater.png" ) self.screen.blit(background, [0, 0]) pygame.display.update() self.controller = BasicDroneController("Keyboard") self.editValue = None # config file path self.settingsPath = expanduser( "~" ) + "/drone_workspace/src/ardrone_lab/src/resources/calibrater_settings.txt" self.Kp, self.Ki, self.Kd = self.GetSettings() self.increment = 0.001 # reads the PID values from the last line of the file def GetSettings(self): # read a text file as a list of lines # find the last line, change to a file you have fileHandle = open(self.settingsPath, 'r') last = fileHandle.readlines() fileHandle.close() last = str(last[len(last) - 1]).split() p, i, d = [float(x) for x in last] return p, i, d # writes the current PID values to the file def WriteAll(self): calibraterFile = open(self.settingsPath, 'a') calibraterFile.write(str(self.Kp) + " ") calibraterFile.write(str(self.Ki) + " ") calibraterFile.write(str(self.Kd) + "\n") calibraterFile.close() rospy.logwarn("P = " + str(self.Kp) + " I = " + str(self.Ki) + " D = " + str(self.Kd)) def setPID(self, Kp, Ki, Kd): self.Kp = Kp self.Ki = Ki self.Kd = Kd def isZero(self, alpha): if (alpha < 0.0000000000000001): return 0.0 else: return alpha 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: #P,I,D = self.GetSettings() #self.setPID(P,I,D) if event.key == pygame.K_SPACE: self.controller.SendLand() 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_p: if (self.editValue != CALIBRATE_P): self.editValue = CALIBRATE_P else: self.editValue = None elif event.key == pygame.K_i: if (self.editValue != CALIBRATE_I): self.editValue = CALIBRATE_I else: self.editValue = None elif event.key == pygame.K_d: if (self.editValue != CALIBRATE_D): self.editValue = CALIBRATE_D else: self.editValue = None elif event.key == pygame.K_UP: if (self.editValue == CALIBRATE_P): self.Kp += self.increment self.Kp = self.isZero(self.Kp) #rospy.logwarn("P: " + str(self.Kp)) self.WriteAll() elif (self.editValue == CALIBRATE_I): self.Ki += self.increment self.Ki = self.isZero(self.Ki) #rospy.logwarn("I: " + str(self.Ki)) self.WriteAll() elif (self.editValue == CALIBRATE_D): self.Kd += self.increment self.Kd = self.isZero(self.Kd) #rospy.logwarn("D: " + str(self.Kd)) self.WriteAll() elif event.key == pygame.K_DOWN: if (self.editValue == CALIBRATE_P): self.Kp = self.Kp - self.increment self.Kp = self.isZero(self.Kp) #rospy.logwarn("P: " + str(self.Kp)) self.WriteAll() elif (self.editValue == CALIBRATE_I): self.Ki = self.Ki - self.increment self.Ki = self.isZero(self.Ki) #rospy.logwarn("I: " + str(self.Ki)) self.WriteAll() elif (self.editValue == CALIBRATE_D): self.Kd = self.Kd - self.increment self.Kd = self.isZero(self.Kd) #rospy.logwarn("D: " + str(self.Kd)) self.WriteAll() elif event.key == pygame.K_KP4: self.Kp = self.isZero(self.Kp + self.increment) self.WriteAll() elif event.key == pygame.K_KP1: self.Kp = self.isZero(self.Kp - self.increment) self.WriteAll() elif event.key == pygame.K_KP5: self.Ki = self.isZero(self.Ki + self.increment) self.WriteAll() elif event.key == pygame.K_KP2: self.Ki = self.isZero(self.Ki - self.increment) self.WriteAll() elif event.key == pygame.K_KP6: self.Kd = self.isZero(self.Kd + self.increment) self.WriteAll() elif event.key == pygame.K_KP3: self.Kd = self.isZero(self.Kd - self.increment) self.WriteAll() 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): pass if event.type == pygame.QUIT: gameExit = True pygame.display.quit() pygame.quit() # this is called by ROS when the node shuts down def ShutdownTasks(self): # at maximum, only keeps 20 pid values in the file lines = open(self.settingsPath).readlines() numLines = 20 if len(lines) < 20: numLines = len(lines) open(self.settingsPath, 'w').writelines(lines[-numLines:len(lines)])