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 CapturePhotoDirective(AbstractDroneDirective):
# sets up this directive
# picture path: location to save this photo
# picturesToTake: the # of pictures to take until this directive is considred done
# pause: time to wait before taking the next picture, in seconds
# objectName: name of picture to be taken (will be appended to picture filename
# angles: Number of angles that is being used for the circle
# objectAltitude: how high off the ground the object being taken is
def __init__(self,
picturePath,
picturesToTake=1,
pause=0.5,
objectName="",
angles=0,
objectAltitude=None,
startingAngle=0):
self.pictureManager = PictureManager(picturePath)
self.picturesToTake = picturesToTake
self.pause = pause
self.objectName = objectName
self.angles = angles
self.objectAltitude = objectAltitude
self.startingAngle = startingAngle
# describes the nth time this class has been called in the state machine
self.captureRound = 1
# describes the # of times pictures have been taken for this state call
self.picturesTaken = 1
self.lastTaken = time.clock()
self.shownInitMessage = False
# stores the images in the form of a tuple (image, filename)
self.imageCache = []
# given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 0 if the algorithm is still working on taking pictures
# 1 if algorithm is finished and the specified # of pictures have been taken
#
# 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):
if self.picturesTaken == 0 and not self.shownInitMessage:
rospy.logwarn("Taking " + str(self.picturesToTake) +
" pictures ...")
self.shownInitMessage = True
if self.picturesTaken <= self.picturesToTake:
# contents are called if it's time to take a picture
if (time.clock() - self.lastTaken) > self.pause:
# creating the filename for this particular picture
if self.angles != 0:
currAngle = (360 / self.angles *
(self.captureRound - 1)) + self.startingAngle
else:
currAngle = 0
if self.objectAltitude == None:
altitude = str(navdata["altitude"][1])
else:
offset = 200
altitude = navdata["altitude"][
1] + offset - self.objectAltitude
if altitude > 0:
altitude = "+" + str(altitude)
else:
altitude = str(altitude)
pictureName = (str(currAngle) + " Degrees _ Picture " +
str(self.picturesTaken) + " _ " +
self.objectName)
self.imageCache.append((image, pictureName))
#pictureName = self.pictureManager.Capture(image, imageName = pictureName)
rospy.logwarn("Took picture # " + str(self.picturesTaken) +
" as " + pictureName + ".png")
self.picturesTaken += 1
self.lastTaken = time.clock()
directiveStatus = 0
else:
rospy.logwarn("****** Successfully took " +
str(self.picturesToTake) + " pictures ******")
directiveStatus = 1
return directiveStatus, (0, 0, 0, 0.0), image, (None, None), 0, 0, None
# This method is called by the state machine when it considers this directive finished
def Finished(self):
rospy.logwarn("Resetting picture counter")
self.picturesTaken = 1
self.captureRound += 1
self.shownInitMessage = False
return None
# saves all the images stored in the cache
def SavePhotos(self, image, navdata):
rospy.logwarn("Processing the " + str(len(self.imageCache)) +
" pictures taken ...")
pictureNum = 1
for image in self.imageCache:
pictureName, picturePath = self.pictureManager.Capture(
image[0], imageName=image[1])
rospy.logwarn("Saving picture " + str(pictureNum) + "/" +
str(len(self.imageCache)) + " : " + pictureName)
rospy.logwarn("@ Location: " + str(picturePath))
pictureNum += 1
rospy.logwarn(" ... Done")
directiveStatus = 1
return directiveStatus, (0, 0, 0, 0.0), image, (None, None), 0, 0, None