class GiraDer:
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.giraDer_sub = rospy.Subscriber('GiraDer', Empty, self.callback)
def callback(self, data):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ - 90
girando = True
while girando:
rotZ = self.controller.getRotZ()
if (rotZ > rotZ_dest):
if self.controller.getYawVelocity() >= 0:
self.controller.SetCommand(0, 0, -1, 0)
else:
girando = False
self.controller.SetCommand(0, 0, 0, 0)
class AutoDrive(object):
def __init__(self):
self.image = None
self.imageLock = Lock()
self.bridge = CvBridge()
rospy.init_node('ardrone_keyboard_controller')
self.controller = BasicDroneController()
self.image_sub = rospy.Subscriber("/ardrone/bottom/image_raw", Image,
self.reciveImage)
self.action = 0 # 0 takeoff status , 1 go foward
self.status = -1 # drone status , 0 flying,-1 landed
def reciveImage(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.processImage(cv_image)
except CvBridgeError as e:
print(e)
#process recived image ,change the drone's action
def processImage(self, img):
#cv2.imwrite('drone.jpg',img)
cv2.imshow("Image window", img)
cv2.waitKey(3)
self.action = 1
def drive(self):
while not rospy.is_shutdown():
# sleep 1 second to wait the drone initial
rate = rospy.Rate(1)
rate.sleep()
if self.status == -1:
print "send take off"
self.controller.SendTakeoff()
self.status = 0
elif self.action == 1:
print "send go foward"
self.controller.SetCommand(pitch=1)
else:
pass
self.controller.Land()
controller.SetCommand(0, 0, 0, 0)
rospy.sleep(1)
if __name__ == '__main__':
rospy.init_node('preplanned_path', anonymous=True)
pub1 = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10)
controller = BasicDroneController()
controller.StartSendCommand()
rospy.sleep(1)
controller.SendTakeoff()
print "Take off"
controller.StartSendCommand()
rospy.sleep(5)
zigzag2()
print "Down"
controller.SetCommand(0, 0, 0, -0.25)
rospy.sleep(3)
#rospy.sleep(1)
print "Land"
controller.SendLand()
#pub2 = rospy.Subscriber("/visualization_marker", Marker, detect_tag)
#search_pattern()
#reset_drone(pub1)
#rospy.spin()
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()
def reset_drone(pub1):
controller.SetCommand(0, 0, 0, 0)
time.sleep(1)
def search_pattern():
controller.SetCommand(0, 0, 0.2, 0)
sleep(1)
controller.SetCommand(0, 0, 0, 0)
sleep(1)
if __name__ == '__main__':
rospy.init_node('autonomous_search', anonymous=True)
pub1 = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
controller = BasicDroneController()
controller.StartSendCommand()
rospy.sleep(2.0)
controller.SendTakeoff()
rospy.sleep(5.0)
controller.SetCommand(0, 0, 0, 1)
rospy.sleep(5.0)
controller.SetCommand(0, 0, 0, -1)
rospy.sleep(2.0)
controller.SendLand()
#controller.StartSendCommand()
#search_pattern()
#reset_drone(pub1)
rospy.spin()
class Ordenes:
def __init__(self):
self.controller = BasicDroneController()
self.girando = False
self.despLatIzq = False
self.despLatDer = False
self.subiendoDer = False
self.subiendoIzq = False
self.bajandoDer = False
self.bajandoIzq = False
self.subiendo = False
self.bajando = False
def getStatus(self):
return self.controller.status
"""
def emergencia(self):
self.controller.SendEmergency()
def despega(self):
self.controller.SendTakeoff()
def aterriza(self):
self.controller.SendLand()
"""
def yawIzq(self):
self.controller.SetCommand(0, 0, 0.25, 0)
def yawDer(self):
self.controller.SetCommand(0, 0, -0.25, 0)
def paraYaw(self):
self.controller.setYaw(0)
def paraCentrar(self):
self.controller.setRoll(0)
self.controller.setZvelocity(0)
def paraPitch(self):
self.controller.setPitch(0)
def estaGirando(self):
return self.girando
def gira(self):
self.controller.setYaw(1)
self.girando = True
def paraGiro(self):
self.controller.setYaw(0)
self.girando = False
def gira90Izq(self):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ + 90
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if (rotZ < rotZ_dest):
if self.controller.getYawVelocity() <= 0:
self.controller.SetCommand(0, 0, 1, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
self.controller.SetCommand(0, 0, 0, 0)
def gira90Der(self):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ - 90
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if (rotZ > rotZ_dest):
if self.controller.getYawVelocity() >= 0:
self.controller.SetCommand(0, 0, -1, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
self.controller.SetCommand(0, 0, 0, 0)
def gira180(self):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ + 160
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if (rotZ < rotZ_dest):
if self.controller.getYawVelocity() <= 0:
self.controller.SetCommand(0, 0, 1, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
self.controller.SetCommand(0, 0, 0, 0)
def corregirYaw(self, angulo):
rotZ = self.controller.getRotZ()
rotZ_dest = rotZ + angulo
self.controller.writeLog('rotZ_ini ' + str(rotZ))
self.controller.writeLog('rotZ_dest ' + str(rotZ_dest))
self.girando = True
while self.girando:
rotZ = self.controller.getRotZ()
if angulo < 0:
if (rotZ > rotZ_dest):
self.controller.writeLog('rotZ ' + str(rotZ))
#if self.controller.getYawVelocity() >= 0:
#self.controller.SetCommand(0, 0, -10, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
#self.controller.SetCommand(0, 0, 0, 0)
else:
if (rotZ < rotZ_dest):
self.controller.writeLog('rotZ ' + str(rotZ))
#if self.controller.getYawVelocity() <= 0:
#self.controller.SetCommand(0, 0, 10, 0)
else:
self.girando = False
self.controller.writeLog('YA LLEGAMOS')
#self.controller.SetCommand(0, 0, 0, 0)
def despLateralDer(self):
self.controller.SetCommand(-0.15, 0, 0, 0)
self.despLateral = True
def despLateralIzq(self):
self.controller.SetCommand(0.15, 0, 0, 0)
self.despLateral = True
def paraDespLateral(self):
self.controller.SetCommand(0, 0, 0, 0)
self.despLateral = False
"""
def sube(self, alt):
alt_ini = self.controller.getNavdata().altd
alt_fin = alt_ini + alt
subiendo = True
while subiendo:
alt_act = self.controller.getNavdata().altd
if alt_act < alt_fin:
if self.controller.getLinearZ() == 0:
self.controller.SetCommand(0, 0, 0, 1)
else:
subiendo = False
self.controller.writeLog('YA ESTAMOS ARRIBA')
self.controller.SetCommand(0, 0, 0, 0)
def baja(self, alt):
alt_ini = self.controller.getNavdata().altd
alt_fin = alt_ini - alt
bajando = True
while bajando:
alt_act = self.controller.getNavdata().altd
if alt_act > alt_fin:
if self.controller.getLinearZ() == 0:
self.controller.SetCommand(0, 0, 0, -1)
else:
bajando = False
self.controller.writeLog('YA ESTAMOS ABAJO')
self.controller.SetCommand(0, 0, 0, 0)
"""
def sube(self):
self.controller.SetCommand(0, 0, 0, 0.15)
self.subiendo = True
def baja(self):
self.controller.SetCommand(0, 0, 0, -0.15)
self.bajando = True
def subeIzq(self):
self.controller.SetCommand(0.15, 0, 0, 0.15)
self.subiendoIzq = True
def subeDer(self):
self.controller.SetCommand(-0.15, 0, 0, 0.15)
self.subiendoDer = True
def bajaIzq(self):
self.controller.SetCommand(0.15, 0, 0, -0.15)
self.bajandoIzq = True
def bajaDer(self):
self.controller.SetCommand(-0.15, 0, 0, -0.15)
self.bajandoDer = True
def avanza(self, vel):
self.controller.SetCommand(0, vel, 0, 0)
def retrocede(self, vel):
self.controller.SetCommand(0, (-1)*vel, 0, 0)
def para(self):
self.controller.SetCommand(0, 0, 0, 0)
self.girando = False
self.despLatIzq = False
self.despLatDer = False
self.subiendoDer = False
self.subiendoIzq = False
self.bajandoDer = False
self.bajandoIzq = False
self.subiendo = False
self.bajando = False
class lector:
def __init__(self):
self.rate = rospy.Rate(10) #10Hz
self.sonido1 = '/home/david/proyecto/a.wav'
self.sonido2 = '/home/david/proyecto/blaster.wav'
self.contImg = 0
self.ordenes = Ordenes()
self.controller = BasicDroneController()
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.img = None
self.manualMode = False
self.nMuestra = 0
self.posX = 0
self.posY = 0
self.posZ = 0
self.oriX = 0
self.oriY = 0
self.oriZ = 0
self.oriW = 0
self.navdata = None
self.euler = None
#Subscribers
self.image_sub = rospy.Subscriber('/aruco_single/result', Image,
self.imageCallback)
#self.marker_center_sub = rospy.Subscriber('/aruco_single/markerCenter',Point,self.centerCallback)
#self.marker_points_sub = rospy.Subscriber('/aruco_single/markerPoints',points,self.pointsCallback)
self.marker_pose_sub = rospy.Subscriber('/aruco_single/pose',
PoseStamped, self.poseCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.ReceiveNavdata)
"""
def virtHorz(self, angY, angZ):
height, width, depth = self.img.shape
horz = (angY + 23) * 360/43
vert = (angZ + 30) * 32/3
if horz > 360:
horz = 360.0
elif horz < 0:
horz = 0.0
horz = int(round(horz))
if vert > 640:
vert = 640.0
elif vert < 0:
vert = 0.0
vert = int(round(vert))
cv2.line(self.img,(0,horz),(width,horz),(255,0,0),1)
cv2.line(self.img,(vert,0),(vert,height),(255,0,0),1)
"""
def virtHorz(self, angX, angY, angZ):
height, width, depth = self.img.shape
horz = (angY + 23) * 360 / 43
vert = (angZ + 30) * 32 / 3
if horz > 360:
horz = 360.0
elif horz < 0:
horz = 0.0
if vert > 640:
vert = 640.0
elif vert < 0:
vert = 0.0
x1 = -width / 2
y1 = horz - height / 2
x2 = width / 2
y2 = horz - height / 2
x1 = int(round(x1 * math.cos(angX) - y1 * math.sin(angX)))
y1 = int(round(x1 * math.sin(angX) + y1 * math.cos(angX)))
x2 = int(round(x2 * math.cos(angX) - y2 * math.sin(angX)))
y2 = int(round(x2 * math.sin(angX) + y2 * math.cos(angX)))
cv2.line(self.img, (x1, y1), (x2, y2), (255, 0, 0), 1)
#cv2.line(self.img,(vert,0),(vert,height),(255,0,0),1)
def ReceiveNavdata(self, navdata):
self.navdata = navdata
def poseCallback(self, poseS):
pose = poseS.pose
position = pose.position
orientation = pose.orientation
#height, width, depth = self.img.shape
self.posX = position.x
self.posY = position.y
self.posZ = position.z
self.oriX = orientation.x
self.oriY = orientation.y
self.oriZ = orientation.z
self.oriW = orientation.w
quaternion = (self.oriX, self.oriY, self.oriZ, self.oriW)
self.euler = tf.transformations.euler_from_quaternion(quaternion)
print quaternion
print 'Euler [0] ' + str(self.euler[0]) + ' Euler [1] ' + str(
self.euler[1]) + ' Euler [2] ' + str(self.euler[2])
"""
La componente z de position nos indica la distancia en metros que hay entre
la camara y el tag.
if self.nMuestra < 10:
self.posX = self.posX + position.x
self.posY = self.posY + position.y
self.posZ = self.posZ + position.z
self.oriX = self.oriX + orientation.x
self.oriY = self.oriY + orientation.y
self.oriZ = self.oriZ + orientation.z
self.oriW = self.oriW + orientation.w
self.nMuestra = self.nMuestra + 1
elif self.nMuestra == 10:
self.posX = self.posX/self.nMuestra
self.posY = self.posY/self.nMuestra
self.posZ = self.posZ/self.nMuestra
self.oriX = self.oriX/self.nMuestra
self.oriY = self.oriY/self.nMuestra
self.oriZ = self.oriZ/self.nMuestra
self.oriW = self.oriW/self.nMuestra
self.nMuestra = self.nMuestra + 1
else:
self.nMuestra = 0
print 'Position x ' + str(self.posX) + ' y ' + str(self.posY) + ' z ' + str(self.posZ)
quaternion = (self.oriX, self.oriY, self.oriZ, self.oriW)
self.euler = tf.transformations.euler_from_quaternion(quaternion)
print 'roll ' + str(self.euler[0]) + ' pitch ' + str(self.euler[1]) + ' yaw ' + str(self.euler[2])
"""
"""
def calDist(self, tam):
return 105.760431653/tam
def calAng(self, tam):
return 105.760431653*90/tam
def pointsCallback(self, points):
tam = math.sqrt(math.pow((points.x1 - points.x2), 2) + math.pow((points.y1 - points.y2), 2))
self.distancia = self.calDist(tam)
self.ang = self.calAng(tam)
#print "Distancia al tag " + str(self.distancia) + " angulo " + str(self.ang) + ' tam lado tag ' + str(tam)
"""
def centerCallback(self, center):
if self.img != None:
height, width, depth = self.img.shape
umbral = 20
if (center.y <= umbral + height / 2) and (center.y >
height / 2 - umbral):
self.controller.SetCommand(0, 0, 0, 0)
elif center.y < height / 2 + umbral:
self.controller.SetCommand(0, 0, 0, 1)
elif center.y > umbral + height / 2:
self.controller.SetCommand(0, 0, 0, -1)
def dibujaMira(self, width, height, img):
cv2.line(img, (0, height / 2), (width, height / 2), (0, 0, 255), 1)
cv2.line(img, (width / 2, 0), (width / 2, height), (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 4, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 10, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 20, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 30, (0, 0, 255), 1)
cv2.circle(img, (width / 2, height / 2), 40, (0, 0, 255), 1)
def imageCallback(self, data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
font = cv2.FONT_HERSHEY_SIMPLEX
height, width, depth = self.img.shape
self.dibujaMira(width, height, self.img)
cv2.putText(self.img, 'Angulo: ' + "%0.2f" % self.navdata.rotZ,
(0, 25), font, 0.75, (255, 255, 255), 1, 255)
"""
if (self.euler != None):
cv2.putText(self.img,'Euler[0]: ' + "%0.2f" % self.euler[0] ,(0,25), font, 0.75,(255,255,255),1,255)
cv2.putText(self.img,'Euler[1]: ' + "%0.2f" % self.euler[1] ,(0,50), font, 0.75,(255,255,255),1,255)
cv2.putText(self.img,'Euler[2]: ' + "%0.2f" % self.euler[2] ,(0,75), font, 0.75,(255,255,255),1,255)
cv2.putText(self.img,'Distancia: ' + "%0.2f" % self.posZ ,(0,100), font, 0.75,(255,255,255),1,255)
"""
"""
if self.navdata != None:
self.virtHorz(self.navdata.rotX, self.navdata.rotY, self.navdata.rotZ)
"""
except CvBridgeError, e:
print e
cv2.imshow("Image window", self.img)
cv2.waitKey(3)
class AutoDrive(object):
def __init__(self):
self.rotZ = 0
self.Zlock = Lock()
self.image = None
self.imageLock = Lock()
self.bridge = CvBridge()
rospy.init_node('ardrone_keyboard_controller')
self.controller = BasicDroneController()
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.receiveNavdata)
self.image_sub = rospy.Subscriber("/ardrone/bottom/image_raw", Image,
self.reciveImage)
self.action = 0 # 0 takeoff status , 1 go foward
def reciveImage(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.processImage(cv_image)
except CvBridgeError as e:
print(e)
#process recived image ,change the drone's action
def processImage(self, img):
cv2.imwrite('./drone.jpg', img)
self.action = 1
def turnLeft(self):
#self.Zlock.acquire()
self.controller.SetCommand(pitch=0)
while (self.rotZ <= 179):
self.controller.SetCommand(yaw_velocity=1)
self.controller.SetCommand(yaw_velocity=0)
#self.Zlock.release()
def receiveNavdata(self, navdata):
self.Zlock.acquire()
self.rotZ = navdata.rotZ
print self.rotZ
self.Zlock.release()
def drive(self):
start_time = rospy.get_rostime()
while not rospy.is_shutdown():
curent = rospy.get_rostime()
if (curent - start_time) < rospy.Duration(1, 0):
self.controller.SendTakeoff()
self.status = 0
elif (curent - start_time) < rospy.Duration(22, 0):
self.controller.SetCommand(pitch=1)
self.status = 1
elif (curent - start_time) < rospy.Duration(23, 0):
self.controller.SetCommand(pitch=0)
self.status = 2
elif (curent - start_time) < rospy.Duration(24, 0):
#self.controller.SetCommand(yaw_velocity=2)
self.turnLeft()
self.status = 3
rate = rospy.Rate(1)
elif (curent - start_time) < rospy.Duration(40.5, 0):
self.controller.SetCommand(pitch=1)
self.status = 4
else:
self.controller.SetCommand(pitch=0)
self.controller.SendLand()
class LQR_sim:
def __init__(self):
self.start_flying = True
self.stop_flying = False
self.return_to_home = False
self.is_takeoff = False
# self.PID = SimplePID()
self.drone_position_x = 0
self.drone_position_y = 0
self.drone_position_z = 0
self.drone_velocity_x = 0
self.drone_velocity_y = 0
self.drone_velocity_z = 0
self.drone_acceleration_x = 0
self.drone_acceleration_y = 0
self.drone_acceleration_z = 0
self.target_position_x = 0
self.target_position_y = 0
self.target_position_z = 0
self.target_velocity_x = 0
self.target_velocity_y = 0
self.target_velocity_z = 0
self.drone_yaw = 0
self.drone_yaw_radians = 0
self.vx = 0
self.vy = 0
self.vx1 = 0
self.vy1 = 0
self.ax = 0
self.ay = 0
self.controller = BasicDroneController()
self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata)
self.logger = logging.getLogger('LQR_simulation')
self.fileHandler_message = logging.StreamHandler(BufferedWriter(FileIO("LQR_simulation_data" + time.strftime("%Y%m%d-%H%M%S") + ".log", "w")))
self.logger.addHandler(self.fileHandler_message)
self.formatter_message = logging.Formatter('%(message)s')
self.fileHandler_message.setFormatter(self.formatter_message)
self.logger.setLevel(LoggerWarningLevel)
self.logger.info('Time;target_position_x,target_position_y,target_position_z;target_velocity_x,target_velocity_y,target_velocity_z;drone_position_x,drone_position_y,drone_position_z;drone_velocity_x,drone_velocity_y,drone_velocity_z,vx1,vy1,ax,ay')
self.logger_land = logging.getLogger('LQR_simulation_land')
self.fileHandler_message = logging.StreamHandler(BufferedWriter(FileIO("LQR_simulation_PD_land_data" + time.strftime("%Y%m%d-%H%M%S") + ".log", "w")))
self.logger_land.addHandler(self.fileHandler_message)
self.formatter_message = logging.Formatter('%(message)s')
self.fileHandler_message.setFormatter(self.formatter_message)
self.logger_land.setLevel(LoggerWarningLevel)
self.logger_land.info('Time;target_position_x,target_position_y,target_position_z;target_velocity_x,target_velocity_y,target_velocity_z;drone_position_x,drone_position_y,drone_position_z;drone_velocity_x,drone_velocity_y,drone_velocity_z,vx1,vy1,ax,ay')
def show_gazebo_models(self):
try:
model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
resp_coordinates_robot1 = model_coordinates('robot1', '')
resp_coordinates_quadrotor = model_coordinates('quadrotor', '')
self.target_position_x = resp_coordinates_robot1.pose.position.x
self.target_position_y = resp_coordinates_robot1.pose.position.y
self.target_position_z = resp_coordinates_robot1.pose.position.z
self.target_velocity_x = resp_coordinates_robot1.twist.linear.x
self.target_velocity_y = resp_coordinates_robot1.twist.linear.y
self.target_velocity_z = resp_coordinates_robot1.twist.linear.z
self.drone_position_x = resp_coordinates_quadrotor.pose.position.x
self.drone_position_y = resp_coordinates_quadrotor.pose.position.y
self.drone_position_z = resp_coordinates_quadrotor.pose.position.z
self.drone_velocity_x = resp_coordinates_quadrotor.twist.linear.x
self.drone_velocity_y = resp_coordinates_quadrotor.twist.linear.y
self.drone_velocity_z = resp_coordinates_quadrotor.twist.linear.z
distance = math.sqrt((self.drone_position_x-self.target_position_x)**2 + (self.drone_position_y-self.target_position_y)**2)
print "distance:",distance
except rospy.ServiceException as e:
rospy.loginfo("Get Model State service call failed: {0}".format(e))
def sleep(self, sec):
while(sec and not rospy.is_shutdown()):
if(detect==1):
break
time.sleep(1)
sec -= 1
def reset_drone(self):
self.controller.SetCommand(0,0,0,0)
def fight_control(self, roll, pitch, z_velocity, yaw_velocity):
self.controller.SetCommand(roll, pitch, z_velocity, yaw_velocity)
def yaw_control(delf, degrees):
if degrees > 0 and degrees < 180:
self.controller.SetCommand(0,0,0,-0.5)
else:
self.controller.SetCommand(0,0,0, 0.5)
def take_off(self, hight):
if hight > self.drone_position_z:
self.controller.SetCommand(0,0,1,0)
# print "go up, current hight:%f, desired hight:%f" % (self.drone_position_z, hight)
else:
self.is_takeoff = True
# print "take off successful"
def dlqr(self, A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = np.matrix(scipy.linalg.solve_discrete_are(A, B, Q, R))
#compute the LQR gain
K = np.matrix(scipy.linalg.inv(B.T*X*B+R)*(B.T*X*A))
eigVals, eigVecs = scipy.linalg.eig(A-B*K)
return K, X, eigVals
def land(self):
self.record_land_data()
u = [ self.target_position_x - self.drone_position_x,
self.target_position_y - self.drone_position_y,
0 ]
u_dot = [ self.target_velocity_x - self.drone_velocity_x,
self.target_velocity_y - self.drone_velocity_y,
0 ]
A = np.array([[1, 0, dt, 0], \
[0, 1, 0, dt], \
[0, 0, 1, 0 ], \
[0, 0, 0, 1 ]])
B = np.array([[dt**2/2, 0], \
[0, dt**2/2], \
[dt, 0], \
[0, dt]])
Q = q * np.array([[1, 0, 0, 0], \
[0, 1, 0, 0], \
[0, 0, 10, 0 ], \
[0, 0, 0, 10 ]])
R = (1-q) * np.array([[2, 0],\
[0, 2]])
state_error = - np.array([[u[0]], [u[1]], [u_dot[0]], [u_dot[1]]])
K, S, E = self.dlqr(A, B, Q, R)
a = np.dot(-K, state_error)
self.ax = a[0]
self.ay = a[1]
# ax = np.clip(ax, -5, 5)
# ay = np.clip(ay, -5, 5)
vx = self.drone_velocity_x + self.ax*0.1
vy = self.drone_velocity_y + self.ay*0.1
self.vx1 = math.cos(self.drone_yaw_radians)*vx + math.sin(self.drone_yaw_radians)*vy
self.vy1 = -math.sin(self.drone_yaw_radians)*vx + math.cos(self.drone_yaw_radians)*vy
self.fight_control(self.vx1, self.vy1, -0.5, 0)
# self.yaw_control(180)
if self.drone_position_z < 0.2:
self.controller.SendLand()
else:
return
def LQR_controller(self):
u = [ self.target_position_x - self.drone_position_x,
self.target_position_y - self.drone_position_y,
0 ]
u_dot = [ self.target_velocity_x - self.drone_velocity_x,
self.target_velocity_y - self.drone_velocity_y,
0 ]
A = np.array([[1, 0, dt, 0], \
[0, 1, 0, dt], \
[0, 0, 1, 0 ], \
[0, 0, 0, 1 ]])
B = np.array([[dt**2/2, 0], \
[0, dt**2/2], \
[dt, 0], \
[0, dt]])
Q = q * np.array([[1, 0, 0, 0], \
[0, 1, 0, 0], \
[0, 0, 10, 0 ], \
[0, 0, 0, 10 ]])
R = (1-q) * np.array([[2, 0],\
[0, 2]])
state_error = - np.array([[u[0]], [u[1]], [u_dot[0]], [u_dot[1]]])
K, S, E = self.dlqr(A, B, Q, R)
a = np.dot(-K, state_error)
self.ax = a[0]
self.ay = a[1]
# self.ax = np.clip(self.ax, -3, 3)
# self.ay = np.clip(self.ay, -3, 3)
self.vx = self.drone_velocity_x + self.ax*0.1
self.vy = self.drone_velocity_y + self.ay*0.1
self.vx1 = math.cos(self.drone_yaw_radians)*self.vx + math.sin(self.drone_yaw_radians)*self.vy
self.vy1 = -math.sin(self.drone_yaw_radians)*self.vx + math.cos(self.drone_yaw_radians)*self.vy
# self.vx1 = np.clip(self.vx1, -5, 5)
# self.vy1 = np.clip(self.vy1, -5, 5)
# self.fight_control(self.vx1, -self.vy1, 0, 0)
self.fight_control(self.vx1, self.vy1, 0, 0)
# desired_angle = 0
# input_yaw = desired_angle - self.drone_yaw
# self.yaw_control(input_yaw)
def record_data(self):
self.logger.info('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f' % \
( time.time(),
self.target_position_x,
self.target_position_y,
self.target_position_z,
self.target_velocity_x,
self.target_velocity_y,
self.target_velocity_z,
self.drone_position_x,
self.drone_position_y,
self.drone_position_z,
self.drone_velocity_x,
self.drone_velocity_y,
self.drone_velocity_z,
self.vx1,
self.vy1,
self.ax,
self.ay,
self.vx,
self.vy,
self.drone_yaw,
self.drone_acceleration_x,
self.drone_acceleration_y,
self.drone_acceleration_z
))
def record_land_data(self):
self.logger_land.info('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f' % \
( time.time(),
self.target_position_x,
self.target_position_y,
self.target_position_z,
self.target_velocity_x,
self.target_velocity_y,
self.target_velocity_z,
self.drone_position_x,
self.drone_position_y,
self.drone_position_z,
self.drone_velocity_x,
self.drone_velocity_y,
self.drone_velocity_z,
self.vx1,
self.vy1,
self.ax,
self.ay,
self.vx,
self.vy,
self.drone_yaw,
self.drone_acceleration_x,
self.drone_acceleration_y,
self.drone_acceleration_z
))
def CallbackUserCommand(self, data):
self.start_flying = data.point.x == 1
self.stop_flying = data.point.y == 1
self.return_to_home = data.point.z == 1
def CallbackAcceleartion(self, data):
self.drone_acceleration_x = data.linear_acceleration.x
self.drone_acceleration_y = data.linear_acceleration.y
self.drone_acceleration_z = data.linear_acceleration.z
def ReceiveNavdata(self, data):
self.drone_yaw = data.rotZ
self.drone_yaw_radians = math.radians(self.drone_yaw)
# print "yaw angle", self.drone_yaw , self.drone_yaw_radians
def TimerCallback(self, event):
self.show_gazebo_models()
if self.is_takeoff == False:
self.take_off(TakeOffHight)
else:
distance_2D = math.sqrt((self.drone_position_x - self.target_position_x)**2 + (self.drone_position_y - self.target_position_y)**2)
if distance_2D < 0.2 and EnableLanding:
self.land()
else:
self.LQR_controller()
self.record_data()
class PN_sim:
def __init__(self):
self.start_flying = True
self.stop_flying = False
self.return_to_home = False
self.is_takeoff = False
self.drone_position_x = 0
self.drone_position_y = 0
self.drone_position_z = 0
self.drone_velocity_x = 0
self.drone_velocity_y = 0
self.drone_velocity_z = 0
self.target_position_x = 0
self.target_position_y = 0
self.target_position_z = 0
self.target_velocity_x = 0
self.target_velocity_y = 0
self.target_velocity_z = 0
self.vx = 0
self.vy = 0
self.vx1 = 0
self.vy1 = 0
self.ax = 0
self.ay = 0
self.controller = BasicDroneController()
self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata)
self.logger = logging.getLogger('PN_simulation')
self.fileHandler_message = logging.StreamHandler(BufferedWriter(FileIO("PN_simulation_data" + time.strftime("%Y%m%d-%H%M%S") + ".log", "w")))
self.logger.addHandler(self.fileHandler_message)
self.formatter_message = logging.Formatter('%(message)s')
self.fileHandler_message.setFormatter(self.formatter_message)
self.logger.setLevel(LoggerWarningLevel)
self.logger.info('Time;target_position_x,target_position_y,target_position_z;target_velocity_x,target_velocity_y,target_velocity_z;drone_position_x,drone_position_y,drone_position_z;drone_velocity_x,drone_velocity_y,drone_velocity_z,vx1,vy1,ax,ay')
self.logger_land = logging.getLogger('PN_simulation_land')
self.fileHandler_message = logging.StreamHandler(BufferedWriter(FileIO("PN_simulation_PD_land_data" + time.strftime("%Y%m%d-%H%M%S") + ".log", "w")))
self.logger_land.addHandler(self.fileHandler_message)
self.formatter_message = logging.Formatter('%(message)s')
self.fileHandler_message.setFormatter(self.formatter_message)
self.logger_land.setLevel(LoggerWarningLevel)
self.logger_land.info('Time;target_position_x,target_position_y,target_position_z;target_velocity_x,target_velocity_y,target_velocity_z;drone_position_x,drone_position_y,drone_position_z;drone_velocity_x,drone_velocity_y,drone_velocity_z,vx1,vy1,ax,ay')
def show_gazebo_models(self):
try:
model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
resp_coordinates_robot1 = model_coordinates('robot1', '')
resp_coordinates_quadrotor = model_coordinates('quadrotor', '')
self.target_position_x = resp_coordinates_robot1.pose.position.x
self.target_position_y = resp_coordinates_robot1.pose.position.y
self.target_position_z = resp_coordinates_robot1.pose.position.z
self.target_velocity_x = resp_coordinates_robot1.twist.linear.x
self.target_velocity_y = resp_coordinates_robot1.twist.linear.y
self.target_velocity_z = resp_coordinates_robot1.twist.linear.z
self.drone_position_x = resp_coordinates_quadrotor.pose.position.x
self.drone_position_y = resp_coordinates_quadrotor.pose.position.y
self.drone_position_z = resp_coordinates_quadrotor.pose.position.z
self.drone_velocity_x = resp_coordinates_quadrotor.twist.linear.x
self.drone_velocity_y = resp_coordinates_quadrotor.twist.linear.y
self.drone_velocity_z = resp_coordinates_quadrotor.twist.linear.z
distance = math.sqrt((self.drone_position_x-self.target_position_x)**2 + (self.drone_position_y-self.target_position_y)**2)
print "distance:",distance
except rospy.ServiceException as e:
rospy.loginfo("Get Model State service call failed: {0}".format(e))
def sleep(self, sec):
while(sec and not rospy.is_shutdown()):
if(detect==1):
break
time.sleep(1)
sec -= 1
def reset_drone(self):
self.controller.SetCommand(0,0,0,0)
def fight_control(self, roll, pitch, z_velocity, yaw_velocity):
self.controller.SetCommand(roll, pitch, z_velocity, yaw_velocity)
def yaw_control(delf, degrees):
if degrees == self.drone_yaw:
return
else:
error_yaw = self.drone_yaw - degrees
if error_yaw > 0 and error < 180:
self.controller.SetCommand(0,0,0,1)
else:
self.controller.SetCommand(0,0,0,-1)
def take_off(self, hight):
if hight > self.drone_position_z:
self.controller.SetCommand(0,0,1,0)
# print "go up, current hight:%f, desired hight:%f" % (self.drone_position_z, hight)
else:
self.is_takeoff = True
# print "take off successful"
def land(self):
self.record_land_data()
u = [ self.target_position_x - self.drone_position_x,
self.target_position_y - self.drone_position_y,
0 ]
u_dot = [ self.target_velocity_x - self.drone_velocity_x,
self.target_velocity_y - self.drone_velocity_y,
0 ]
lamda = 4
Kp = 1
Kd = 2
m = 1.5 # I guess the mass is 0.5 kg
g = 9.8
a_tan = np.dot(Kp,u) + np.dot(Kd,u_dot)
a = a_tan
self.ax = a[0]
self.ay = a[1]
# ax = np.clip(ax, -5, 5)
# ay = np.clip(ay, -5, 5)
vx = self.drone_velocity_x + self.ax*0.1
vy = self.drone_velocity_y + self.ay*0.1
self.vx1 = math.cos(self.drone_yaw_radians)*vx + math.sin(self.drone_yaw_radians)*vy
self.vy1 = math.sin(self.drone_yaw_radians)*vx - math.cos(self.drone_yaw_radians)*vy
self.fight_control(self.vx1, -self.vy1, -0.5, 0)
if self.drone_position_z < 0.2:
self.controller.SendLand()
else:
return
# self.record_land_data()
# u = [ self.target_position_x - self.drone_position_x,
# self.target_position_y - self.drone_position_y,
# 0 ]
# u_dot = [ self.target_velocity_x - self.drone_velocity_x,
# self.target_velocity_y - self.drone_velocity_y,
# 0 ]
# A = np.array([[1, 0, dt, 0], \
# [0, 1, 0, dt], \
# [0, 0, 1, 0 ], \
# [0, 0, 0, 1 ]])
# B = np.array([[dt**2/2, 0], \
# [0, dt**2/2], \
# [dt, 0], \
# [0, dt]])
# q = 0.1
# Q = q * np.eye(4)
# R = (1-q) * np.eye(2)
# state_error = - np.array([[u[0]], [u[1]], [u_dot[0]], [u_dot[1]]])
# K, S, E = control.lqr(A, B, Q, R)
# a = np.dot(-K, state_error)
# self.ax = a[0]
# self.ay = a[1]
# # ax = np.clip(ax, -5, 5)
# # ay = np.clip(ay, -5, 5)
# vx = self.drone_velocity_x + self.ax*0.1
# vy = self.drone_velocity_y + self.ay*0.1
# self.vx1 = math.cos(self.drone_yaw_radians)*vx + math.sin(self.drone_yaw_radians)*vy
# self.vy1 = -math.sin(self.drone_yaw_radians)*vx + math.cos(self.drone_yaw_radians)*vy
# self.fight_control(self.vx1, self.vy1, -0.5, 0)
# if self.drone_position_z < 0.2:
# self.controller.SendLand()
# else:
# return
def PN_controller(self):
u = [ self.target_position_x - self.drone_position_x,
self.target_position_y - self.drone_position_y,
0 ]
u_dot = [ self.target_velocity_x - self.drone_velocity_x,
self.target_velocity_y - self.drone_velocity_y,
0 ]
lamda = 4
Kp = 1
Kd = 2
m = 1.5 # I guess the mass is 0.5 kg
g = 9.8
Omiga = np.cross(u,u_dot)/np.dot(u,u)
a_nor = -lamda*np.linalg.norm(u_dot)*np.cross(u/np.linalg.norm(u), Omiga)
a_tan = np.dot(Kp,u) + np.dot(Kd,u_dot)
a = a_nor + a_tan
self.ax = a[0]
self.ay = a[1]
# ax = np.clip(ax, -5, 5)
# ay = np.clip(ay, -5, 5)
self.vx = self.drone_velocity_x + self.ax*0.1
self.vy = self.drone_velocity_y + self.ay*0.1
self.vx1 = math.cos(self.drone_yaw_radians)*self.vx + math.sin(self.drone_yaw_radians)*self.vy
self.vy1 = math.sin(self.drone_yaw_radians)*self.vx - math.cos(self.drone_yaw_radians)*self.vy
# self.fight_control(self.vx1, -self.vy1, 0, 0)
if self.drone_position_z < 0.5:
self.fight_control(self.vx1, -self.vy1, 0, 0)
else:
self.fight_control(self.vx1, -self.vy1, 0, 0)
def record_data(self):
self.logger.info('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f' % \
( time.time(),
self.target_position_x,
self.target_position_y,
self.target_position_z,
self.target_velocity_x,
self.target_velocity_y,
self.target_velocity_z,
self.drone_position_x,
self.drone_position_y,
self.drone_position_z,
self.drone_velocity_x,
self.drone_velocity_y,
self.drone_velocity_z,
self.vx1,
self.vy1,
self.ax,
self.ay,
self.vx,
self.vy,
self.drone_yaw
))
def record_land_data(self):
self.logger_land.info('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f' % \
( time.time(),
self.target_position_x,
self.target_position_y,
self.target_position_z,
self.target_velocity_x,
self.target_velocity_y,
self.target_velocity_z,
self.drone_position_x,
self.drone_position_y,
self.drone_position_z,
self.drone_velocity_x,
self.drone_velocity_y,
self.drone_velocity_z,
self.vx1,
self.vy1,
self.ax,
self.ay,
self.vx,
self.vy,
self.drone_yaw
))
def CallbackUserCommand(self, data):
self.start_flying = data.point.x == 1
self.stop_flying = data.point.y == 1
self.return_to_home = data.point.z == 1
def ReceiveNavdata(self, data):
self.drone_yaw = data.rotZ
self.drone_yaw_radians = math.radians(self.drone_yaw)
def TimerCallback(self, event):
self.show_gazebo_models()
if self.is_takeoff == False:
self.take_off(TakeOffHight)
else:
distance_2D = math.sqrt((self.drone_position_x - self.target_position_x)**2 + (self.drone_position_y - self.target_position_y)**2)
if distance_2D < 0.2:
self.land()
else:
self.PN_controller()
self.record_data()
class Fly:
def __init__(self):
#Drone's odometry subscriber
self.sub_odom = rospy.Subscriber("/drone_control/odom_converted",
Odometry, self.callback_odom)
#Aruco's data subscribers
self.sub_aruco = rospy.Subscriber("/drone_control/aruco_detected",
Bool, self.callback_detected)
self.sub_aruco_center = rospy.Subscriber("/drone_control/aruco_center",
Float32MultiArray,
self.callback_center)
self.sub_aruco_id = rospy.Subscriber("/drone_control/aruco_id", Int8,
self.callback_id)
#Drone's linear and anglar velocity publishers
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.pub_angle = rospy.Publisher('/drone_control/drone_angle',
Float32,
queue_size=1)
self.controller = BDC()
self.twist = Twist()
#Drone's coordinates
self.drone_x = 0
self.drone_y = 0
self.drone_z = 0
#angle towards aruco marker
self.angle = 0
#Difference between drone's angle and its destination
self.diff_angle = 0
self.aruco_center = (320, 180)
self.Image_center = (320, 180)
self.old_aruco_center = None
#The variable is incremented each frame the aruco marker is detected
self.detection_counter = 0
#Aruco markers' coordinates dictionary
self.dest_dict = OrderedDict()
self.dest_dict['1'] = (3, -14)
self.dest_dict['10'] = (-14, 2)
self.dest_dict['20'] = (-4, 13)
self.dest_dict['50'] = (11, 4)
self.dest_dict['100'] = (0, 0)
self.dest_id = self.dest_dict.keys()[0]
self.marker_coords = self.dest_dict[self.dest_id]
#Additional boolean variables
self.aruco_id = None
self.detected = False
self.rotated = False
self.on_target = False
def callback_id(self, message):
#Get aruco's id
self.aruco_id = message.data
def callback_center(self, message):
#Get aruco's center coordinates (in pixels)
self.aruco_center = message.data
def rotate_2_angle_and_fly(self):
#Rotate towards the next marker
if self.diff_angle >= 5:
self.controller.SetCommand(yaw_velocity=0.3, pitch=0)
elif self.diff_angle <= -5:
self.controller.SetCommand(yaw_velocity=-0.3, pitch=0)
#If the angle is correct fly forward to the next marker
else:
self.controller.SetCommand(pitch=1)
def land_on_target(self):
#Calculate the difference (in pixels) between image's center and detected aruco's center
x_diff = self.Image_center[0] - self.aruco_center[0]
y_diff = self.Image_center[1] - self.aruco_center[1]
x_diff = x_diff / (self.Image_center[0])
y_diff = y_diff / (self.Image_center[1])
#Drone's landing movement is slower if aruco marker's visibility keeps getting obstructed
if self.detection_counter <= 2:
self.controller.SetCommand(pitch=x_diff * 0.3,
roll=y_diff * 0.3,
z_velocity=-0.5)
else:
self.controller.SetCommand(pitch=x_diff,
roll=y_diff,
z_velocity=-0.3)
#If the drone is low enough commence a built-in landing sequence
if self.drone_z < 1.5:
self.controller.SetCommand(pitch=0, roll=0, z_velocity=0)
self.controller.SendLand()
#If the drone has landed, prepare for the next destination
if self.dest_dict and self.controller.status == DroneStatus.Landed:
self.on_target = True
self.dest_dict.popitem(last=False)
self.dest_id = self.dest_dict.keys()[0]
self.marker_coords = self.dest_dict[self.dest_id]
self.detection_counter = 0
rospy.sleep(3)
self.on_target = False
else:
print("Finished flying!")
def callback_odom(self, odom):
#Get drone's coordinates
self.drone_x = odom.pose.pose.position.x
self.drone_y = odom.pose.pose.position.y
self.drone_z = odom.pose.pose.position.z
#Find an angle towards aruco marker
angle = math.degrees(
self.calculate_angle((self.drone_x, self.drone_y),
self.marker_coords))
#Get an angular difference between destination's angle and drone's current angle
self.diff_angle = angle - self.controller.rotation
print("kat:", self.controller.rotation)
print("kurs:", angle)
print("diff:", self.diff_angle)
#Take off if the drone is on the ground
if self.controller.status == DroneStatus.Landed and self.dest_dict and not self.on_target:
self.controller.SendTakeoff()
#Keep going up if the drone is flying low
if self.controller.status == DroneStatus.Flying and self.drone_z < 10:
self.controller.SetCommand(z_velocity=2)
#If the drone is high enough, fly towards the next destination
elif self.controller.status == DroneStatus.Flying and self.drone_z >= 10:
self.rotate_2_angle_and_fly()
def callback_detected(self, message):
self.detected = message.data
#If the proper aruco marker is detected, begin landing on it
if self.detected and self.aruco_id == int(self.dest_id):
self.detection_counter += 1
self.land_on_target()
def calculate_angle(self, point1, point2):
#Calculate an angle towards aruco marker
angle = math.atan2((point2[1] - point1[1]), (point2[0] - point1[0]))
print('wspolrzedne drona:', point1[0], point1[1])
print('wspolrzedne markera:', point2[0], point2[1])
return angle
