diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6194533 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*.pyc +*~ +.cache \ No newline at end of file diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..7375457 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,11 @@ +language: python +python: + - "2.7" +notifications: + - email: false +install: + - sudo apt-get update + - sudo apt-get install ffmpeg + - pip install -r requirements.txt + - pip install -r dev-requirements.txt +script: py.test -n 4 diff --git a/README b/README index 7d3a760..bf53b21 100644 --- a/README +++ b/README @@ -43,9 +43,7 @@ Requirements: This software was tested with the following setup: * Python 2.6.6 - * Psyco 1.6 (recommended) - * Pygame 1.8.1 (only for the demo) - * Unmodified AR.Drone firmware 1.5.1 + * Unmodified AR.Drone firmware 2.0 License: diff --git a/arnetwork.py b/arnetwork.py deleted file mode 100644 index bab11cc..0000000 --- a/arnetwork.py +++ /dev/null @@ -1,119 +0,0 @@ -# Copyright (c) 2011 Bastian Venthur -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - - -""" -This module provides access to the data provided by the AR.Drone. -""" - -import select -import socket -import threading -import multiprocessing - -import libardrone -import arvideo - - -class ARDroneNetworkProcess(multiprocessing.Process): - """ARDrone Network Process. - - This process collects data from the video and navdata port, converts the - data and sends it to the IPCThread. - """ - - def __init__(self, nav_pipe, video_pipe, com_pipe): - multiprocessing.Process.__init__(self) - self.nav_pipe = nav_pipe - self.video_pipe = video_pipe - self.com_pipe = com_pipe - - def run(self): - video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - video_socket.setblocking(0) - video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT)) - video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT)) - - nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - nav_socket.setblocking(0) - nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT)) - nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT)) - - stopping = False - while not stopping: - inputready, outputready, exceptready = select.select([nav_socket, video_socket, self.com_pipe], [], []) - for i in inputready: - if i == video_socket: - while 1: - try: - data = video_socket.recv(65535) - except IOError: - # we consumed every packet from the socket and - # continue with the last one - break - w, h, image, t = arvideo.read_picture(data) - self.video_pipe.send(image) - elif i == nav_socket: - while 1: - try: - data = nav_socket.recv(65535) - except IOError: - # we consumed every packet from the socket and - # continue with the last one - break - navdata = libardrone.decode_navdata(data) - self.nav_pipe.send(navdata) - elif i == self.com_pipe: - _ = self.com_pipe.recv() - stopping = True - break - video_socket.close() - nav_socket.close() - - -class IPCThread(threading.Thread): - """Inter Process Communication Thread. - - This thread collects the data from the ARDroneNetworkProcess and forwards - it to the ARDreone. - """ - - def __init__(self, drone): - threading.Thread.__init__(self) - self.drone = drone - self.stopping = False - - def run(self): - while not self.stopping: - inputready, outputready, exceptready = select.select([self.drone.video_pipe, self.drone.nav_pipe], [], [], 1) - for i in inputready: - if i == self.drone.video_pipe: - while self.drone.video_pipe.poll(): - image = self.drone.video_pipe.recv() - self.drone.image = image - elif i == self.drone.nav_pipe: - while self.drone.nav_pipe.poll(): - navdata = self.drone.nav_pipe.recv() - self.drone.navdata = navdata - - def stop(self): - """Stop the IPCThread activity.""" - self.stopping = True - diff --git a/dev-requirements.txt b/dev-requirements.txt new file mode 100644 index 0000000..a79efde --- /dev/null +++ b/dev-requirements.txt @@ -0,0 +1,2 @@ +pytest==2.3.5 +pytest-xdist==1.8 diff --git a/libardrone.py b/libardrone.py deleted file mode 100644 index 3f90c2a..0000000 --- a/libardrone.py +++ /dev/null @@ -1,443 +0,0 @@ -# Copyright (c) 2011 Bastian Venthur -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - - -""" -Python library for the AR.Drone. - -This module was tested with Python 2.6.6 and AR.Drone vanilla firmware 1.5.1. -""" - - -import socket -import struct -import sys -import threading -import multiprocessing - -import arnetwork - - -__author__ = "Bastian Venthur" - - -ARDRONE_NAVDATA_PORT = 5554 -ARDRONE_VIDEO_PORT = 5555 -ARDRONE_COMMAND_PORT = 5556 - - -class ARDrone(object): - """ARDrone Class. - - Instanciate this class to control your drone and receive decoded video and - navdata. - """ - - def __init__(self): - self.seq_nr = 1 - self.timer_t = 0.2 - self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) - self.lock = threading.Lock() - self.speed = 0.2 - self.at(at_config, "general:navdata_demo", "TRUE") - self.video_pipe, video_pipe_other = multiprocessing.Pipe() - self.nav_pipe, nav_pipe_other = multiprocessing.Pipe() - self.com_pipe, com_pipe_other = multiprocessing.Pipe() - self.network_process = arnetwork.ARDroneNetworkProcess(nav_pipe_other, video_pipe_other, com_pipe_other) - self.network_process.start() - self.ipc_thread = arnetwork.IPCThread(self) - self.ipc_thread.start() - self.image = "" - self.navdata = dict() - self.time = 0 - - def takeoff(self): - """Make the drone takeoff.""" - self.at(at_ftrim) - self.at(at_config, "control:altitude_max", "20000") - self.at(at_ref, True) - - def land(self): - """Make the drone land.""" - self.at(at_ref, False) - - def hover(self): - """Make the drone hover.""" - self.at(at_pcmd, False, 0, 0, 0, 0) - - def move_left(self): - """Make the drone move left.""" - self.at(at_pcmd, True, -self.speed, 0, 0, 0) - - def move_right(self): - """Make the drone move right.""" - self.at(at_pcmd, True, self.speed, 0, 0, 0) - - def move_up(self): - """Make the drone rise upwards.""" - self.at(at_pcmd, True, 0, 0, self.speed, 0) - - def move_down(self): - """Make the drone decent downwards.""" - self.at(at_pcmd, True, 0, 0, -self.speed, 0) - - def move_forward(self): - """Make the drone move forward.""" - self.at(at_pcmd, True, 0, -self.speed, 0, 0) - - def move_backward(self): - """Make the drone move backwards.""" - self.at(at_pcmd, True, 0, self.speed, 0, 0) - - def turn_left(self): - """Make the drone rotate left.""" - self.at(at_pcmd, True, 0, 0, 0, -self.speed) - - def turn_right(self): - """Make the drone rotate right.""" - self.at(at_pcmd, True, 0, 0, 0, self.speed) - - def reset(self): - """Toggle the drone's emergency state.""" - self.at(at_ref, False, True) - self.at(at_ref, False, False) - - def trim(self): - """Flat trim the drone.""" - self.at(at_ftrim) - - def set_speed(self, speed): - """Set the drone's speed. - - Valid values are floats from [0..1] - """ - self.speed = speed - - def at(self, cmd, *args, **kwargs): - """Wrapper for the low level at commands. - - This method takes care that the sequence number is increased after each - at command and the watchdog timer is started to make sure the drone - receives a command at least every second. - """ - self.lock.acquire() - self.com_watchdog_timer.cancel() - cmd(self.seq_nr, *args, **kwargs) - self.seq_nr += 1 - self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) - self.com_watchdog_timer.start() - self.lock.release() - - def commwdg(self): - """Communication watchdog signal. - - This needs to be send regulary to keep the communication w/ the drone - alive. - """ - self.at(at_comwdg) - - def halt(self): - """Shutdown the drone. - - This method does not land or halt the actual drone, but the - communication with the drone. You should call it at the end of your - application to close all sockets, pipes, processes and threads related - with this object. - """ - self.lock.acquire() - self.com_watchdog_timer.cancel() - self.com_pipe.send('die!') - self.network_process.terminate() - self.network_process.join() - self.ipc_thread.stop() - self.ipc_thread.join() - self.lock.release() - - -############################################################################### -### Low level AT Commands -############################################################################### - -def at_ref(seq, takeoff, emergency=False): - """ - Basic behaviour of the drone: take-off/landing, emergency stop/reset) - - Parameters: - seq -- sequence number - takeoff -- True: Takeoff / False: Land - emergency -- True: Turn of the engines - """ - p = 0b10001010101000000000000000000 - if takeoff: - p += 0b1000000000 - if emergency: - p += 0b0100000000 - at("REF", seq, [p]) - -def at_pcmd(seq, progressive, lr, fb, vv, va): - """ - Makes the drone move (translate/rotate). - - Parameters: - seq -- sequence number - progressive -- True: enable progressive commands, False: disable (i.e. - enable hovering mode) - lr -- left-right tilt: float [-1..1] negative: left, positive: right - rb -- front-back tilt: float [-1..1] negative: forwards, positive: - backwards - vv -- vertical speed: float [-1..1] negative: go down, positive: rise - va -- angular speed: float [-1..1] negative: spin left, positive: spin - right - - The above float values are a percentage of the maximum speed. - """ - p = 1 if progressive else 0 - at("PCMD", seq, [p, float(lr), float(fb), float(vv), float(va)]) - -def at_ftrim(seq): - """ - Tell the drone it's lying horizontally. - - Parameters: - seq -- sequence number - """ - at("FTRIM", seq, []) - -def at_zap(seq, stream): - """ - Selects which video stream to send on the video UDP port. - - Parameters: - seq -- sequence number - stream -- Integer: video stream to broadcast - """ - # FIXME: improve parameters to select the modes directly - at("ZAP", seq, [stream]) - -def at_config(seq, option, value): - """Set configuration parameters of the drone.""" - at("CONFIG", seq, [str(option), str(value)]) - -def at_comwdg(seq): - """ - Reset communication watchdog. - """ - # FIXME: no sequence number - at("COMWDG", seq, []) - -def at_aflight(seq, flag): - """ - Makes the drone fly autonomously. - - Parameters: - seq -- sequence number - flag -- Integer: 1: start flight, 0: stop flight - """ - at("AFLIGHT", seq, [flag]) - -def at_pwm(seq, m1, m2, m3, m4): - """ - Sends control values directly to the engines, overriding control loops. - - Parameters: - seq -- sequence number - m1 -- front left command - m2 -- fright right command - m3 -- back right command - m4 -- back left command - """ - # FIXME: what type do mx have? - pass - -def at_led(seq, anim, f, d): - """ - Control the drones LED. - - Parameters: - seq -- sequence number - anim -- Integer: animation to play - f -- ?: frequence in HZ of the animation - d -- Integer: total duration in seconds of the animation - """ - pass - -def at_anim(seq, anim, d): - """ - Makes the drone execute a predefined movement (animation). - - Parameters: - seq -- sequcence number - anim -- Integer: animation to play - d -- Integer: total duration in sections of the animation - """ - at("ANIM", seq, [anim, d]) - -def at(command, seq, params): - """ - Parameters: - command -- the command - seq -- the sequence number - params -- a list of elements which can be either int, float or string - """ - param_str = '' - for p in params: - if type(p) == int: - param_str += ",%d" % p - elif type(p) == float: - param_str += ",%d" % f2i(p) - elif type(p) == str: - param_str += ',"'+p+'"' - msg = "AT*%s=%i%s\r" % (command, seq, param_str) - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - sock.sendto(msg, ("192.168.1.1", ARDRONE_COMMAND_PORT)) - -def f2i(f): - """Interpret IEEE-754 floating-point value as signed integer. - - Arguments: - f -- floating point value - """ - return struct.unpack('i', struct.pack('f', f))[0] - -############################################################################### -### navdata -############################################################################### -def decode_navdata(packet): - """Decode a navdata packet.""" - offset = 0 - _ = struct.unpack_from("IIII", packet, offset) - drone_state = dict() - drone_state['fly_mask'] = _[1] & 1 # FLY MASK : (0) ardrone is landed, (1) ardrone is flying - drone_state['video_mask'] = _[1] >> 1 & 1 # VIDEO MASK : (0) video disable, (1) video enable - drone_state['vision_mask'] = _[1] >> 2 & 1 # VISION MASK : (0) vision disable, (1) vision enable */ - drone_state['control_mask'] = _[1] >> 3 & 1 # CONTROL ALGO (0) euler angles control, (1) angular speed control */ - drone_state['altitude_mask'] = _[1] >> 4 & 1 # ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */ - drone_state['user_feedback_start'] = _[1] >> 5 & 1 # USER feedback : Start button state */ - drone_state['command_mask'] = _[1] >> 6 & 1 # Control command ACK : (0) None, (1) one received */ - drone_state['fw_file_mask'] = _[1] >> 7 & 1 # Firmware file is good (1) */ - drone_state['fw_ver_mask'] = _[1] >> 8 & 1 # Firmware update is newer (1) */ - drone_state['fw_upd_mask'] = _[1] >> 9 & 1 # Firmware update is ongoing (1) */ - drone_state['navdata_demo_mask'] = _[1] >> 10 & 1 # Navdata demo : (0) All navdata, (1) only navdata demo */ - drone_state['navdata_bootstrap'] = _[1] >> 11 & 1 # Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */ - drone_state['motors_mask'] = _[1] >> 12 & 1 # Motor status : (0) Ok, (1) Motors problem */ - drone_state['com_lost_mask'] = _[1] >> 13 & 1 # Communication lost : (1) com problem, (0) Com is ok */ - drone_state['vbat_low'] = _[1] >> 15 & 1 # VBat low : (1) too low, (0) Ok */ - drone_state['user_el'] = _[1] >> 16 & 1 # User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/ - drone_state['timer_elapsed'] = _[1] >> 17 & 1 # Timer elapsed : (1) elapsed, (0) not elapsed */ - drone_state['angles_out_of_range'] = _[1] >> 19 & 1 # Angles : (0) Ok, (1) out of range */ - drone_state['ultrasound_mask'] = _[1] >> 21 & 1 # Ultrasonic sensor : (0) Ok, (1) deaf */ - drone_state['cutout_mask'] = _[1] >> 22 & 1 # Cutout system detection : (0) Not detected, (1) detected */ - drone_state['pic_version_mask'] = _[1] >> 23 & 1 # PIC Version number OK : (0) a bad version number, (1) version number is OK */ - drone_state['atcodec_thread_on'] = _[1] >> 24 & 1 # ATCodec thread ON : (0) thread OFF (1) thread ON */ - drone_state['navdata_thread_on'] = _[1] >> 25 & 1 # Navdata thread ON : (0) thread OFF (1) thread ON */ - drone_state['video_thread_on'] = _[1] >> 26 & 1 # Video thread ON : (0) thread OFF (1) thread ON */ - drone_state['acq_thread_on'] = _[1] >> 27 & 1 # Acquisition thread ON : (0) thread OFF (1) thread ON */ - drone_state['ctrl_watchdog_mask'] = _[1] >> 28 & 1 # CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */ - drone_state['adc_watchdog_mask'] = _[1] >> 29 & 1 # ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */ - drone_state['com_watchdog_mask'] = _[1] >> 30 & 1 # Communication Watchdog : (1) com problem, (0) Com is ok */ - drone_state['emergency_mask'] = _[1] >> 31 & 1 # Emergency landing : (0) no emergency, (1) emergency */ - data = dict() - data['drone_state'] = drone_state - data['header'] = _[0] - data['seq_nr'] = _[2] - data['vision_flag'] = _[3] - offset += struct.calcsize("IIII") - while 1: - try: - id_nr, size = struct.unpack_from("HH", packet, offset) - offset += struct.calcsize("HH") - except struct.error: - break - values = [] - for i in range(size-struct.calcsize("HH")): - values.append(struct.unpack_from("c", packet, offset)[0]) - offset += struct.calcsize("c") - # navdata_tag_t in navdata-common.h - if id_nr == 0: - values = struct.unpack_from("IIfffIfffI", "".join(values)) - values = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], values)) - # convert the millidegrees into degrees and round to int, as they - # are not so precise anyways - for i in 'theta', 'phi', 'psi': - values[i] = int(values[i] / 1000) - #values[i] /= 1000 - data[id_nr] = values - return data - - -if __name__ == "__main__": - - import termios - import fcntl - import os - - fd = sys.stdin.fileno() - - oldterm = termios.tcgetattr(fd) - newattr = termios.tcgetattr(fd) - newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO - termios.tcsetattr(fd, termios.TCSANOW, newattr) - - oldflags = fcntl.fcntl(fd, fcntl.F_GETFL) - fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK) - - drone = ARDrone() - - try: - while 1: - try: - c = sys.stdin.read(1) - c = c.lower() - print "Got character", c - if c == 'a': - drone.move_left() - if c == 'd': - drone.move_right() - if c == 'w': - drone.move_forward() - if c == 's': - drone.move_backward() - if c == ' ': - drone.land() - if c == '\n': - drone.takeoff() - if c == 'q': - drone.turn_left() - if c == 'e': - drone.turn_right() - if c == '1': - drone.move_up() - if c == '2': - drone.hover() - if c == '3': - drone.move_down() - if c == 't': - drone.reset() - if c == 'x': - drone.hover() - if c == 'y': - drone.trim() - except IOError: - pass - finally: - termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) - fcntl.fcntl(fd, fcntl.F_SETFL, oldflags) - drone.halt() - diff --git a/libardrone/__init__.py b/libardrone/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/libardrone/ar2video.py b/libardrone/ar2video.py new file mode 100644 index 0000000..b31ab9d --- /dev/null +++ b/libardrone/ar2video.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python + +# Copyright (c) 2011 Bastian Venthur +# Copyright (c) 2013 Adrian Taylor +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + + +""" +Video decoding for the AR.Drone 2.0. + +This is just H.264 encapsulated in a funny way. +""" + +import h264decoder +import paveparser + + +class ARVideo2(object): + def __init__(self, drone, debug=False): + self.h264 = h264decoder.H264Decoder(self, drone.image_shape) + self.paveparser = paveparser.PaVEParser(self.h264) + self.latest_image = None + self._drone = drone + + """ + Called by the PNG splitter when there's an image ready + """ + def image_ready(self, image): + self.latest_image = image + self._drone.set_image(image) + + """ + Guaranteed to return an image as a PIL Image object. + """ + def get_image(self): + return self.latest_image + + def write(self, data): + self.paveparser.write(data) diff --git a/libardrone/ardrone2_video_example.capture b/libardrone/ardrone2_video_example.capture new file mode 100644 index 0000000..eac6654 Binary files /dev/null and b/libardrone/ardrone2_video_example.capture differ diff --git a/libardrone/arnetwork.py b/libardrone/arnetwork.py new file mode 100644 index 0000000..7b5b744 --- /dev/null +++ b/libardrone/arnetwork.py @@ -0,0 +1,136 @@ +# Copyright (c) 2011 Bastian Venthur +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import logging + +""" +This module provides access to the data provided by the AR.Drone. +""" +import threading +import select +import socket +import multiprocessing +import libardrone + +class ARDroneNetworkProcess(threading.Thread): + """ARDrone Network Process. + + This process collects data from the video and navdata port, converts the + data and sends it to the IPCThread. + """ + + def __init__(self, com_pipe, is_ar_drone_2, drone): + threading.Thread.__init__(self) + self._drone = drone + self.com_pipe = com_pipe + self.is_ar_drone_2 = is_ar_drone_2 + if is_ar_drone_2: + import ar2video + self.ar2video = ar2video.ARVideo2(self._drone, libardrone.DEBUG) + else: + import arvideo + + def run(self): + + def _connect(): + logging.warn('Connection to ardrone') + if self.is_ar_drone_2: + video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + video_socket.connect(('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT)) + video_socket.setblocking(0) + else: + video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + video_socket.setblocking(0) + video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT)) + video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT)) + + nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + nav_socket.setblocking(0) + nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT)) + nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT)) + + control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + control_socket.connect(('192.168.1.1', libardrone.ARDRONE_CONTROL_PORT)) + control_socket.setblocking(0) + logging.warn('Connection established') + return video_socket, nav_socket, control_socket + + def _disconnect(video_socket, nav_socket, control_socket): + logging.warn('Disconnection to ardrone streams') + video_socket.close() + nav_socket.close() + control_socket.close() + + video_socket, nav_socket, control_socket = _connect() + + stopping = False + connection_lost = 1 + reconnection_needed = False + while not stopping: + if reconnection_needed: + _disconnect(video_socket, nav_socket, control_socket) + video_socket, nav_socket, control_socket = _connect() + reconnection_needed = False + inputready, outputready, exceptready = select.select([nav_socket, video_socket, self.com_pipe, control_socket], [], [], 1.) + if len(inputready) == 0: + connection_lost += 1 + reconnection_needed = True + for i in inputready: + if i == video_socket: + while 1: + try: + data = video_socket.recv(65536) + if self.is_ar_drone_2: + self.ar2video.write(data) + except IOError: + # we consumed every packet from the socket and + # continue with the last one + break + # Sending is taken care of by the decoder + if not self.is_ar_drone_2: + w, h, image, t = arvideo.read_picture(data) + self._drone.set_image(image) + elif i == nav_socket: + while 1: + try: + data = nav_socket.recv(500) + except IOError: + # we consumed every packet from the socket and + # continue with the last one + break + navdata, has_information = libardrone.decode_navdata(data) + if (has_information): + self._drone.set_navdata(navdata) + elif i == self.com_pipe: + _ = self.com_pipe.recv() + stopping = True + break + elif i == control_socket: + reconnection_needed = False + while not reconnection_needed: + try: + data = control_socket.recv(65536) + if len(data) == 0: + logging.warning('Received an empty packet on control socket') + reconnection_needed = True + else: + logging.warning("Control Socket says : %s", data) + except IOError: + break + _disconnect(video_socket, nav_socket, control_socket) diff --git a/arvideo.py b/libardrone/arvideo.py similarity index 100% rename from arvideo.py rename to libardrone/arvideo.py diff --git a/demo.py b/libardrone/demo.py old mode 100755 new mode 100644 similarity index 95% rename from demo.py rename to libardrone/demo.py index cbd5cae..ae2e152 --- a/demo.py +++ b/libardrone/demo.py @@ -32,18 +32,18 @@ import libardrone - def main(): pygame.init() W, H = 320, 240 screen = pygame.display.set_mode((W, H)) - drone = libardrone.ARDrone() + drone = libardrone.ARDrone(True) + drone.reset() clock = pygame.time.Clock() running = True while running: for event in pygame.event.get(): if event.type == pygame.QUIT: - running = False + running = False elif event.type == pygame.KEYUP: drone.hover() elif event.type == pygame.KEYDOWN: @@ -52,8 +52,10 @@ def main(): running = False # takeoff / land elif event.key == pygame.K_RETURN: + print "return" drone.takeoff() elif event.key == pygame.K_SPACE: + print "space" drone.land() # emergency elif event.key == pygame.K_BACKSPACE: @@ -101,6 +103,7 @@ def main(): drone.speed = 1.0 try: + # print pygame.image surface = pygame.image.fromstring(drone.image, (W, H), 'RGB') # battery status hud_color = (255, 0, 0) if drone.navdata.get('drone_state', dict()).get('emergency_mask', 1) else (10, 10, 255) @@ -122,4 +125,3 @@ def main(): if __name__ == '__main__': main() - diff --git a/libardrone/h264decoder.py b/libardrone/h264decoder.py new file mode 100644 index 0000000..f911cdb --- /dev/null +++ b/libardrone/h264decoder.py @@ -0,0 +1,123 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2013 Adrian Taylor +# Inspired by equivalent node.js code by Felix Geisendörfer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import os + + +""" +H.264 video decoder for AR.Drone 2.0. Uses ffmpeg. +""" + +import sys +from subprocess import PIPE, Popen +from threading import Thread +import time +import libardrone +import ctypes +import numpy as np +import sys + + +try: + from Queue import Queue, Empty +except ImportError: + from queue import Queue, Empty # python 3.x + +ON_POSIX = 'posix' in sys.builtin_module_names + + +def enqueue_output(out, outfileobject, frame_size): + frame_size_bytes = frame_size[0] * frame_size[1] * 3 + while True: + buffer_str = out.read(frame_size_bytes) + im = np.frombuffer(buffer_str, count=frame_size_bytes, dtype=np.uint8) + im = im.reshape((frame_size[0], frame_size[1], 3)) + outfileobject.image_ready(im) + + +# Logic for making ffmpeg terminate on the death of this process +def set_death_signal(signal): + libc = ctypes.CDLL('libc.so.6') + PR_SET_DEATHSIG = 1 + libc.prctl(PR_SET_DEATHSIG, signal) + + +def set_death_signal_int(): + if sys.platform != 'darwin': + SIGINT = 2 + SIGTERM = 15 + set_death_signal(SIGINT) + + +""" +Usage: pass a listener, with a method 'data_ready' which will be called whenever there's output +from ffmpeg. This will be called in an arbitrary thread. You can later call H264ToPng.get_data_if_any to retrieve +said data. +You should then call write repeatedly to write some encoded H.264 data. +""" +class H264Decoder(object): + + def __init__(self, outfileobject=None, frame_size=(360, 640)): + if outfileobject is not None: + + if (H264Decoder.which('ffmpeg') is None): + raise Exception("You need to install ffmpeg to be able to run ardrone") + + p = Popen(["nice", "-n", "15", "ffmpeg", "-i", "-", "-f", "sdl", + "-probesize", "2048", "-flags", "low_delay", "-f", + "rawvideo", "-pix_fmt", 'rgb24', "-"], + stdin=PIPE, stdout=PIPE, stderr=open('/dev/null', 'w'), + bufsize=0, preexec_fn=set_death_signal_int) + t = Thread(target=enqueue_output, args=(p.stdout, outfileobject, frame_size)) + t.daemon = True # thread dies with the program + t.start() + else: + if (H264Decoder.which('ffplay') is None): + raise Exception("You need to install ffmpeg and ffplay to be able to run ardrone in debug mode") + + p = Popen(["nice", "-n", "15", "ffplay", "-probesize", "2048", + "-flags", "low_delay", "-i", "-"], + stdin=PIPE, stdout=open('/dev/null', 'w'), + stderr=open('/dev/null', 'w'), bufsize=-1, + preexec_fn=set_death_signal_int) + + self.writefd = p.stdin + + def write(self, data): + self.writefd.write(data) + + @staticmethod + def which(program): + def is_exe(fpath): + return os.path.isfile(fpath) and os.access(fpath, os.X_OK) + + fpath, fname = os.path.split(program) + if fpath: + if is_exe(program): + return program + else: + for path in os.environ["PATH"].split(os.pathsep): + path = path.strip('"') + exe_file = os.path.join(path, program) + if is_exe(exe_file): + return exe_file + + return None diff --git a/libardrone/libardrone.py b/libardrone/libardrone.py new file mode 100644 index 0000000..92bc025 --- /dev/null +++ b/libardrone/libardrone.py @@ -0,0 +1,653 @@ +# Copyright (c) 2011 Bastian Venthur +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +""" +Python library for the AR.Drone. + +V.1 This module was tested with Python 2.6.6 and AR.Drone vanilla firmware 1.5.1. +V.2.alpha +""" + +import logging +import socket +import struct +import sys +import threading +import multiprocessing + +import arnetwork + +import time +import numpy as np +from mutex import mutex + +__author__ = "Bastian Venthur" + +ARDRONE_NAVDATA_PORT = 5554 +ARDRONE_VIDEO_PORT = 5555 +ARDRONE_COMMAND_PORT = 5556 +ARDRONE_CONTROL_PORT = 5559 + +SESSION_ID = "943dac23" +USER_ID = "36355d78" +APP_ID = "21d958e4" + +DEBUG = False + +# 0: "Not defined" +# 131072: "Landed" +# 393216: "Taking-off-Floor" +# 393217: "Taking-off-Air" +# 262144: "Hovering" +# 524288: "Landing" +# 458752: "Stabilizing" +# 196608: "Moving" +# 262153 and 196613: "Undefined" +ctrl_state_dict={0:0, 131072:1, 393216:2, 393217:3, 262144:4, 524288:5, 458752:6, 196608:7, 262153:8, 196613:9} + + +class ARDrone(object): + """ARDrone Class. + + Instanciate this class to control your drone and receive decoded video and + navdata. + Possible value for video codec (drone2): + NULL_CODEC = 0, + UVLC_CODEC = 0x20, // codec_type value is used for START_CODE + P264_CODEC = 0x40, + MP4_360P_CODEC = 0x80, + H264_360P_CODEC = 0x81, + MP4_360P_H264_720P_CODEC = 0x82, + H264_720P_CODEC = 0x83, + MP4_360P_SLRS_CODEC = 0x84, + H264_360P_SLRS_CODEC = 0x85, + H264_720P_SLRS_CODEC = 0x86, + H264_AUTO_RESIZE_CODEC = 0x87, // resolution is automatically adjusted according to bitrate + MP4_360P_H264_360P_CODEC = 0x88, + """ + + def __init__(self, is_ar_drone_2=False, hd=False): + + self.seq_nr = 1 + self.timer_t = 0.2 + self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) + self.lock = threading.Lock() + self.speed = 0.2 + self.hd = hd + if (self.hd): + self.image_shape = (720, 1280, 3) + else: + self.image_shape = (360, 640, 3) + + time.sleep(0.5) + self.config_ids_string = [SESSION_ID, USER_ID, APP_ID] + self.configure_multisession(SESSION_ID, USER_ID, APP_ID, self.config_ids_string) + self.set_session_id (self.config_ids_string, SESSION_ID) + time.sleep(0.5) + self.set_profile_id(self.config_ids_string, USER_ID) + time.sleep(0.5) + self.set_app_id(self.config_ids_string, APP_ID) + time.sleep(0.5) + self.set_video_bitrate_control_mode(self.config_ids_string, "1") + time.sleep(0.5) + self.set_video_bitrate(self.config_ids_string, "10000") + time.sleep(0.5) + self.set_max_bitrate(self.config_ids_string, "10000") + time.sleep(0.5) + self.set_fps(self.config_ids_string, "30") + time.sleep(0.5) + if (self.hd): + self.set_video_codec(self.config_ids_string, 0x83) + else: + self.set_video_codec(self.config_ids_string, 0x81) + + self.last_command_is_hovering = True + self.com_pipe, com_pipe_other = multiprocessing.Pipe() + + self.navdata = dict() + self.navdata[0] = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) + + self.network_process = arnetwork.ARDroneNetworkProcess(com_pipe_other, is_ar_drone_2, self) + self.network_process.start() + + self.image = np.zeros(self.image_shape, np.uint8) + self.time = 0 + + self.last_command_is_hovering = True + + time.sleep(1.0) + + self.at(at_config_ids , self.config_ids_string) + self.at(at_config, "general:navdata_demo", "TRUE") + + + def takeoff(self): + """Make the drone takeoff.""" + self.at(at_ftrim) + self.at(at_config, "control:altitude_max", "20000") + self.at(at_ref, True) + + def land(self): + """Make the drone land.""" + self.at(at_ref, False) + + def hover(self): + """Make the drone hover.""" + self.at(at_pcmd, False, 0, 0, 0, 0) + + def move_left(self): + """Make the drone move left.""" + self.at(at_pcmd, True, -self.speed, 0, 0, 0) + + def move_right(self): + """Make the drone move right.""" + self.at(at_pcmd, True, self.speed, 0, 0, 0) + + def move_up(self): + """Make the drone rise upwards.""" + self.at(at_pcmd, True, 0, 0, self.speed, 0) + + def move_down(self): + """Make the drone decent downwards.""" + self.at(at_pcmd, True, 0, 0, -self.speed, 0) + + def move_forward(self): + """Make the drone move forward.""" + self.at(at_pcmd, True, 0, -self.speed, 0, 0) + + def move_backward(self): + """Make the drone move backwards.""" + self.at(at_pcmd, True, 0, self.speed, 0, 0) + + def turn_left(self): + """Make the drone rotate left.""" + self.at(at_pcmd, True, 0, 0, 0, -self.speed) + + def turn_right(self): + """Make the drone rotate right.""" + self.at(at_pcmd, True, 0, 0, 0, self.speed) + + def reset(self): + """Toggle the drone's emergency state.""" + self.at(at_ref, False, True) + self.at(at_ref, False, False) + + def trim(self): + """Flat trim the drone.""" + self.at(at_ftrim) + + def set_speed(self, speed): + """Set the drone's speed. + + Valid values are floats from [0..1] + """ + self.speed = speed + + + def at(self, cmd, *args, **kwargs): + """Wrapper for the low level at commands. + + This method takes care that the sequence number is increased after each + at command and the watchdog timer is started to make sure the drone + receives a command at least every second. + """ + self.lock.acquire() + self.com_watchdog_timer.cancel() + cmd(self.seq_nr, *args, **kwargs) + self.seq_nr += 1 + self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) + self.com_watchdog_timer.start() + self.lock.release() + + def configure_multisession(self, session_id, user_id, app_id, config_ids_string): + self.at(at_config, "custom:session_id", session_id) + self.at(at_config, "custom:profile_id", user_id) + self.at(at_config, "custom:application_id", app_id) + + def set_session_id (self, config_ids_string, session_id): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "custom:session_id", session_id) + + def set_profile_id (self, config_ids_string, profile_id): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "custom:profile_id", profile_id) + + def set_app_id (self, config_ids_string, app_id): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "custom:application_id", app_id) + + def set_video_bitrate_control_mode (self, config_ids_string, mode): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "video:bitrate_control_mode", mode) + + def set_video_bitrate (self, config_ids_string, bitrate): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "video:bitrate", bitrate) + + def set_max_bitrate(self, config_ids_string, max_bitrate): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "video:max_bitrate", max_bitrate) + + def set_fps (self, config_ids_string, fps): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "video:codec_fps", fps) + + def set_video_codec (self, config_ids_string, codec): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "video:video_codec", codec) + + def commwdg(self): + """Communication watchdog signal. + + This needs to be send regulary to keep the communication w/ the drone + alive. + """ + self.at(at_comwdg) + + def halt(self): + """Shutdown the drone. + + This method does not land or halt the actual drone, but the + communication with the drone. You should call it at the end of your + application to close all sockets, pipes, processes and threads related + with this object. + """ + self.lock.acquire() + self.com_watchdog_timer.cancel() + self.com_pipe.send('die!') + self.network_process.terminate() + self.network_process.join() + self.ipc_thread.stop() + self.ipc_thread.join() + self.lock.release() + + def get_image(self): + _im = np.copy(self.image) + return _im + + def get_navdata(self): + return self.navdata + + def set_navdata(self, navdata): + self.navdata = navdata + self.get_navdata() + + def set_image(self, image): + if (image.shape == self.image_shape): + self.image = image + self.image = image + + def apply_command(self, command): + available_commands = ["emergency", + "land", "takeoff", "move_left", "move_right", "move_down", "move_up", + "move_backward", "move_forward", "turn_left", "turn_right", "hover"] + if command not in available_commands: + logging.error("Command %s is not a recognized command" % command) + + if command != "hover": + self.last_command_is_hovering = False + + if (command == "emergency"): + self.reset() + elif (command == "land"): + self.land() + self.last_command_is_hovering = True + elif (command == "takeoff"): + self.takeoff() + self.last_command_is_hovering = True + elif (command == "move_left"): + self.move_left() + elif (command == "move_right"): + self.move_right() + elif (command == "move_down"): + self.move_down() + elif (command == "move_up"): + self.move_up() + elif (command == "move_backward"): + self.move_backward() + elif (command == "move_forward"): + self.move_forward() + elif (command == "turn_left"): + self.turn_left() + elif (command == "turn_right"): + self.turn_right() + elif (command == "hover" and not self.last_command_is_hovering): + self.hover() + self.last_command_is_hovering = True + +class ARDrone2(ARDrone): + def __init__(self, hd=False): + ARDrone.__init__(self, True, hd) + +############################################################################### +### Low level AT Commands +############################################################################### + +def at_ref(seq, takeoff, emergency=False): + """ + Basic behaviour of the drone: take-off/landing, emergency stop/reset) + + Parameters: + seq -- sequence number + takeoff -- True: Takeoff / False: Land + emergency -- True: Turn off the engines + """ + p = 0b10001010101000000000000000000 + if takeoff: + p += 0b1000000000 + if emergency: + p += 0b0100000000 + at("REF", seq, [p]) + +def at_pcmd(seq, progressive, lr, fb, vv, va): + """ + Makes the drone move (translate/rotate). + + Parameters: + seq -- sequence number + progressive -- True: enable progressive commands, False: disable (i.e. + enable hovering mode) + lr -- left-right tilt: float [-1..1] negative: left, positive: right + rb -- front-back tilt: float [-1..1] negative: forwards, positive: + backwards + vv -- vertical speed: float [-1..1] negative: go down, positive: rise + va -- angular speed: float [-1..1] negative: spin left, positive: spin + right + + The above float values are a percentage of the maximum speed. + """ + p = 1 if progressive else 0 + at("PCMD", seq, [p, float(lr), float(fb), float(vv), float(va)]) + +def at_ftrim(seq): + """ + Tell the drone it's lying horizontally. + + Parameters: + seq -- sequence number + """ + at("FTRIM", seq, []) + +def at_zap(seq, stream): + """ + Selects which video stream to send on the video UDP port. + + Parameters: + seq -- sequence number + stream -- Integer: video stream to broadcast + """ + # FIXME: improve parameters to select the modes directly + at("ZAP", seq, [stream]) + +def at_config(seq, option, value): + """Set configuration parameters of the drone.""" + at("CONFIG", seq, [str(option), str(value)]) + +def at_config_ids(seq, value): + """Set configuration parameters of the drone.""" + at("CONFIG_IDS", seq, value) + +def at_ctrl(seq, num): + """Ask the parrot to drop its configuration file""" + at("CTRL", seq, [num, 0]) + +def at_comwdg(seq): + """ + Reset communication watchdog. + """ + # FIXME: no sequence number + at("COMWDG", seq, []) + +def at_aflight(seq, flag): + """ + Makes the drone fly autonomously. + + Parameters: + seq -- sequence number + flag -- Integer: 1: start flight, 0: stop flight + """ + at("AFLIGHT", seq, [flag]) + +def at_pwm(seq, m1, m2, m3, m4): + """ + Sends control values directly to the engines, overriding control loops. + + Parameters: + seq -- sequence number + m1 -- front left command + m2 -- fright right command + m3 -- back right command + m4 -- back left command + """ + # FIXME: what type do mx have? + raise NotImplementedError() + +def at_led(seq, anim, f, d): + """ + Control the drones LED. + + Parameters: + seq -- sequence number + anim -- Integer: animation to play + f -- ?: frequence in HZ of the animation + d -- Integer: total duration in seconds of the animation + """ + pass + +def at_anim(seq, anim, d): + """ + Makes the drone execute a predefined movement (animation). + + Parameters: + seq -- sequcence number + anim -- Integer: animation to play + d -- Integer: total duration in sections of the animation + """ + at("ANIM", seq, [anim, d]) + +def at(command, seq, params): + """ + Parameters: + command -- the command + seq -- the sequence number + params -- a list of elements which can be either int, float or string + """ + param_str = '' + for p in params: + if type(p) == int: + param_str += ",%d" % p + elif type(p) == float: + param_str += ",%d" % f2i(p) + elif type(p) == str: + param_str += ',"' + p + '"' + msg = "AT*%s=%i%s\r" % (command, seq, param_str) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.sendto(msg, ("192.168.1.1", ARDRONE_COMMAND_PORT)) + +def f2i(f): + """Interpret IEEE-754 floating-point value as signed integer. + + Arguments: + f -- floating point value + """ + return struct.unpack('i', struct.pack('f', f))[0] + +############################################################################### +### navdata +############################################################################### +def decode_navdata(packet): + """Decode a navdata packet.""" + offset = 0 + _ = struct.unpack_from("IIII", packet, offset) + drone_state = dict() + drone_state['fly_mask'] = _[1] & 1 # FLY MASK : (0) ardrone is landed, (1) ardrone is flying + drone_state['video_mask'] = _[1] >> 1 & 1 # VIDEO MASK : (0) video disable, (1) video enable + drone_state['vision_mask'] = _[1] >> 2 & 1 # VISION MASK : (0) vision disable, (1) vision enable */ + drone_state['control_mask'] = _[1] >> 3 & 1 # CONTROL ALGO (0) euler angles control, (1) angular speed control */ + drone_state['altitude_mask'] = _[1] >> 4 & 1 # ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */ + drone_state['user_feedback_start'] = _[1] >> 5 & 1 # USER feedback : Start button state */ + drone_state['command_mask'] = _[1] >> 6 & 1 # Control command ACK : (0) None, (1) one received */ + drone_state['fw_file_mask'] = _[1] >> 7 & 1 # Firmware file is good (1) */ + drone_state['fw_ver_mask'] = _[1] >> 8 & 1 # Firmware update is newer (1) */ + drone_state['fw_upd_mask'] = _[1] >> 9 & 1 # Firmware update is ongoing (1) */ + drone_state['navdata_demo_mask'] = _[1] >> 10 & 1 # Navdata demo : (0) All navdata, (1) only navdata demo */ + drone_state['navdata_bootstrap'] = _[1] >> 11 & 1 # Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */ + drone_state['motors_mask'] = _[1] >> 12 & 1 # Motor status : (0) Ok, (1) Motors problem */ + drone_state['com_lost_mask'] = _[1] >> 13 & 1 # Communication lost : (1) com problem, (0) Com is ok */ + drone_state['vbat_low'] = _[1] >> 15 & 1 # VBat low : (1) too low, (0) Ok */ + drone_state['user_el'] = _[1] >> 16 & 1 # User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/ + drone_state['timer_elapsed'] = _[1] >> 17 & 1 # Timer elapsed : (1) elapsed, (0) not elapsed */ + drone_state['angles_out_of_range'] = _[1] >> 19 & 1 # Angles : (0) Ok, (1) out of range */ + drone_state['ultrasound_mask'] = _[1] >> 21 & 1 # Ultrasonic sensor : (0) Ok, (1) deaf */ + drone_state['cutout_mask'] = _[1] >> 22 & 1 # Cutout system detection : (0) Not detected, (1) detected */ + drone_state['pic_version_mask'] = _[1] >> 23 & 1 # PIC Version number OK : (0) a bad version number, (1) version number is OK */ + drone_state['atcodec_thread_on'] = _[1] >> 24 & 1 # ATCodec thread ON : (0) thread OFF (1) thread ON */ + drone_state['navdata_thread_on'] = _[1] >> 25 & 1 # Navdata thread ON : (0) thread OFF (1) thread ON */ + drone_state['video_thread_on'] = _[1] >> 26 & 1 # Video thread ON : (0) thread OFF (1) thread ON */ + drone_state['acq_thread_on'] = _[1] >> 27 & 1 # Acquisition thread ON : (0) thread OFF (1) thread ON */ + drone_state['ctrl_watchdog_mask'] = _[1] >> 28 & 1 # CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */ + drone_state['adc_watchdog_mask'] = _[1] >> 29 & 1 # ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */ + drone_state['com_watchdog_mask'] = _[1] >> 30 & 1 # Communication Watchdog : (1) com problem, (0) Com is ok */ + drone_state['emergency_mask'] = _[1] >> 31 & 1 # Emergency landing : (0) no emergency, (1) emergency */ + data = dict() + data['drone_state'] = drone_state + data['header'] = _[0] + data['seq_nr'] = _[2] + data['vision_flag'] = _[3] + offset += struct.calcsize("IIII") + has_flying_information = False + while 1: + try: + id_nr, size = struct.unpack_from("HH", packet, offset) + offset += struct.calcsize("HH") + except struct.error: + break + values = [] + for i in range(size - struct.calcsize("HH")): + values.append(struct.unpack_from("c", packet, offset)[0]) + offset += struct.calcsize("c") + # navdata_tag_t in navdata-common.h + if id_nr == 0: + has_flying_information = True + values = struct.unpack_from("IIfffifffI", "".join(values)) + values = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], values)) + # convert the millidegrees into degrees and round to int, as they + values['ctrl_state'] = ctrl_state_dict[values['ctrl_state']] + # are not so precise anyways + for i in 'theta', 'phi', 'psi': + values[i] = int(values[i] / 1000) + data[id_nr] = values + return data, has_flying_information + + +if __name__ == "__main__": + ''' + For testing purpose only + ''' + import termios + import fcntl + import os + + fd = sys.stdin.fileno() + + oldterm = termios.tcgetattr(fd) + newattr = termios.tcgetattr(fd) + newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO + termios.tcsetattr(fd, termios.TCSANOW, newattr) + + oldflags = fcntl.fcntl(fd, fcntl.F_GETFL) + fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK) + + drone = ARDrone(is_ar_drone_2=True) + + import cv2 + try: + startvideo = True + video_waiting = False + while 1: + time.sleep(.0001) + if startvideo: + try: + cv2.imshow("Drone camera", cv2.cvtColor(drone.get_image(), cv2.COLOR_BGR2RGB)) + cv2.waitKey(1) + except: + if not video_waiting: + print "Video will display when ready" + video_waiting = True + pass + + try: + c = sys.stdin.read(1) + c = c.lower() + print "Got character", c + if c == 'a': + drone.move_left() + if c == 'd': + drone.move_right() + if c == 'w': + drone.move_forward() + if c == 's': + drone.move_backward() + if c == ' ': + drone.land() + if c == '\n': + drone.takeoff() + if c == 'q': + drone.turn_left() + if c == 'e': + drone.turn_right() + if c == '1': + drone.move_up() + if c == '2': + drone.hover() + if c == '3': + drone.move_down() + if c == 't': + drone.reset() + if c == 'x': + drone.hover() + if c == 'y': + drone.trim() + if c == 'i': + startvideo = True + try: + navdata = drone.get_navdata() + + print 'Emergency landing =', navdata['drone_state']['emergency_mask'] + print 'User emergency landing = ', navdata['drone_state']['user_el'] + print 'Navdata type= ', navdata['drone_state']['navdata_demo_mask'] + print 'Altitude= ', navdata[0]['altitude'] + print 'video enable= ', navdata['drone_state']['video_mask'] + print 'vision enable= ', navdata['drone_state']['vision_mask'] + print 'command_mask= ', navdata['drone_state']['command_mask'] + except: + pass + + if c == 'j': + print "Asking for configuration..." + drone.at(at_ctrl, 5) + time.sleep(0.5) + drone.at(at_ctrl, 4) + except IOError: + pass + finally: + termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) + fcntl.fcntl(fd, fcntl.F_SETFL, oldflags) + drone.halt() + diff --git a/libardrone/paveparser.output b/libardrone/paveparser.output new file mode 100644 index 0000000..1943e6d Binary files /dev/null and b/libardrone/paveparser.output differ diff --git a/libardrone/paveparser.py b/libardrone/paveparser.py new file mode 100644 index 0000000..5e6fc08 --- /dev/null +++ b/libardrone/paveparser.py @@ -0,0 +1,159 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2013 Adrian Taylor +# Inspired by equivalent node.js code by Felix Geisendörfer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import struct +""" +The AR Drone 2.0 allows a tcp client to receive H264 (MPEG4.10 AVC) video +from the drone. However, the frames are wrapped by Parrot Video +Encapsulation (PaVE), which this class parses. +""" + +""" +Usage: Pass in an output file object into the constructor, then call write on this. +""" +class PaVEParser(object): + + HEADER_SIZE_SHORT = 64; # sometimes header is longer + + def __init__(self, outfileobject): + self.buffer = "" + self.state = self.handle_header + self.outfileobject = outfileobject + self.misaligned_frames = 0 + self.payloads = 0 + self.drop_old_frames = True + self.align_on_iframe = True + + if self.drop_old_frames: + self.state = self.handle_header_drop_frames + + def write(self, data): + self.buffer += data + while True: + made_progress = self.state() + if not made_progress: + return + + def handle_header(self): + if self.fewer_remaining_than(self.HEADER_SIZE_SHORT): + return False + + (signature, version, video_codec, header_size, self.payload_size, encoded_stream_width, + encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks, + chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw, + stream_id, total_slices, slice_index, header1_size, header2_size, + reserved2, advertised_size, reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s", + self.buffer[0:self.HEADER_SIZE_SHORT]) + + if signature != "PaVE": + self.state = self.handle_misalignment + return True + self.buffer = self.buffer[header_size:] + self.state = self.handle_payload + return True + + def handle_header_drop_frames(self): + + eligible_index = self.buffer.find('PaVE') + + if (eligible_index < 0): + return False + self.buffer = self.buffer[eligible_index:] + + if self.fewer_remaining_than(self.HEADER_SIZE_SHORT): + return False + + eligible_index = 0 + current_index = eligible_index + + while current_index != -1 and len(self.buffer[current_index:]) > self.HEADER_SIZE_SHORT: + (signature, version, video_codec, header_size, payload_size, encoded_stream_width, + encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks, + chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw, + stream_id, total_slices, slice_index, header1_size, + header2_size, reserved2, advertised_size, + reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s", + self.buffer[current_index:current_index + self.HEADER_SIZE_SHORT]) + + if (frame_type != 3 or current_index == 0): + eligible_index = current_index + self.payload_size = payload_size + + offset = self.buffer[current_index + 1:].find('PaVE') + 1 + if (offset == 0): + break + + current_index += offset + + self.buffer = self.buffer[eligible_index + header_size:] + self.state = self.handle_payload + return True + + + def handle_misalignment(self): + """Sometimes we start of in the middle of frame - look for the PaVE header.""" + IFrame = False + if self.align_on_iframe: + while (not IFrame): + index = self.buffer.find('PaVE') + if index == -1: + return False + + self.buffer = self.buffer[index:] + + if self.fewer_remaining_than(self.HEADER_SIZE_SHORT): + return False + + (signature, version, video_codec, header_size, self.payload_size, encoded_stream_width, + encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks, + chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw, + stream_id, total_slices, slice_index, header1_size, header2_size, reserved2, advertised_size, + reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s", self.buffer[0:self.HEADER_SIZE_SHORT]) + + IFrame = (frame_type == 1 or frame_type == 2) + if not IFrame: + self.buffer = self.buffer[header_size:] + else: + index = self.buffer.find('PaVE') + if index == -1: + return False + self.buffer = self.buffer[index:] + + self.misaligned_frames += 1 + self.state = self.handle_header + + return True + + def handle_payload(self): + if self.fewer_remaining_than(self.payload_size): + return False + self.state = self.handle_header + if self.drop_old_frames: + self.state = self.handle_header_drop_frames + + self.outfileobject.write(self.buffer[0:self.payload_size]) + self.buffer = self.buffer[self.payload_size:] + self.payloads += 1 + return True + + def fewer_remaining_than(self, desired_size): + return len(self.buffer) < desired_size diff --git a/libardrone/test_h264_decoder.py b/libardrone/test_h264_decoder.py new file mode 100644 index 0000000..1107fd7 --- /dev/null +++ b/libardrone/test_h264_decoder.py @@ -0,0 +1,17 @@ +import paveparser +import mock +import h264decoder +import os + + +def test_h264_decoder(): + outfileobj = mock.Mock() + decoder = h264decoder.H264Decoder(outfileobj) + example_video_stream = open(os.path.join(os.path.dirname(__file__), 'paveparser.output')) + while True: + data = example_video_stream.read(1000) + if len(data) == 0: + break + decoder.write(data) + + assert outfileobj.image_ready.called diff --git a/test_libardrone.py b/libardrone/test_libardrone.py similarity index 100% rename from test_libardrone.py rename to libardrone/test_libardrone.py diff --git a/libardrone/test_losing_connection.py b/libardrone/test_losing_connection.py new file mode 100644 index 0000000..0765d76 --- /dev/null +++ b/libardrone/test_losing_connection.py @@ -0,0 +1,67 @@ +import select +import socket +import struct +import time + +DRONE_IP = "192.168.1.1" +ARDRONE_NAVDATA_PORT = 5554 +ARDRONE_COMMAND_PORT = 5556 + +''' +Small endless loop to test the robustness of the tcp ip connection (video streaming) +Warning: This test does not stop, it raises an exception when the connection is lost or +if something goes wrong (most likely the drone stops sending video data and +send empty packets on the command port... +''' + +def at(command, seq, params): + """ + Parameters: + command -- the command + seq -- the sequence number + params -- a list of elements which can be either int, float or string + """ + param_str = '' + for p in params: + if type(p) == int: + param_str += ",%d" % p + elif type(p) == float: + param_str += ",%d" % f2i(p) + elif type(p) == str: + param_str += ',"' + p + '"' + msg = "AT*%s=%i%s\r" % (command, seq, param_str) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.sendto(msg, ("192.168.1.1", ARDRONE_COMMAND_PORT)) + +def f2i(f): + """Interpret IEEE-754 floating-point value as signed integer. + Arguments: + f -- floating point value + """ + return struct.unpack('i', struct.pack('f', f))[0] + + +if __name__ == '__main__': + nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + nav_socket.connect((DRONE_IP, ARDRONE_NAVDATA_PORT)) + nav_socket.setblocking(0) + nav_socket.send("\x01\x00\x00\x00") + + seq = 1 + stopping = 1 + while stopping < 100: + inputready, outputready, exceptready = select.select([nav_socket], [], [], 1) + seq += 1 + at("COMWDG", seq, []) + if len(inputready) == 0: + print "Connection lost for the %d time !" % stopping + nav_socket.send("\x01\x00\x00\x00") + stopping += 1 + for i in inputready: + while 1: + try: + data = nav_socket.recv(500) + except IOError: + break + + raise Exception("Should not get here") diff --git a/libardrone/test_paveparser.py b/libardrone/test_paveparser.py new file mode 100644 index 0000000..49e65f9 --- /dev/null +++ b/libardrone/test_paveparser.py @@ -0,0 +1,20 @@ +import paveparser +import mock +import os + + +def test_misalignment(): + outfile = mock.Mock() + p = paveparser.PaVEParser(outfile) + example_video_stream = open(os.path.join(os.path.dirname(__file__), 'ardrone2_video_example.capture')) + while True: + data = example_video_stream.read(1000000) + if len(data) == 0: + break + p.write(data) + + assert outfile.write.called + assert p.misaligned_frames < 3 + +if __name__ == "__main__": + test_misalignment() diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..6eb0dc9 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +PIL==1.1.7 +mock==1.0.1 +numpy==1.7.1 diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..5777e6d --- /dev/null +++ b/setup.py @@ -0,0 +1,13 @@ +from setuptools import setup, find_packages + + +setup( + name='python-ardrone', + author='Brain Corporation', + author_email='passot@braincorporation.com', + url='https://github.com/braincorp/python-ardrone', + long_description='', + version='dev', + packages=find_packages(), + include_package_data=True, + install_requires=[])