class ToggleCameraDirective(AbstractDroneDirective): # sets up this directive def __init__(self): 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 toggled # # 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.ToggleCamera() rospy.logwarn("Toggled Drone Camera") return 1, (0, 0, 0, 0), image, (None,None), 0, 0, None
class DroneMaster(DroneVideo, FlightstatsReceiver): def __init__(self): # getting access to elements in DroneVideo and FlightstatsReciever super(DroneMaster, self).__init__() self.objectName = "NASA Airplane" self.startingAngle = 0 # backpack: 120/-55/95 # +100 for big objects, +50 for shorter objects. (Modifies how close drone is to object; smaller # > closer) # +10 for very small objects. self.yOffset = 0 # -30 for big objects, -15 for shorter objects. (Modifies how close drone is to object; larger # > closer) self.ySizeOffset = -30 # +75 for tall objects or using cube, 0 for shorter objects. (Modifies how high the drone should fly; smaller # > lower self.zOffset = 25 # Seting up a timestamped folder inside Flight_Info that will have the pictures & log of this flight self.droneRecordPath = ( expanduser("~") + "/drone_workspace/src/ardrone_lab/src/Flight_Info/" + datetime.datetime.now().strftime("%m-%d-%Y__%H:%M:%S, %A") + "_Flight_" + self.objectName + "/") if not os.path.exists(self.droneRecordPath): os.makedirs(self.droneRecordPath) #self.logger = Logger(self.droneRecordPath, "AR Drone Flight") #self.logger.Start() #import PID and color constants self.settingsPath = expanduser( "~" ) + "/drone_workspace/src/ardrone_lab/src/resources/calibrater_settings.txt" # initalizing the state machine that will handle which algorithms to run at which time; # the results of the algorithms will be used to control the drone self.stateMachine = StateMachine() # drone starts without any machine loaded, so that it can be controlled using the keyboard self.currMachine = None # initalizing helper objects self.pictureManager = PictureManager(self.droneRecordPath) self.controller = BasicDroneController("TraceCircle") self.startTimer = time.clock() # max height of drone, in mm; any higher and the drone will auto-land self.maxHeight = 2530 self.enableEmergency = False self.emergency = False self.captureRound = 0.5 self.oldBattery = -1 self.photoDirective = None # Each state machine that drone mastercan use is defined here; # When key is pressed, define the machine to be used and switch over to it. # Machines are defined as array of tuples. Each tuple represents a state's directive and duration # in the format (directive, stateduration, errorDirective). # ErrorDirectives are optional, so it can be just in the format # (directive, stateduration); # directive & errorDirective must subclass AbstractDroneDirective. def KeyListener(self): key = cv2.waitKey(1) & 0xFF # hover over orange if key == ord('1'): self.moveTime = 0.04 self.waitTime = 0 pidDirective = PIDHoverColorDirective2("orange") pidDirective.Reset() alg = [(pidDirective, 6)] #rospy.logwarn("test3") #alg = [(HoverColorDirective("orange"),6)] algCycles = -1 self.MachineSwitch(None, alg, algCycles, None, HOVER_ORANGE_MACHINE) # take picture if key == ord('3'): pictureName = self.pictureManager.Capture(self.cv_image) rospy.logwarn("Saved picture") # toggle camera elif key == ord('c'): self.controller.ToggleCamera() rospy.logwarn("Toggled Camera") # land (32 => spacebar) elif key == 32: self.controller.SendLand() rospy.logwarn( "***______--______-_***Landing Drone***_-______--_____***") # if there is a photo directive running, save pictures just in case self.SaveCachePictures() # save all pictures in cache elif key == ord('s'): self.SaveCachePictures() elif key == ord('q') or key == ord('t'): # main algorithm components self.moveTime = 0.20 self.waitTime = 0.10 flightAltitude = 1360 + self.zOffset objectAltitude = 1360 + self.zOffset angles = 8 # ~30 for big objects, ~ for small objects (can-sized) heightTolerance = 32 orangePlatformErrHoriz = (ReturnToColorDirective( 'orange', "blue", speedModifier=0.85), 4) orangePlatformErrParallel = (ReturnToColorDirective( 'orange', "pink", speedModifier=0.85), 4) blueLineErr = (ReturnToLineDirective('blue', speedModifier=0.85), 6) self.SaveCachePictures() self.photoDirective = CapturePhotoDirective( self.droneRecordPath, 30, 0.014, self.objectName, angles, objectAltitude, self.startingAngle) # 0.018 alg = [(OrientLineDirective('PARALLEL', 'pink', 'orange', flightAltitude, heightTolerance, self.yOffset, self.ySizeOffset), 1, orangePlatformErrParallel), (SetCameraDirective("FRONT"), 1), (IdleDirective("Pause for setting camera to front"), 25), (self.photoDirective, 1), (SetCameraDirective("BOTTOM"), 1), (IdleDirective("Pause for setting camera to bottom"), 25), (OrientLineDirective('PERPENDICULAR', 'blue', 'orange', flightAltitude), 5, orangePlatformErrHoriz), (FollowLineDirective('blue', speed=0.09), 6, blueLineErr)] # land on the 8th angle end = [(OrientLineDirective('PARALLEL', 'pink', 'orange', flightAltitude, heightTolerance, self.yOffset, self.ySizeOffset), 1, orangePlatformErrParallel), (SetCameraDirective("FRONT"), 1), (IdleDirective("Pause for setting camera to bottom"), 25), (self.photoDirective, 1), (LandDirective(), 1), (IdleDirective("Pause for land"), 25), (self.photoDirective, 1, None, "SavePhotos")] if key == ord('q'): #doesn't auto takeoff self.MachineSwitch(None, alg, angles - 1, end, AUTO_CIRCLE_MACHINE) if key == ord('t'): # does the entire circle algorithm, in order; takes off by itself init = [ (SetupDirective(), 1), (IdleDirective("Pause for setup"), 10), (FlatTrimDirective(), 1), (IdleDirective("Pause for flat trim"), 10), (SetCameraDirective("BOTTOM"), 1), (IdleDirective(" Pause for seting camera"), 10), (TakeoffDirective(), 1), (IdleDirective("Pause for takeoff"), 120), (ReturnToOriginDirective('orange', 50), 7), (FindPlatformAltitudeDirective('orange', flightAltitude + 200), 5), #( ReachAltitudeDirective(flightAltitude, 85), 2) ] self.MachineSwitch(init, alg, angles - 1, end, AUTO_CIRCLE_MACHINE) # just contains a machine to test any particular directive elif key == ord('b'): self.moveTime = 0.04 self.waitTime = 0.14 error = (ReturnToLineDirective('blue', speedModifier=0.4), 10) blueLineErr = (ReturnToLineDirective('blue'), 10) #orangePlatformErr = (ReturnToColorDirective('orange'), 10) orangePlatformErr = None orangePlatformErrHoriz = (ReturnToColorDirective('orange', "blue"), 10) #testalg = ( OrientLineDirective( 'PARALLEL', 'pink', 'orange', 1360, 150, self.yOffset, self.ySizeOffset), 1, orangePlatformErr) #testalg = ( PIDOrientLineDirective( 'PERPENDICULAR', 'blue', 'orange', self.settingsPath ), 4, error) #testalg = ( FollowLineDirective('blue', speed = 0.25), 6, blueLineErr) #testalg = ( OrientLineDirective('PERPENDICULAR', 'blue', 'orange', 700), 8, orangePlatformErrHoriz) #testalg = ( CapturePhotoDirective(self.droneRecordPath, 10, 0.3), 1 ) testalg = (MultiCenterTestDirective("orange"), 6) algCycles = -1 alg = [testalg] self.MachineSwitch(None, alg, algCycles, None, TEST_MACHINE) # Taking in some machine's definition of states and a string name, # provides a "switch" for loading and removing the machines that # drone master uses to control the drone def MachineSwitch(self, newMachineInit, newMachineAlg, newMachineAlgCycles, newMachineEnd, newMachineName): # pause the current state self.controller.SetCommand(0, 0, 0, 0) oldMachine = self.currMachine # if the current machine is toggled again with the same machine, # remove the machine and return back to the idle if oldMachine == newMachineName: self.currMachine = None # Otherwise, just switch to the machine else: self.stateMachine.DefineMachine(newMachineInit, newMachineAlg, newMachineAlgCycles, newMachineEnd, self) self.currMachine = newMachineName rospy.logwarn('======= Drone Master: Changing from the "' + str(oldMachine) + '" machine to the "' + str(self.currMachine) + '" machine =======') # This is called every time a frame (in self.cv_image) is updated. # Runs an iteration of the current state machine to get the next set of instructions, depending on the # machine's current state. def ReceivedVideo(self): # checks altitude; if it is higher than allowed, then drone will land currHeightReg = self.flightInfo["altitude"][1] if currHeightReg == '?': currHeightReg = 0 currHeightCalc = self.flightInfo["SVCLAltitude"][1] if currHeightCalc != -1: height = currHeightCalc else: height = currHeightReg #rospy.logwarn( "reg: " + str(currHeightReg) + " Calc: " + str(currHeightCalc) + #" avg: " + str(height)) if self.enableEmergency and height > self.maxHeight: self.controller.SendLand() self.emergency = True if self.emergency: rospy.logwarn("***** EMERGENCY LANDING: DRONE'S ALTITUDE IS " + str(height) + " mm; MAX IS " + str(self.maxHeight) + " mm *****") rospy.logwarn( "***______--______-_***Landing Drone***_-______--_____***") # if there is a photo directive running, save pictures just in case if self.photoDirective != None: self.photoDirective.SavePhotos(None, None) self.photoDirective = None # If no machine is loaded, then drone master does nothing # (so that the drone may be controlled with the keyboard) if self.currMachine == None: pass else: # retrieving the instructions for whatever state the machine is in # and commanding the drone to move accordingly droneInstructions, segImage, moveTime, waitTime = self.stateMachine.GetUpdate( self.cv_image, self.flightInfo) self.cv_image = segImage self.MoveFixedTime(droneInstructions[0], droneInstructions[1], droneInstructions[2], droneInstructions[3], moveTime, waitTime) # draws battery display and height for info Window color = (255, 255, 255) if self.flightInfo["batteryPercent"][1] != "?": batteryPercent = int(self.flightInfo["batteryPercent"][1]) sum = int(batteryPercent * .01 * (255 + 255)) if sum > 255: base = 255 overflow = sum - 255 else: base = sum overflow = 0 green = base red = 255 - overflow color = (0, green, red) batteryPercent = str(batteryPercent) + "%" if self.flightInfo["batteryPercent"][1] != self.oldBattery: self.oldBattery = self.flightInfo["batteryPercent"][1] self.info = np.zeros((70, 100, 3), np.uint8) cv2.putText(self.info, batteryPercent, (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, color, 1, cv2.LINE_AA) #cv2.putText(self.info, str(int(height)) + " mm", #(50,120), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,255),1,cv2.LINE_AA) # this function will go a certain speed for a set amount of time, then rest for wait_time # of cycles def MoveFixedTime(self, xSpeed, ySpeed, yawSpeed, zSpeed, move_time, wait_time): # Moving if time.clock() < (self.startTimer + move_time) or wait_time == 0: xSetSpeed = xSpeed ySetSpeed = ySpeed yawSetSpeed = yawSpeed zSetSpeed = zSpeed # Waiting else: xSetSpeed = 0 ySetSpeed = 0 yawSetSpeed = 0 zSetSpeed = 0 # Resetting timer, so that drone moves again if time.clock() > (self.startTimer + move_time + wait_time): self.startTimer = time.clock() self.controller.SetCommand(xSetSpeed, ySetSpeed, yawSetSpeed, zSetSpeed) # logs info #self.logger.Log( #" altitude: " + str(self.flightInfo["altitude"]) + #" yawSpeed: " + str(yawSetSpeed) + " zSpeed: " + str(zSetSpeed) ) # if there is a photo directive running, save pictures def SaveCachePictures(self): rospy.logwarn("saving cache pictures") if self.photoDirective != None: self.photoDirective.SavePhotos(None, None) self.photoDirective = None else: rospy.logwarn("none") # this is called by ROS when the node shuts down def ShutdownTasks(self): self.logger.Stop()
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)])