def fly(drone): print "Going to go for a small trip ..." # drone.videoCbk = videoCallback drone.videoEnable() try: drone.trim() drone.takeoff() drone.takePicture() speed = 5 drone.update(cmd=movePCMDCmd(True, 0, speed, 0, 0)) drone.flyToAltitude(1.5) drone.wait(2.0) drone.update(cmd=movePCMDCmd(True, 0, speed, 0, 0)) drone.flyToAltitude(0.5) drone.takePicture() drone.wait(2.0) drone.update(cmd=movePCMDCmd(True, 0, speed, 0, 0)) drone.flyToAltitude(1) drone.wait(2.0) drone.takePicture() # Stop drone.update(cmd=movePCMDCmd(True, 0, 0, 0, 0)) drone.land() except ManualControlException, e: print print "ManualControlException" if drone.flyingState is None or drone.flyingState == 1: # taking off drone.emergency() drone.land()
def fly( drone ): print "Going to go for a small trip ..." # drone.videoCbk = videoCallback drone.videoEnable() try: drone.trim() drone.takeoff() drone.takePicture() speed = 5 drone.update( cmd=movePCMDCmd( True, 0, speed, 0, 0 ) ) drone.flyToAltitude( 1.5 ) drone.wait( 2.0 ) drone.update( cmd=movePCMDCmd( True, 0, speed, 0, 0 ) ) drone.flyToAltitude( 0.5 ) drone.takePicture() drone.wait( 2.0 ) drone.update( cmd=movePCMDCmd( True, 0, speed, 0, 0 ) ) drone.flyToAltitude( 1 ) drone.wait( 2.0 ) drone.takePicture() # Stop drone.update( cmd=movePCMDCmd( True, 0, 0, 0, 0 ) ) drone.land() except ManualControlException, e: print print "ManualControlException" if drone.flyingState is None or drone.flyingState == 1: # taking off drone.emergency() drone.land()
def videoCallback( frame, robot=None, debug=False ): if frame: print "Video", len(frame) # workaround for a single frame f = open( TMP_VIDEO_FILE, "wb" ) f.write( frame[-1] ) f.close() cap = cv2.VideoCapture( TMP_VIDEO_FILE ) ret, img = cap.read() cap.release() if ret: detImg, detected = detectTwoColors( img, loadColors("cap-colors.txt") ) print "Detected", detected if len(detected) > 0: target = detected[0] for alt in detected[1:]: if alt[1] > target[1]: # area target = alt print "Target", target, img.shape cv2.circle( img, target[0], 20, (255,0,0), 3) if robot: diff = target[0][0] - 320 if diff < 0: # robot.moveCamera( robot.cameraTilt, robot.cameraPan - 1 ) robot.update( movePCMDCmd(active=True, roll=0, pitch=0, yaw=-10, gaz=0) ) elif diff > 0: # robot.moveCamera( robot.cameraTilt, robot.cameraPan + 1 ) robot.update( movePCMDCmd(active=True, roll=0, pitch=0, yaw=+10, gaz=0) ) if debug: cv2.imshow('image', img) key = cv2.waitKey(200)
def testAutomaticLanding(drone): "Takeoff and land" drone.setVideoCallback(videoCallback, videoCallbackResults) drone.moveCamera(tilt=-100, pan=0) drone.videoEnable() try: drone.trim() drone.takeoff() print "COMPLETED", drone.altitude prev = drone.lastImageResult up = 0 left = 0 fwd = 0 for i in xrange(1000): if drone.altitude > 1.5: up = -20 elif drone.altitude < 1.3: up = 20 else: up = 0 print "ALT", drone.altitude, up if prev != drone.lastImageResult: left, fwd = 0, 0 print drone.lastImageResult prev = drone.lastImageResult if drone.lastImageResult is not None: index, result = drone.lastImageResult if result is not None: (centerX, centerY), point = result if centerY < 368 / 2 - 30: fwd = 20 elif centerY > 368 / 2 + 30: fwd = -20 else: fwd = 0 if centerX < 640 / 2 - 30: left = 20 elif centerY > 640 / 2 + 30: left = -20 else: left = 0 print "HETREERERE!", left, fwd print i, drone.update(cmd=movePCMDCmd( active=True, roll=left, pitch=fwd, yaw=0, gaz=up)) drone.land() except ManualControlException, e: print print "ManualControlException" drone.hover() if drone.flyingState is None or drone.flyingState == 1: # taking off drone.emergency() drone.land()
def videoCallback(frame, robot=None, debug=False): if frame: print "Video", len(frame) # workaround for a single frame f = open(TMP_VIDEO_FILE, "wb") f.write(frame[-1]) f.close() cap = cv2.VideoCapture(TMP_VIDEO_FILE) ret, img = cap.read() cap.release() if ret: detImg, detected = detectTwoColors(img, loadColors("cap-colors.txt")) print "Detected", detected if len(detected) > 0: target = detected[0] for alt in detected[1:]: if alt[1] > target[1]: # area target = alt print "Target", target, img.shape cv2.circle(img, target[0], 20, (255, 0, 0), 3) if robot: diff = target[0][0] - 320 if diff < 0: # robot.moveCamera( robot.cameraTilt, robot.cameraPan - 1 ) robot.update( movePCMDCmd(active=True, roll=0, pitch=0, yaw=-10, gaz=0)) elif diff > 0: # robot.moveCamera( robot.cameraTilt, robot.cameraPan + 1 ) robot.update( movePCMDCmd(active=True, roll=0, pitch=0, yaw=+10, gaz=0)) if debug: cv2.imshow('image', img) key = cv2.waitKey(200)
def testAutomaticLanding( drone ): "Takeoff and land" drone.setVideoCallback( videoCallback, videoCallbackResults ) drone.moveCamera( tilt=-100, pan=0 ) drone.videoEnable() try: drone.trim() drone.takeoff() print "COMPLETED", drone.altitude prev = drone.lastImageResult up = 0 left = 0 fwd = 0 for i in xrange(1000): if drone.altitude > 1.5: up = -20 elif drone.altitude < 1.3: up = 20 else: up = 0 print "ALT", drone.altitude, up if prev != drone.lastImageResult: left, fwd = 0, 0 print drone.lastImageResult prev = drone.lastImageResult if drone.lastImageResult is not None: index,result = drone.lastImageResult if result is not None: (centerX,centerY), point = result if centerY < 368/2 - 30: fwd = 20 elif centerY > 368/2 + 30: fwd = -20 else: fwd = 0 if centerX < 640/2 - 30: left = 20 elif centerY > 640/2 + 30: left = -20 else: left = 0 print "HETREERERE!", left, fwd print i, drone.update( cmd=movePCMDCmd( active=True, roll=left, pitch=fwd, yaw=0, gaz=up ) ) drone.land() except ManualControlException, e: print print "ManualControlException" drone.hover() if drone.flyingState is None or drone.flyingState == 1: # taking off drone.emergency() drone.land()