Пример #1
0
def test_2_fly_straight_turn_fly_back():
    drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
    drone.start_video_streaming()
    drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()

    drone(
        moveBy(dX=10, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
            state="hovering", _timeout=20)).wait()

    drone(
        moveBy(dX=0, dY=0, dZ=0, dPsi=math.pi) >> FlyingStateChanged(
            state="hovering", _timeout=20)).wait()

    drone(moveBy(dX=10, dY=0, dZ=0, dPsi=0))
    #     >> FlyingStateChanged(state="hovering", _timeout=20)
    # ).wait()

    drone(CancelMoveBy(_timeout=10))

    drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
    drone.stop_video_streaming()
    drone.disconnection()
Пример #2
0
def goToHome():
    print("Going to HOME")
    drone(
        moveBy(0, 0, -5, 0) >> FlyingStateChanged(state="hovering", _timeout=5)
    ).wait()
    drone(
        moveTo(latitude=homeLatitude,
               longitude=homeLongitude,
               altitude=homeAltitude,
               orientation_mode=0,
               heading=0) >> FlyingStateChanged(state="hovering", _timeout=5)
    ).wait()
    return
Пример #3
0
    def start(self):
        # Connect the the drone
        self.drone.connection()

        print("Takeoff if necessary...")
        self.drone(
            FlyingStateChanged(state="hovering", _policy="check")
            | FlyingStateChanged(state="flying", _policy="check")
            | (
                GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait")
                >> (
                    TakeOff(_no_expect=True)
                    & FlyingStateChanged(
                        state="hovering", _timeout=10, _policy="check_wait")
                )
            )
        ).wait()

        self.drone(moveBy(0,-1.15,-0.5,-3.142)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()

        self.drone(moveBy(2.0,0, 0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()

        # You can record the video stream from the drone if you plan to do some
        # post processing.
        self.drone.set_streaming_output_files(
            h264_data_file=os.path.join(self.tempd, 'h264_data.264'),
            h264_meta_file=os.path.join(self.tempd, 'h264_metadata.json'),
            # Here, we don't record the (huge) raw YUV video stream
            # raw_data_file=os.path.join(self.tempd,'raw_data.bin'),
            # raw_meta_file=os.path.join(self.tempd,'raw_metadata.json'),
        )

        # Setup your callback functions to do some live video processing
        self.drone.set_streaming_callbacks(
            raw_cb=self.yuv_frame_cb,
            h264_cb=self.h264_frame_cb
        )
        # Start video streaming
        self.drone.start_video_streaming()
Пример #4
0
def function_8():
    drone(
        moveBy(0, 0, -1, 0)
        >> FlyingStateChanged(state='hovering', _timeout=5)
        >> f.write("drone(")
        >> f.write("\n")
        >> f.write("    moveBy(0, 0, -1, 0)")
        >> f.write("\n")
        >> f.write("    >> FlyingStateChanged(state='hovering', _timeout=5)")
        >> f.write("\n")
        >> f.write(").wait()")
        >> f.write("\n")
    )
Пример #5
0
    def checkWP(self):
        #This is a 1D exaple for checking if the drone has made it to a current waypoint

        while True:

            # Drone Pose
            x_dr = self.curPose.x
            y_dr = self.curPose.y
            z_dr = self.curPose.z
            yaw_dr = self.curPose.yaw

            # Reference
            x_R = self.waypoints[self.curWPindex].x
            y_R = self.waypoints[self.curWPindex].y
            z_R = self.waypoints[self.curWPindex].z

            Thr_vec = np.array([0.1, 0.1, 0.05,
                                1 * pi / 180])  # Threshold Vector

            er_mag = math.pow((x_dr - x_R), 2) + math.pow(
                (y_dr - y_R), 2) + math.pow((z_dr - z_R, 2))

            if er_mag <= math.pow(self.r, 2):
                print(colored("MADE IT TO WP!", "green"))
                #send new drone control command
                self.curWPindex += 1
            else:

                gn_mat = [0.2, 0.2, 0.1, 0.1]

                xEr = x_R - x_dr
                yEr = y_R - y_dr
                zEr = z_R - z_dr

                th_req = New_Sign(yEr) * acos(xEr / er_mag)

                R_ER_I = np.array([xEr, yEr, zEr, th_req])

                mov_cm_UC = R3_Mat_Cords(yaw_dr, R_ER_I, gn_mat)

                x_c, y_c, z_c, yaw_c = Check_Move_Sat(mov_cm_UC, Thr_vec)

                drone(
                    moveBy(x_c, y_c, z_c, yaw_c) >> FlyingStateChanged(
                        state="hovering", _timeout=5)).wait()

                time.sleep(0.3)

                #check if end of trajectory
                if self.curWPindex == len(self.waypoints):
                    break
Пример #6
0
def test_3_fly_straight_2_seconds():
    drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
    drone.start_video_streaming()
    drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
    sleep(5)

    drone(
        moveBy(dX=5, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
            state="hovering", _timeout=2)).wait()

    sleep(5)
    drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
    drone.stop_video_streaming()
    drone.disconnection()
Пример #7
0
    def ManualMove(self,params):
        if self._joystick_ctrl :
            self.my_log.info("Mode mobile : commande {} ignorée ".format(params.to_string()))
            return;

        if isinstance(params, dict):
            params = DroneCommandParams(**params)
        if self._manual_unit :
            if not self._on_test:
                self._drone(moveBy(params.x,params.y,params.z,params.orientation))
            else :
                self.my_log.info("Mode simulation: informations reçues {} ".format(params.to_string()))
        else :
            self.my_log.info("Mode automatique : commande {} ignorée ".format(params.to_string()))
Пример #8
0
def test_10_reach_target():
    drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
    drone.start_video_streaming()
    change_gimbal_angle(-85)
    print("------take off-------")
    drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
    sleep(2)

    while detect() != 'blue':
        drone(
            moveBy(dX=1, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
                state="hovering", _timeout=20)).wait()

    drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
    drone.stop_video_streaming()
    drone.disconnection()
Пример #9
0
    def fly(self):
        # Takeoff, fly, land, ...
        print("Takeoff if necessary...")
        self.drone(
            FlyingStateChanged(state="hovering", _policy="check")
            | FlyingStateChanged(state="flying", _policy="check")
            | (
                GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait")
                >> (
                    TakeOff(_no_expect=True)
                    & FlyingStateChanged(
                        state="hovering", _timeout=10, _policy="check_wait")
                )
            )
        ).wait()

        self.drone(moveBy(0,-1.15,-0.5,-3.142)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
Пример #10
0
def function_2():
    drone(

        moveBy(1, 0, 0, 0)
        >> FlyingStateChanged(state='hovering', _timeout=5, _policy='check_wait', _float_tol=(1e-07, 1e-09))
        >> f.write("drone(")
        >> f.write("\n")
        >> f.write("    moveBy(1, 0, 0, 0)")
        >> f.write("\n")
        >> f.write("    >> FlyingStateChanged(state='hovering', _timeout=5)")
        >> f.write("\n")
        >> f.write(").wait()")
        >> f.write("\n")



    )
Пример #11
0
    def fly(self):
        # Takeoff, fly, land, ...
        print("Takeoff")
        self.drone(
            TakeOff()
            & FlyingStateChanged(
                state="hovering", _timeout=10, _policy="check_wait")).wait()
        #for i in range(2):
        #    print("Moving by ({}/2)...".format(i + 1))
        self.drone(moveBy(1, 0, -0.5, 0, _timeout=20)).wait().success()

        print("Landing...")
        self.drone(
            Orientation(pan=0, tilt=-82)
            & StateOrientation(pan=0, tilt=-82)).wait().success()
        self.land()
        print("Landed\n")
Пример #12
0
    def start(self):
        # Connect the the drone
        pygame.init()
        W, H = 320, 240
        fps=40
        vx=0;
        vy=0;
        vz=0.0;
        vr=0.0;
        screen = pygame.display.set_mode((W, H))
        self.drone.connection()
        #clock = pygame.time.Clock()
        running=True
        while running:
            self.drone.start_piloting()
            for event in pygame.event.get():
                if event.type==pygame.QUIT:
                    running=False
                elif event.type==pygame.KEYUP:
                        self.drone.piloting_pcmd(0, 0, 0, 0, 0.0)
                        time.sleep(1)
                elif event.type==pygame.KEYDOWN:
                    if event.key==pygame.K_ESCAPE:
                        self.drone.piloting_pcmd(0, 0, 0, 0, 0.0) # (roll, pitch, yaw, gaz, piloting_time)
                        time.sleep(1)
                        running=False
                    elif event.key==pygame.K_p:
                        self.drone(
    TakeOff()
    >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
                    elif event.key==pygame.K_SPACE:
                        self.drone(
    Landing()
    >> FlyingStateChanged(state="landing", _timeout=5)
).wait()

                    elif event.key==pygame.K_w:
                        self.drone(moveBy(1.0,0,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
                    elif event.key==pygame.K_s:
                        self.drone(moveBy(-1.0,0,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
                    elif event.key==pygame.K_a:
                        self.drone(moveBy(0,-1,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
                    elif event.key==pygame.K_d:
                        self.drone(moveBy(0,1,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
                    elif event.key==pygame.K_r:
                        self.drone(moveBy(0,0,-0.5,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
                    elif event.key==pygame.K_f:
                        self.drone(moveBy(0,0,0.5,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()

            #self.drone.piloting_pcmd(vy, vx, 0, 0, 0.5) # (roll, pitch, yaw, gaz, piloting_time)
            time.sleep(1)
            pygame.display.flip()
Пример #13
0
def take_photo_burst_Move(drone, th_mov, th_R0, dxm):
    # take a photo burst and get the associated media_id

    xc, yc, zc =  R3_Mat_Cords(th_R0, dxm)
    photo_saved = drone(photo_progress(result="photo_saved", _policy="wait"))
    drone(take_photo(cam_id=0) & moveBy(xc, yc, zc, th_mov)).wait()
    photo_saved.wait()
    media_id = photo_saved.received_events().last().args["media_id"]

    # download the photos associated with this media id
    media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
    media_info_response.raise_for_status()
    os.chdir("/home/rnallapu/code/Results")
    download_dir = tempfile.mkdtemp()
    Res_dir = filecreation()

    #tempfile.gettempdir()
    for resource in media_info_response.json()["resources"]:
        image_response = requests.get(ANAFI_URL + resource["url"], stream=True)
        download_path = os.path.join(download_dir, resource["resource_id"])

        print('Hello')

        print(download_path)
        image_response.raise_for_status()

        with open(download_path, "wb") as image_file:
            shutil.copyfileobj(image_response.raw, image_file)

        # parse the xmp metadata
        with open(download_path, "rb") as image_file:
            image_data = image_file.read()
            image_xmp_start = image_data.find(b"<x:xmpmeta")
            image_xmp_end = image_data.find(b"</x:xmpmeta")
            image_xmp = ET.fromstring(image_data[image_xmp_start : image_xmp_end + 12])
            for image_meta in image_xmp[0][0]:
                xmp_tag = re.sub(r"{[^}]*}", "", image_meta.tag)
                xmp_value = image_meta.text
                # only print the XMP tags we are interested in
                if xmp_tag in XMP_TAGS_OF_INTEREST:
                    print(resource["resource_id"], xmp_tag, xmp_value)

    #
        #shutil.copy2(download_path, "/home/rnallapu/code/Photos/")
        shutil.copy2(download_path, Res_dir)
Пример #14
0
def take_Map_Video_Move(drone, th_mov, th_R0, dxm):
    # take a photo burst and get the associated media_id

    xc, yc, zc =  R3_Mat_Cords(th_R0, dxm)
    photo_saved = drone(recording_progress(result="stopped", _policy="wait"))
    # drone(
    #     MaxRotationSpeed(1)
    #     >> MaxRotationSpeedChanged(current=1, _policy='wait')
    # ).wait()
    #drone(setAutonomousFlightMaxRotationSpeed(1e-7)).wait()
    drone(start_recording(cam_id=0) & moveBy(xc, yc, zc, th_mov)).wait()
    drone(stop_recording(cam_id=0)).wait()

    print('Action Completed')
    photo_saved.wait()
    media_id = photo_saved.received_events().last().args["media_id"]
    print(media_id)

    # download the photos associated with this media id
    os.chdir("/home/rnallapu/code/Results")
    media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
    media_info_response.raise_for_status()
    

    download_dir = tempfile.mkdtemp()
    Res_dir = filecreation()
    

    #tempfile.gettempdir()
    for resource in media_info_response.json()["resources"]:
        image_response = requests.get(ANAFI_URL + resource["url"], stream=True)
        download_path = os.path.join(download_dir, resource["resource_id"])

        print('Hello')
        print(download_path)
        image_response.raise_for_status()

        with open(download_path, "wb") as image_file:
            shutil.copyfileobj(image_response.raw, image_file)

        #shutil.copy2(download_path, "/home/rnallapu/code/Photos/")
        shutil.copy2(download_path, Res_dir)
Пример #15
0
def test_asyncaction():
    with olympe.Drone(DRONE_IP) as drone:
        drone.connect()

        # Start a flying action asynchronously
        flyingAction = drone(
            TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5) >>
            moveBy(10, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                      _timeout=5) >> Landing())

        # Start video recording while the drone is flying
        if not drone(start_recording(cam_id=0)).wait().success():
            assert False, "Cannot start video recording"

        # Send a gimbal pitch velocity target while the drone is flying
        cameraAction = drone(
            gimbal.set_target(
                gimbal_id=0,
                control_mode="velocity",
                yaw_frame_of_reference="none",
                yaw=0.0,
                pitch_frame_of_reference="none",
                pitch=0.1,
                roll_frame_of_reference="none",
                roll=0.0,
            )).wait()

        if not cameraAction.success():
            assert False, "Cannot set gimbal velocity target"

        # Wait for the end of the flying action
        if not flyingAction.wait().success():
            assert False, "Cannot complete the flying action"

        # Stop video recording while the drone is flying
        if not drone(stop_recording(cam_id=0)).wait().success():
            assert False, "Cannot stop video recording"

        # Leaving the with statement scope: implicit drone.disconnect() but that
        # is still a good idea to perform the drone disconnection explicitly
        drone.disconnect()
Пример #16
0
def test_9_calculate_drone_speed():
    drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
    # drone.start_video_streaming()
    drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()

    counter = 3
    while counter < 6:
        start_time = time.time()
        distance = 1 * counter

        drone(
            moveBy(dX=distance, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
                state="hovering", _timeout=5)).wait()

        speed = distance / (time.time() - start_time)
        print("speed [m/s]: ", speed)
        counter += 1

    drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
    drone.stop_video_streaming()
    drone.disconnection()
Пример #17
0
def test_7_detect_and_move():
    drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
    # drone.start_video_streaming()
    drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()

    change_gimbal_angle(-80)
    sleep(1)

    start_time = time.time()
    while time.time() - start_time < 500:
        tc = detect.detect()
        if tc > 0:
            drone(
                moveBy(dX=2, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
                    state="hovering", _timeout=5)).wait()
            break

    reset_gimbal_orientation()
    drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
    drone.stop_video_streaming()
    drone.disconnection()
Пример #18
0
    def launch(self, hover_altitude):
        self.bebop(
            FlyingStateChanged(state='hovering', _policy='check')
            | FlyingStateChanged(state='flying', _policy='check') | (
                # GPSFixStateChanged(fixed=1, _timeout=10, _policy='check_wait')
                # >> (
                (TakeOff(_no_expect=True)
                 & FlyingStateChanged(
                     state='hovering', _timeout=10, _policy='check_wait')))
        ).wait()

        self.drone_location = self.bebop.get_state(GpsLocationChanged)

        if hover_altitude > 0:
            self.bebop(
                moveBy(0, 0, -hover_altitude, 0)  # Move drone up
                >> PCMD(1, 0, 0, 0, 0,
                        0)  # Force drone to go into hovering state
                >> FlyingStateChanged(state='hovering',
                                      _timeout=5)  # Set state to hovering
            ).wait().success()
Пример #19
0
    def fly(self):
        # Takeoff, fly, land, ...
        print("Takeoff if necessary...")
        self.drone(
            FlyingStateChanged(state="hovering", _policy="check")
            | FlyingStateChanged(state="flying", _policy="check")
            |
            (GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait") >>
             (TakeOff(_no_expect=True)
              & FlyingStateChanged(
                  state="hovering", _timeout=10, _policy="check_wait")))).wait(
                  )
        self.drone(MaxTilt(40)).wait().success()
        for i in range(3):
            print("Moving by ({}/3)...".format(i + 1))
            self.drone(moveBy(10, 0, 0, math.pi, _timeout=20)).wait().success()

        print("Landing...")
        self.drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)
                   ).wait()
        print("Landed\n")
Пример #20
0
def goToEntrance():
    print("Going to entrance")
    ##Go up
    drone(
        moveBy(0, 0, -5, 0) >> FlyingStateChanged(state="hovering", _timeout=5)
    ).wait()
    ##Go to entrance
    drone(
        moveTo(latitude=entranceLatitude,
               longitude=entranceLongitude,
               altitude=5,
               orientation_mode=0,
               heading=0) >> FlyingStateChanged(state="hovering", _timeout=5)
    ).wait()
    drone(
        moveTo(latitude=entranceLatitude,
               longitude=entranceLongitude,
               altitude=entranceAltitude,
               orientation_mode=0,
               heading=0) >> FlyingStateChanged(state="hovering", _timeout=5)
    ).wait()
    return
Пример #21
0
def test_subscriber():
    drone = olympe.Drone(DRONE_IP, media_autoconnect=False)
    # subscribe to all events during the drone connection
    with drone.subscribe(lambda event, controller: print(f"{event}")):
        drone.connect()

    # Subscribe to FlyingStateChanged
    # If you call `drone.subscribe` without using the `with` statement,
    # you'll have to call `drone.unsubscribe()` later.
    flying_sub = drone.subscribe(
        lambda event, controller: print("Flyingstate =", event.args["state"]),
        FlyingStateChanged(_policy="check"),
    )
    assert (drone(
        FlyingStateChanged(state="hovering")
        | (TakeOff()
           & FlyingStateChanged(state="hovering"))).wait(5).success())
    assert drone(moveBy(10, 0, 0, 0)).wait().success()
    drone(Landing()).wait()
    assert drone(FlyingStateChanged(state="landed")).wait().success()
    # unsubscribe from FlyingStateChanged
    drone.unsubscribe(flying_sub)
    drone.disconnect()
Пример #22
0
  def reset(self):
    # Resets the drone back to start of simulation after completion of episode
    self.pos_feedback() # Update state of the drone in self.agent_pos

    # Random initialization(reset) for every episode
    x_r = random.randrange(-5,5,1)
    y_r = random.randrange(-5,5,1)
    z_r = random.randrange(1,5,1)

    # Random goal setting for every episode
    x_d = random.randrange(-5,5,1)
    y_d = random.randrange(-5,5,1)
    z_d = random.randrange(1,5,1)

    self.destination = [x_d,y_d,z_d]

    print('------------RESET-------------',[x_r,y_r,z_r],'------------RESET-------------')
    print('------------GOAL-------------',self.destination,'------------GOAL-------------')

    # Move the drone to random initialization coordinate
    self.drone(moveBy(x_r-self.agent_pos[0], self.agent_pos[1]-y_r, self.agent_pos[2]-z_r, 0)>> FlyingStateChanged(state="hovering", _timeout=5)).wait()
    self.pos_feedback() # Update state of the drone in self.agent_pos
    obs = [self.agent_pos[0]-self.destination[0],self.agent_pos[1]-self.destination[1], self.agent_pos[2]-self.destination[2]]
    return np.array(obs).astype(np.float32)  # reward, done, info can't be included
Пример #23
0
def test_listener():
    drone = olympe.Drone(DRONE_IP)
    # Explicit subscription to every event
    every_event_listener = EveryEventListener(drone)
    every_event_listener.subscribe()
    drone.connect()
    every_event_listener.unsubscribe()

    # You can also subscribe/unsubscribe automatically using a with statement
    with FlightListener(drone) as flight_listener:
        for i in range(2):
            get_state = drone(GetState(camera_id=0)).wait()
            assert get_state.success()
            assert drone(
                FlyingStateChanged(state="hovering")
                | (TakeOff() & FlyingStateChanged(state="hovering"))
            ).wait(5).success()
            assert drone(moveBy(10, 0, 0, 0)).wait().success()
            drone(Landing()).wait()
            assert drone(FlyingStateChanged(state="landed")).wait().success()

    print(f"Takeoff count = {flight_listener.takeoff_count}")
    assert flight_listener.takeoff_count > 0
    drone.disconnect()
    def yuv_frame_cb(self, yuv_frame):
        info = yuv_frame.info()
        height, width = info["yuv"]["height"], info["yuv"]["width"]
        cv2_cvt_color_flag = {
            olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
            olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
        }[info["yuv"]["format"]]
        img = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)
        #lg = np.array([40, 62, 0])
        #ug = np.array([96, 255, 255])
        global lg
        global ug
        blurred = cv2.GaussianBlur(img, (15, 15), 0)
        hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, lg, ug)
        mask = cv2.erode(mask, None, iterations=1)  #remove noise
        mask = cv2.dilate(mask, None, iterations=1)  #remove noise
        mask = cv2.GaussianBlur(mask, (15, 15), 2, 2)  #window size 15x15
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                                               cv2.CHAIN_APPROX_SIMPLE)

        vx = 0.0
        vy = 0.0
        vz = 0.0
        vr = 0.0
        marker_y = 0.0
        marker_x = 0.0
        kp = 0.001
        #movement speed (control gain): higher value higher speed
        global largest
        #define it one time, (allow user to modify the variable in current scope)

        for cnt in contours:
            lg = np.array([40, 62, 0])
            ug = np.array([96, 255, 255])
            approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True),
                                      True)
            print(len(approx))
            if len(approx) == 4:
                print("square")
                cv2.drawContours(img, [cnt], 0, (0, 0, 255), -1)
            elif len(approx) > 14:
                cv2.drawContours(img, [cnt], 0, (0, 255, 255), -1)
                area = cv2.contourArea(cnt)
                print("Area Circle = %.2f" % area)
                if largest is None or cv2.contourArea(cnt) > cv2.contourArea(
                        largest):
                    if cv2.contourArea(cnt) < 1000000 and cv2.contourArea(
                            cnt) > 5000:
                        largest = cnt
                        M = cv2.moments(largest)
                        marker_y = int(M["m01"] / M["m00"])
                        marker_x = int(M["m10"] / M["m00"])
                        vx = 0.5
                        vy = 0.0
                        vz = kp * (img.shape[0] / 2 - marker_y)  #horizontal
                        vr = kp * (img.shape[1] / 2 - marker_x)  #vertical
                        cv2.circle(img, (marker_x, marker_y), 2, (0, 0, 255),
                                   -1)

                self.drone(moveBy(vx, vy, -vz, -vr)).wait(10)

            else:
                circles = cv2.HoughCircles(gray,
                                           cv2.HOUGH_GRADIENT,
                                           5,
                                           500,
                                           param1=80,
                                           param2=100,
                                           minRadius=50,
                                           maxRadius=150)
                if circles is not None:
                    print("Circle There !")
                    circles = np.uint16(np.around(circles))
                    for i in circles[0, :]:
                        # draw the outer circle
                        cv2.circle(img, (i[0], i[1]), i[2], (0, 255, 0), 2)
                        # draw the center of the circle
                        cv2.circle(img, (i[0], i[1]), 2, (0, 0, 255), 3)

        #for len(approx) != 4 and len(approx) < 14:
        #cirles=cv2.HoughCircles(mask, cv2.HOUGH_GRADIENT, 1, 10)
        #if cirles!=None:
        #print ("Circle There !")

        #cv2.drawContours(img,[cnt],0,(255,0,0),-1)
        #area = cv2.contourArea(cnt)
        #print (area)
        #vx=0
        #vy=3
        #vz=0
        #vr=-3.142

        cv2.circle(img, (int(img.shape[1] / 2), int(img.shape[0] / 2)), 2,
                   (0, 0, 255), -1)

        cv2.imshow('streaming', img)

        cv2.waitKey(1)
 def apriltag(self):
     self.drone(
         moveBy(0, 0, 0.5, 0) >> FlyingStateChanged(state="hovering",
                                                    _timeout=10)).wait()
Пример #26
0
# -*- coding: UTF-8 -*-

import olympe
from olympe.messages.ardrone3.Piloting import TakeOff, moveBy, Landing

drone = olympe.Drone("10.202.0.1")
drone.connect()
drone(TakeOff()).wait()
drone(moveBy(10, 0, 0, 0)).wait()
drone(Landing()).wait()
drone.disconnect()
Пример #27
0
    # is optional for the default method, but you can use it to further
    # restrict the event messages handled by this method or to limit the
    # maximum size of it associated event queue (remember that the oldest
    # events are dropped silently when the event queue is full).
    @olympe.listen_event(queue_size=10)
    def default(self, event, scheduler):
        print_event(event)


if __name__ == "__main__":
    drone = olympe.Drone(DRONE_IP)
    # Explicit subscription to every event
    every_event_listener = EveryEventListener(drone)
    every_event_listener.subscribe()
    drone.connect()
    every_event_listener.unsubscribe()

    # You can also subscribe/unsubscribe automatically using a with statement
    with FlightListener(drone) as flight_listener:
        for i in range(2):
            assert drone(
                FlyingStateChanged(state="hovering")
                | (TakeOff() & FlyingStateChanged(state="hovering"))
            ).wait().success()
            assert drone(moveBy(10, 0, 0, 0)).wait().success()
            drone(Landing()).wait()
            assert drone(FlyingStateChanged(state="landed")).wait().success()

    assert flight_listener.has_observed_takeoff
    drone.disconnect()
Пример #28
0
def goToDestination(destination):

    if (destination == "A1"):
        goToEntrance()
        drone(
            moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                        _timeout=5)).wait()
        goToHome()
    elif (destination == "A2"):
        goToEntrance()
        drone(
            moveBy(0, 2.3, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                        _timeout=5)).wait()
        goToHome()
    elif (destination == "A3"):
        goToEntrance()
        drone(
            moveBy(0, 4.8, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                        _timeout=5)).wait()
        goToHome()
    elif (destination == "A4"):
        goToEntrance()
        drone(
            moveBy(0, 7.1, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                        _timeout=5)).wait()
        goToHome()
    elif (destination == "A5"):
        goToEntrance()
        drone(
            moveBy(0, 9.5, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                        _timeout=5)).wait()
        goToHome()
    elif (destination == "A6"):
        goToEntrance()
        drone(
            moveBy(0, 12, 0, 0) >> FlyingStateChanged(state="hovering",
                                                      _timeout=5)).wait()
        drone(
            moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                        _timeout=5)).wait()
        goToHome()
    elif (destination == "B1"):
        goToEntrance()
        drone(
            moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                         _timeout=5)).wait()
        goToHome()
    elif (destination == "B2"):
        goToEntrance()
        drone(
            moveBy(0, 2.3, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                         _timeout=5)).wait()
        goToHome()
    elif (destination == "B3"):
        goToEntrance()
        drone(
            moveBy(0, 4.8, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                         _timeout=5)).wait()
        goToHome()
    elif (destination == "B4"):
        goToEntrance()
        drone(
            moveBy(0, 7.1, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                         _timeout=5)).wait()
        goToHome()
    elif (destination == "B5"):
        goToEntrance()
        drone(
            moveBy(0, 9.5, 0, 0) >> FlyingStateChanged(state="hovering",
                                                       _timeout=5)).wait()
        drone(
            moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                         _timeout=5)).wait()
        goToHome()
    elif (destination == "B6"):
        goToEntrance()
        drone(
            moveBy(0, 12, 0, 0) >> FlyingStateChanged(state="hovering",
                                                      _timeout=5)).wait()
        drone(
            moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
                                                         _timeout=5)).wait()
        goToHome()

    return
Пример #29
0
# Take-off
drone(
    FlyingStateChanged(state="hovering", _policy="check")
    | FlyingStateChanged(state="flying", _policy="check")
    | (GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait") >>
       (TakeOff(_no_expect=True)
        & FlyingStateChanged(
            state="hovering", _timeout=10, _policy="check_wait")))).wait()

# Get the home position
drone_location = drone.get_state(GpsLocationChanged)

# Move 10m
drone(
    moveBy(10, 0, 0, math.pi) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged(
        state="hovering", _timeout=5)).wait().success()

# Go back home
drone(
    moveTo(drone_location["latitude"], drone_location["longitude"],
           drone_location["altitude"], MoveTo_Orientation_mode.TO_TARGET, 0.0)
    >> FlyingStateChanged(state="hovering", _timeout=5) >> moveToChanged(
        latitude=drone_location["latitude"],
        longitude=drone_location["longitude"],
        altitude=drone_location["altitude"],
        orientation_mode=MoveTo_Orientation_mode.TO_TARGET,
        status='DONE',
        _policy='wait') >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
Пример #30
0
    def yuv_frame_cb(self, yuv_frame):
        """
        This function will be called by Olympe for each decoded YUV frame.

            :type yuv_frame: olympe.VideoFrame
        """
        # the VideoFrame.info() dictionary contains some useful informations
        # such as the video resolution
        info = yuv_frame.info()
        height, width = info["yuv"]["height"], info["yuv"]["width"]

        # convert pdraw YUV flag to OpenCV YUV flag
        cv2_cvt_color_flag = {
            olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
            olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
        }[info["yuv"]["format"]]

        # yuv_frame.as_ndarray() is a 2D numpy array with the proper "shape"
        # i.e (3 * height / 2, width) because it's a YUV I420 or NV12 frame

        # Use OpenCV to convert the yuv frame to RGB
        img = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)

        blurred = cv2.GaussianBlur(img, (11, 11), 0)
        hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

        # Threshold the HSV image to get only certain colors
        mask = cv2.inRange(hsv, greenLower, greenUpper)
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)

        mask = cv2.GaussianBlur(mask, (15, 15), 2, 2)

        contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                                               cv2.CHAIN_APPROX_SIMPLE)

        #shape = cv2.drawContours(img, contours, -1, (0,255,0), 3)

        vx = 0.0
        vy = 0.0
        vz = 0.0
        vr = 0.0
        marker_y = 0.0
        marker_x = 0.0
        kp = 0.001
        global largest

        for cnt in contours:
            approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True),
                                      True)
            #print (len(approx))
            if len(approx) == 4:
                print("square")
                cv2.drawContours(img, [cnt], 0, (0, 0, 255), -1)
            elif len(approx) > 13:
                print("circle")
                cv2.drawContours(img, [cnt], 0, (0, 255, 255), -1)
                if largest is None or cv2.contourArea(cnt) > cv2.contourArea(
                        largest):
                    if cv2.contourArea(cnt) < 1000000:
                        largest = cnt

        if largest is not None:
            cv2.drawContours(img, largest, -1, (0, 255, 0), 3)
            M = cv2.moments(largest)
            marker_y = int(M["m01"] / M["m00"])
            marker_x = int(M["m10"] / M["m00"])
            vx = 0.1
            vy = 0.0
            vz = kp * (img.shape[0] / 2 - marker_y)
            vr = kp * (img.shape[1] / 2 - marker_x)
            cv2.circle(img, (marker_x, marker_y), 2, (0, 0, 255), -1)

        cv2.circle(img, (int(img.shape[1] / 2), int(img.shape[0] / 2)), 2,
                   (0, 0, 255), -1)

        cv2.imshow('img', img)

        self.drone(moveBy(vx, vy, -vz, -vr)).wait(10)

        cv2.waitKey(1)