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
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 __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 __init__(self):
# getting access to elements in DroneVideo and FlightstatsReciever
super(DroneMaster, self).__init__()
self.objectName = "Test Object"
self.startingAngle = 0
self.circleRadius = 1 #meters
self.circlePoints = 8 #numbers of points equally spaced along circle
self.startTime = 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_ws/src/SVCL_ardrone_automation/src/Flight_Info/" +
self.objectName + "_Flight_" +
datetime.datetime.now().strftime("%m-%d-%Y__%H:%M:%S, %A") + "/")
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_ws/src/SVCL_ardrone_automation/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
# i1nitalizing 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.emergency = False
self.captureRound = 0.5
self.oldBattery = -1
self.photoDirective = CapturePhotoDirective(self.droneRecordPath, 30,
0.014, self.objectName,
self.circlePoints, 1000, 0)
self.keySub = rospy.Subscriber('/controller/keyboard', ROSString,
self.keyPress)
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 __init__(self, camera):
# camera=0 is for front camera, camera=1 is for bottom camera
if camera == "FRONT":
self.camera = 0
elif camera == "BOTTOM":
self.camera = 1
else:
raise Exception("Camera must be 0 (front) or 1 (bottom)")
self.controller = BasicDroneController("Toggle Camera Directive")
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 __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 __init__(self):
# getting access to elements in DroneVideo and FlightstatsReciever
super(DroneMaster,self).__init__()
# 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"+"/")
if not os.path.exists(self.droneRecordPath):
os.makedirs(self.droneRecordPath)
self.logger = Logger(self.droneRecordPath, "AR Drone Flight")
self.logger.Start()
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()
#self.stateMachine = StateMachine((ReturnToColorDirective('orange'),30))
# drone starts without any machine loaded, so that it can be controlled using the keyboard
self.currMachine = None
# initalizing helper objects
self.process = ProcessVideo()
self.controller = BasicDroneController("TraceCircle")
self.startTimer = time.clock()
self.waitTime = 0
self.moveTime = 0
class SetupDirective(AbstractDroneDirective):
# sets up this directive
def __init__(self):
self.controller = BasicDroneController("Setup Directive")
# given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 1 if algorithm is finished and the drone is now ready to fly
#
# A tuple of (xspeed, yspeed, yawspeed, zspeed):
# indicating the next instructions to fly the drone
#
# An image reflecting what is being done as part of the algorithm
def RetrieveNextInstruction(self, image, navdata):
state = navdata["state"][1]
# Drone is currently in emergency mode, unset it
if state == 0:
self.controller.SendEmergency()
rospy.logwarn("Drone is set up to fly")
return 1, (0, 0, 0, 0), image, (None, None), 0, 0, None
class Despega:
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.despega_sub = rospy.Subscriber('Despega', Empty, self.callback)
def callback(self, data):
self.controller.SendTakeoff()
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 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)
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
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()
class FlatTrimDirective(AbstractDroneDirective):
def __init__(self):
self.controller = BasicDroneController("Flat Trim Directive")
def RetrieveNextInstruction(self, image, navdata):
self.controller.FlatTrim()
rospy.logwarn("Reset Flat Trim")
return 1, (0, 0, 0, 0), image, (None, None), 0, 0, None
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
class navdata(object):
def __init__(self):
# Holds the current drone status
self.status = -1
self.navdata = None
self.controller = BasicDroneController()
def getRotZ(self):
rotZ = self.controller.getNavdata().rotZ
if rotZ < 0:
return rotZ + 360
else:
return rotZ
def __init__(self):
# Always do Qt init first.
QDialog.__init__(self)
self.controller = BasicDroneController()
# Set up the user interface from Designer.
uic.loadUi(join(dirname(__file__), 'mav_control.ui'), self)
self.setWindowTitle('AR.Drone Video Feed')
self.cv = CvBridge()
self.trackingColor = np.array([1, 0, 0], dtype=np.float32)
class navdata(object):
def __init__(self):
# Holds the current drone status
self.status = -1
self.navdata = None
self.controller = BasicDroneController()
def getRotZ(self):
rotZ = self.controller.getNavdata().rotZ
if rotZ < 0:
return rotZ + 360
else:
return rotZ
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.roll = 0
self.pitch = 0
self.yaw = 0
self.zVelocity = 0
self.cameraHeight = 0
self.cameraWidth = 0
self.command = Twist()
#Subscribers
self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw', Image,
self.imageCallback)
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.ReceiveNavdata)
self.subCameraInfo = rospy.Subscriber('/ardrone/camera_info',
CameraInfo,
self.cameraInfoCallback)
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
class SetCameraDirective(AbstractDroneDirective):
# sets up this directive
def __init__(self, camera):
# camera=0 is for front camera, camera=1 is for bottom camera
if camera == "FRONT":
self.camera = 0
elif camera == "BOTTOM":
self.camera = 1
else:
raise Exception("Camera must be 0 (front) or 1 (bottom)")
self.controller = BasicDroneController("Toggle Camera Directive")
# given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 1 if algorithm is finished and the drone's camera has been set
#
# A tuple of (xspeed, yspeed, yawspeed, zspeed):
# indicating the next instructions to fly the drone
#
# An image reflecting what is being done as part of the algorithm
def RetrieveNextInstruction(self, image, navdata):
self.controller.SwitchCamera(self.camera)
if self.camera == 0:
cameraName = "front camera"
else:
cameraName = "bottom camera"
rospy.logwarn("Set Drone Camera to: " + cameraName)
return 1, (0, 0, 0, 0), image, (None, None), 0, 0, None
self.pitch = 0
self.z_velocity = 0
elif key == KeyMapping.VisionMode:
rospy.logwarn("Unsubscribing")
self.image_sub.unregister()
self.vision_mode = 0
# finally we set the command to be sent. The controller handles sending this at regular intervals
controller.SetCommand(self.roll, self.pitch, self.yaw_velocity,
self.z_velocity)
# Setup the application
if __name__ == '__main__':
import sys
# Firstly we setup a ros node, so that we can communicate with the other packages
rospy.init_node('ardrone_keyboard_controller')
# Now we construct our Qt Application and associated controllers and windows
app = QtGui.QApplication(sys.argv)
controller = BasicDroneController()
display = KeyboardController()
display.show()
# executes the QT application
status = app.exec_()
# and only progresses to here once the application has been shutdown
rospy.signal_shutdown('Great Flying!')
sys.exit(status)
print "Stop"
controller.SetCommand(0, 0, 0, 0)
rospy.sleep(5)
print "Down"
controller.SetCommand(0, 0, 0, -0.3)
rospy.sleep(3)
print "Stop"
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)
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.giraDer_sub = rospy.Subscriber('GiraDer', Empty, self.callback)
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
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.despega_sub = rospy.Subscriber('Despega', Empty, self.callback)
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()
def search_pattern():
controller.SetCommand(0, 0, 0, 1.5)
sleep(5)
controller.SetCommand(0, 0, 0, 0)
sleep(1)
while not rospy.is_shutdown() and detect == 0:
controller.SetCommand(0, 1, 0, 0)
sleep(1)
controller.SetCommand(-1, 0, 0, 0)
sleep(9)
controller.SetCommand(0, 1, 0, 0)
sleep(1)
controller.SetCommand(1, 0, 0, 0)
sleep(9)
if __name__ == '__main__':
rospy.init_node('autonomous_search', anonymous=True)
pub1 = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
controller = BasicDroneController()
controller.StartSendCommand()
time.sleep(1)
detect = 0
controller.SendTakeoff()
controller.StartSendCommand()
pub2 = rospy.Subscriber("/visualization_marker", Marker, detect_tag)
search_pattern()
reset_drone(pub1)
rospy.spin()
def __init__(self):
self.controller = BasicDroneController()
#Subscribers
self.giraDer_sub = rospy.Subscriber('GiraDer',Empty,self.callback)
def __init__(self):
# Holds the current drone status
self.status = -1
self.navdata = None
self.controller = BasicDroneController()
def __init__(self):
self.controller = BasicDroneController("Flat Trim Directive")
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()
