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 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 TakeoffDirective(AbstractDroneDirective): # sets up this directive def __init__(self): self.controller = BasicDroneController("Takeoff 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 taking off # # 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.SendTakeoff() rospy.logwarn("Drone is taking off") return 1, (0, 0, 0, 0), image, (None, None), 0, 0, None
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) print "Land" controller.SendLand() #pub2 = rospy.Subscriber("/visualization_marker", Marker, detect_tag)
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 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