class Aterriza:
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.aterriza_sub = rospy.Subscriber('Aterriza',Empty,self.callback)
def callback(self,data):
self.controller.SendLand()
class LandDirective(AbstractDroneDirective):
# sets up this directive
def __init__(self):
self.controller = BasicDroneController("Land 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 landing
#
# 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):
rospy.logwarn(" __________ Drone is landing __________ ")
self.controller.SendLand()
return 1, (0, 0, 0, 0), image, (None, None), 0, 0, None
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()
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 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)])
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
