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)])
