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