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)
