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