def save_snaps(width=0, height=0, name="snapshot", folder=".", app_ip=None): aircraft = Aircraft(app_ip) streaming_manager = aircraft.getLiveStreamManager() cv2_manager = streaming_manager.getCV2Manager() cv2_manager.setWidth(width) cv2_manager.setHeigth(height) result = cv2_manager.startStream("9003") print("result startStream %s" % result) if isinstance(result, CustomError): raise Exception("%s" % result) try: if not os.path.exists(folder): os.makedirs(folder) # ----------- CREATE THE FOLDER ----------------- folder = os.path.dirname(folder) try: os.stat(folder) except: os.mkdir(folder) except: pass nSnap = 0 w = width h = height fileName = "%s/%s_%d_%d_" % (folder, name, w, h) cv2.namedWindow("output", cv2.WINDOW_NORMAL) cv2.resizeWindow("output", 150, 150) while True: frame = cv2_manager.getFrame() if frame is None: continue cv2.imshow('output', frame) key = cv2.waitKey(1) & 0xFF if key == ord('q'): break if key == ord(' '): print("Saving image ", nSnap) frame = cv2.resize(frame, (w, h)) cv2.imwrite("%s%d.jpg" % (fileName, nSnap), frame) nSnap += 1 cv2.destroyAllWindows() cv2_manager.stopStream()
import cv2, time from dji_asdk_to_python.products.aircraft import Aircraft from dji_asdk_to_python.errors import CustomError APP_IP = "192.168.50.158" STREAMING_DURATION = 1000 # seconds aircraft = Aircraft(APP_IP) streaming_manager = aircraft.getLiveStreamManager() rtp_manager = streaming_manager.getRTPManager() rtp_manager.setWidth(1920) rtp_manager.setHeigth(1080) result = rtp_manager.startStream() print("result startStream %s" % result) start = time.time() elapsed_seconds = 0 if isinstance(result, CustomError): raise Exception("%s" % result) while elapsed_seconds < STREAMING_DURATION: end = time.time() elapsed_seconds = end - start frame = rtp_manager.getFrame() if frame is None: continue
class ArucoLanding: """ Inits the Stage2 class Parameters: drone_ip (str) -> The IP of the drone camera_matrix (ndarray) -> The camera matrix of the drone's camera camera_distortion (ndarray) -> The camera distortion of the drone's camera marker_id (int) -> The ID of the aruco marker to be detected on the landing stage marker_size_cm (int) -> The size in CM of the aruco marker to be detected in the stage """ def __init__(self, drone_ip, camera_matrix, camera_distortion, marker_id, marker_size_cm): self.aircraft = Aircraft(drone_ip) self.marker_id = marker_id self.marker_size_cm = marker_size_cm self.ast = ArucoSingleTracker(camera_distortion=camera_distortion, camera_matrix=camera_matrix) self.rtp_manager = self.aircraft.getLiveStreamManager().getRTPManager() self.p = 0.004 self.i = 0.000005 self.d = 0.0005 self.pidx = PID(P=self.p, I=self.i, D=self.d) self.pidy = PID(P=self.p, I=self.i, D=self.d) self.pidz = PID(P=self.p, I=self.i, D=self.d) self.pidx.SetPoint = 0.0 self.pidy.SetPoint = 10.0 self.pidz.SetPoint = 100.0 self.pidx.setSampleTime(0.1) self.pidy.setSampleTime(0.1) self.pidz.setSampleTime(0.1) self.yaw_margin = 15 def start(self): self.rtp_manager.setWidth(1280) self.rtp_manager.setHeigth(720) self.rtp_manager.startStream() result = self.rtp_manager.startStream() print("result startStream %s" % result) if isinstance(result, CustomError): raise Exception("%s" % result) print( "--------------------------------------- STAGE 1 --------------------------------------------------------" ) gimbal = self.aircraft.getGimbal() gimbal.rotate(-90, 0, 0) print("Gimbal set to -90 degrees") fc = self.aircraft.getFlightController() fc.setVirtualStickModeEnabled(True) fcd = FlightControlData() while True: fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0) flight_controller_state = fc.getState() if isinstance(flight_controller_state, FlightControllerState): flying = flight_controller_state.isFlying() if not flying: break frame = self.rtp_manager.getFrame() if frame is None: continue ( marker_found, x_marker, y_marker, z_marker, x_camera, y_camera, z_camera, roll_marker, yaw_marker, pitch_marker, roll_marker, roll_camera, yaw_camera, pitch_camera, ) = self.ast.track(frame, self.marker_id, self.marker_size_cm) if marker_found: print("x_marker %s y_marker %s z_marker %s" % (x_marker, y_marker, z_marker)) print("yaw_camera %s" % yaw_camera) if abs(yaw_camera) > self.yaw_margin: print("CORRECTING YAW") if yaw_camera < 0: fcd.setYaw(10) else: fcd.setYaw(-10) self.pidx.update(x_marker) self.pidy.update(y_marker) self.pidz.update(z_marker) xoutput = self.pidx.output youtput = self.pidy.output zoutput = self.pidz.output print("X output:%s" % xoutput) print("Y output:%s" % youtput) print("Z output:%s" % zoutput) fcd.setPitch(youtput) fcd.setRoll(xoutput * -1) if z_marker > 100 and abs(yaw_camera) < 20: print("DESCENDING") if abs(zoutput) > 4: fcd.setVerticalThrottle(-2) else: fcd.setVerticalThrottle(zoutput) else: fcd.setVerticalThrottle(0) if z_marker < 120 and abs(yaw_camera) < 15 and abs( x_camera) < 8 and abs(y_camera - 10) < 8: fc.startLanding() print("LANDING") self.rtp_manager.stopStream() print("pitch %s, roll %s, yaw %s, throttle %s" % (fcd.getPitch(), fcd.getRoll(), fcd.getYaw(), fcd.getVerticalThrottle())) fc.sendVirtualStickFlightControlData(fcd) fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0) cv2.destroyAllWindows() def stop_streaming(self): self.rtp_manager.stopStream()
class ArucoLanding: """ Inits ArucoLanding class Parameters: drone_ip (str) -> The IP of the drone camera_matrix (ndarray) -> The camera matrix of the drone's camera camera_distortion (ndarray) -> The camera distortion of the drone's camera marker_id (int) -> The ID of the aruco marker to be detected on the landing stage marker_size_cm (int) -> The size in CM of the aruco marker to be detected in the stage """ def __init__(self, drone_ip, camera_matrix, camera_distortion, marker_id, marker_size_cm): self.aircraft = Aircraft(drone_ip) self.marker_id = marker_id self.marker_size_cm = marker_size_cm self.ast = ArucoSingleTracker(camera_distortion=camera_distortion, camera_matrix=camera_matrix) self.rtp_manager = self.aircraft.getLiveStreamManager().getRTPManager() self.p = 0.004 self.i = 0.000005 self.d = 0.0005 self.pidx = PID(P=self.p, I=self.i, D=self.d) self.pidy = PID(P=self.p, I=self.i, D=self.d) self.pidz = PID(P=self.p, I=self.i, D=self.d) self.pidx.SetPoint = 0.0 self.pidy.SetPoint = 20.0 self.pidz.SetPoint = 0.0 self.pidx.setSampleTime(0.03) self.pidy.setSampleTime(0.03) self.pidz.setSampleTime(0.03) self.yaw_margin = 15 def start(self): self.rtp_manager.setWidth(1280) self.rtp_manager.setHeigth(720) self.rtp_manager.startStream() result = self.rtp_manager.startStream() print("result startStream %s" % result) if isinstance(result, CustomError): raise Exception("%s" % result) gimbal = self.aircraft.getGimbal() gimbal.rotate(-90, 0, 0) print("Gimbal set to -90 degrees") # send_set_iso(self.pk_drone, "ISO_100", "SHUTTER_SPEED_1_8000") fc = self.aircraft.getFlightController() fc.setVirtualStickModeEnabled(True) fcd = FlightControlData() # get_down = False while True: fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0) frame = self.rtp_manager.getFrame() if isinstance(frame, None): continue frame = cv2.resize(frame, (1280, 720), interpolation=cv2.INTER_AREA) ( marker_found, x_marker, y_marker, z_marker, x_camera, y_camera, z_camera, roll_marker, yaw_marker, pitch_marker, roll_marker, roll_camera, yaw_camera, pitch_camera, ) = self.ast.track(frame, self.marker_id, self.marker_size_cm) if marker_found: print("x_marker %s y_marker %s z_marker %s" % (x_marker, y_marker, z_marker)) print("yaw_camera %s" % yaw_camera) if abs(yaw_camera) > self.yaw_margin: print("CORRECTING YAW") if yaw_camera < 0: fcd.setYaw(5) else: fcd.setYaw(-5) self.pidx.update(x_marker) self.pidy.update(y_marker) xoutput = self.pidx.output youtput = self.pidy.output print("X output:%s" % xoutput) print("Y output:%s" % youtput) fcd.setPitch(youtput) fcd.setRoll(xoutput * -1) if z_marker > 180 and abs(yaw_camera) < 20: self.pidz.update(z_marker) zoutput = self.pidz.output fcd.setVerticalThrottle(-abs(zoutput) / 5) print("Z output:%s" % zoutput) if z_marker < 200 and abs(yaw_camera) < 30 and math.sqrt( math.pow(x_marker, 2) + math.pow(y_marker, 2)) < 15: fc.startLanding() print("LANDING") time.sleep(10) # esperar a que haga landing flight_controller_state = fc.getState() if not isinstance(flight_controller_state, FlightControllerState): continue flying = flight_controller_state.isFlying() if flying is not None and flying: fc.move_distance(pitch_distance=0.1, roll_distance=0, throttle_distance=2, meters_per_second=0.3, order=["THROTTLE", "PITCH", "ROLL"]) # send_move_distance(self.pk_drone, 0, 0, 200, 70, "PITCH") gimbal.rotate(-90, 0, 0) fc.setVirtualStickModeEnabled(True) else: # send_set_iso_auto(self.pk_drone) # send_set_iso(self.pk_drone,"ISO_200","SHUTTER_SPEED_1_320") break fc.sendVirtualStickFlightControlData(fcd) fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0)