Exemplo n.º 1
0
    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
Exemplo n.º 2
0
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 numpy as np

from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.precision_landing.landing import ArucoLanding

APP_IP = "192.168.20.37 "
MARKER_ID = 17
MARKER_SIZE_CM = 60
IMAGE_WIDTH = 1280
IMAGE_HEIGHT = 720
CAMERA_MATRIX = np.loadtxt(
    "/home/aras/aras-current/dji-asdk-to-python/examples/calibration/camera_matrix.txt",
    delimiter=",")
CAMERA_DISTORTION = np.loadtxt(
    "/home/aras/aras-current/dji-asdk-to-python/examples/calibration/camera_distortion.txt",
    delimiter=",")

aircraft = Aircraft(APP_IP)
aruco_landing = ArucoLanding(aircraft=aircraft,
                             width=IMAGE_WIDTH,
                             height=IMAGE_HEIGHT,
                             camera_distortion=CAMERA_DISTORTION,
                             camera_matrix=CAMERA_MATRIX,
                             marker_id=MARKER_ID,
                             marker_size_cm=MARKER_SIZE_CM)
aruco_landing.start(is_night=False)
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
from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.flight_controller.flight_controller_state import (
    FlightControllerState, )

APP_IP = "192.168.0.110"

drone = Aircraft(APP_IP)
fc = drone.getFlightController()

flight_controller_state = fc.getState()

print("areMotorsOn %s " % flight_controller_state.areMotorsOn())
print("isFlying %s " % flight_controller_state.isFlying())
print("velocityX %s " % flight_controller_state.getVelocityX())
print("velocityY %s " % flight_controller_state.getVelocityY())
print("velocityZ %s " % flight_controller_state.getVelocityZ())

aircraft_location = flight_controller_state.getAircraftLocation()

print("getAltitude %s " % aircraft_location.getAltitude())
print("getLatitude %s " % aircraft_location.getLatitude())
print("getLongitude %s " % aircraft_location.getLongitude())

aircraft_attitude = flight_controller_state.getAttitude()

print("pitch %s " % aircraft_attitude.pitch)
print("roll %s " % aircraft_attitude.roll)
print("yaw %s " % aircraft_attitude.yaw)
print("GoHomeExecutionState %s" %
      flight_controller_state.getGoHomeExecutionState())
print("getFlightMode %s" % flight_controller_state.getFlightMode())
Exemplo n.º 6
0
from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.camera.display_mode import DisplayMode

APP_IP = "192.168.100.210"

drone = Aircraft(APP_IP)
camera = drone.getCamera()

#camera_mode = DisplayMode.VISUAL_ONLY
camera_mode = DisplayMode.THERMAL_ONLY

camera.setDisplayMode(camera_mode)

print(f"Changed camera mode to {camera_mode}")
Exemplo n.º 7
0
import time
from dji_asdk_to_python.products.aircraft import Aircraft

APP_IP = "192.168.0.109"

drone = Aircraft(APP_IP)
gimbal = drone.getGimbal()

for i in range(10):
    print("iteration %s" % i)
    if i % 2 == 0:
        gimbal.rotate(pitch=-90, roll=0, yaw=0)
    else:
        gimbal.rotate(pitch=0, roll=0, yaw=0)
    time.sleep(2)
Exemplo n.º 8
0
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()
Exemplo n.º 9
0
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)
Exemplo n.º 10
0
from dji_asdk_to_python.products.aircraft import Aircraft

APP_IP = "192.168.100.206"

aircraft = Aircraft(APP_IP)
fc = aircraft.getFlightController()
res = fc.setCollisionAvoidanceEnabled(True)
print(res)
Exemplo n.º 11
0
from dji_asdk_to_python.flight_controller.location_coordinate_2d import LocationCoordinate2D
from dji_asdk_to_python.products.aircraft import Aircraft
import time

app_ip = "192.168.100.203"

drone = Aircraft(app_ip)
fc = drone.getFlightController()

new_home_location = LocationCoordinate2D(3.3310794794873844,
                                         -76.53948434110453)

current_home_location = fc.getHomeLocation()
print("current home: lat %s, lng %s " %
      (current_home_location.latitude, current_home_location.longitude))
result = fc.setHomeLocation(new_home_location)
print("result", result)
# fc.startGoHome(startGoHomeCallback)
# fc.cancelGoHome(cancelGoHomeCallback)
Exemplo n.º 12
0
from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.battery.battery_state import BatteryState

APP_IP = "192.168.0.174"

drone = Aircraft(APP_IP)
battery = drone.getBattery()
battery_state = battery.getBatteryState()

print("battery state %s " % battery_state.getChargeRemainingInPercent())
Exemplo n.º 13
0
from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.flight_controller.flight_controller_state import (
    FlightControllerState, )

# Setting global variables

APP_IP = "192.168.20.37"
FREQ_SEND = 5  # hz

REQUEST_DELAY = 3  # sec
MIN_ELAPSED_TIME = 300  # seconds
IS_FLYING = False

# Initializing aircraft object to handle drone

aircraft = Aircraft(APP_IP)
print(" ############################## Starting Telemetry process in DJI")

time_0 = time.time()
flight_controller = aircraft.getFlightController()
start = time_0
elapsed = 0
flying = IS_FLYING
count = 0

# Loop to do actions while time be lesser than min allowed or drone is flying}
#i = 0
print("############################## Starting Telemetry loop before flying")
import datetime
while elapsed < MIN_ELAPSED_TIME or flying:
    #while i != 2:
Exemplo n.º 14
0
from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.flight_controller.flight_controller_state import FlightControllerState
from dji_asdk_to_python.flight_controller.flight_mode import FlightMode

APP_IP = "192.168.100.210"

if __name__ == '__main__':
    
    drone = Aircraft(APP_IP)
    ...
Exemplo n.º 15
0
from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.flight_controller.location_coordinate_2d import LocationCoordinate2D
from dji_asdk_to_python.mission_action.mission_action import MissionAction

import time
APP_IP = "192.168.0.174"

drone = Aircraft(APP_IP)
mission_action = drone.getMissionAction()

coordinate = LocationCoordinate2D(3.3310334728130204, -76.53937525628518)

print(mission_action)

print(mission_action.aircraftYawAction(-180, False))
#time.sleep(5)
print(mission_action.aircraftYawAction(180, False))
print(mission_action.goToAction(coordinate, 15))
Exemplo n.º 16
0
import time
from dji_asdk_to_python.mission_control.waypoint import Waypoint
from dji_asdk_to_python.mission_control.waypoint import WaypointMissionOperator
from dji_asdk_to_python.mission_control.waypoint import WaypointMission
from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.mission_control.waypoint import (
    WaypointMissionGoToWaypointMode,
    WaypointMissionFinishedAction,
    WaypointMissionFlighPathMode,
    WaypointMissionHeadingMode,
)
import json

APP_IP = "192.168.100.210"

drone = Aircraft(app_ip=APP_IP)

wp1 = Waypoint(3.3312591288067144, -76.53951935644871, 45)
wp2 = Waypoint(3.3311239061197466, -76.5389064716888, 45)
wp3 = Waypoint(3.3310315262535997, -76.53939865704304, 45)

wp2.setGimbalPitch(-90)
wp2.setSpeed(5)

wp3.setGimbalPitch(-90)
wp3.setSpeed(10)

waypoints = [wp1, wp2, wp3]

builder = WaypointMission.Builder()
Exemplo n.º 17
0
import time

from dji_asdk_to_python.products.aircraft import Aircraft

APP_IP = "192.168.100.203"

aircraft = Aircraft(APP_IP)
video_record_manager = aircraft.getVideoRecordManager()

video_record_manager.start_record(
    "/media/jetson-a0002/DATA/video_records/drone.mp4")
time.sleep(20)  # record for 20 seconds
video_record_manager.stop_record()