diff --git a/README.md b/README.md index bad9052..20d45c2 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,14 @@ -Flattr this +About: +------ -[![Video of the drone in action](https://img.youtube.com/vi/2HEV37GbUow/0.jpg)](https://www.youtube.com/watch?v=2HEV37GbUow "Click to go to the video.") +This is a quick and dirty fork of Bastian Venthur's python-ardrone library modified to support the ARDrone2 as inspired by (and mostly borrowed from) Jonathan Hunt's proposed changes. + +The primary difference between this fork and the parent is support for the unique way the ARDrone2 transmits video to clients. Instead of sending raw images via UDP as the original ARDrone does, a TCP connection is maintained over which H.264 encoded video (wrapped in a strange codec designed by Parrot) is streamed. + +**Warning**: This is dirty. Very dirty. Both the code (sloppy) and the architecture (piping H.264 over stdin to a separate ffmpeg process and listening for decoded frames over stdout). But it works! And performance seems to be perfectly acceptable. + +A big thank you to Bastian Venthur and Jonathan Hunt for doing 99.99999% of the work behind this! Consider tipping Bastian Venthur's Flattr account (see the original python-ardrone) if you find this helpful. -A video of the library controlling a drone in action (click to jump to the video). Getting Started: ---------------- @@ -40,13 +46,6 @@ Here is a [video] of the library in action: [video]: http://youtu.be/2HEV37GbUow -Repository: ------------ - -The public repository is located here: - - git://github.com/venthur/python-ardrone.git - Requirements: ------------- diff --git a/__init__.py b/__init__.py new file mode 100644 index 0000000..f795acc --- /dev/null +++ b/__init__.py @@ -0,0 +1 @@ +from libardrone import * diff --git a/ar2video.py b/ar2video.py new file mode 100644 index 0000000..ee0a76b --- /dev/null +++ b/ar2video.py @@ -0,0 +1,229 @@ +# -*- 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 ctypes +import os +import struct +from subprocess import PIPE, Popen +import sys +from threading import Thread + +import numpy as np + + +ON_POSIX = 'posix' in sys.builtin_module_names + + +def enqueue_output(out, output_pipe, 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)) + #output_pipe.send(im) + output_pipe.send(buffer_str) + + +# 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) + +""" +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, outputpipe, frame_size=(360, 640)): + self.buffer = "" + self.state = self.handle_header + self.outputpipe = outputpipe + 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 + + if (PaVEParser.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", + p = Popen(["nice", "-n", "15", "ffmpeg", "-i", "-", "-f", "h264", + "-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, self.outputpipe, frame_size)) + t.daemon = True # thread dies with the program + t.start() + self._ffmpeg_output_thread = t + self._ffmpeg_process = p + self.ffmpeg_write_buffer = p.stdin + + + 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.ffmpeg_write_buffer.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\ + + @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 \ No newline at end of file diff --git a/arnetwork.py b/arnetwork.py index bab11cc..16803f6 100644 --- a/arnetwork.py +++ b/arnetwork.py @@ -29,7 +29,7 @@ import multiprocessing import libardrone -import arvideo +import ar2video class ARDroneNetworkProcess(multiprocessing.Process): @@ -39,17 +39,21 @@ class ARDroneNetworkProcess(multiprocessing.Process): data and sends it to the IPCThread. """ - def __init__(self, nav_pipe, video_pipe, com_pipe): + def __init__(self, nav_pipe, video_pipe, com_pipe, frame_size=(360,640)): multiprocessing.Process.__init__(self) self.nav_pipe = nav_pipe self.video_pipe = video_pipe self.com_pipe = com_pipe + self.paveparser = ar2video.PaVEParser(self.video_pipe, frame_size=frame_size) def run(self): - video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + #video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + #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)) + 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) - 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) @@ -64,12 +68,13 @@ def run(self): while 1: try: data = video_socket.recv(65535) + self.paveparser.write(data) 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) + #w, h, image, t = arvideo.read_picture(data) + #self.video_pipe.send(image) elif i == nav_socket: while 1: try: diff --git a/demo.py b/demo.py index cbd5cae..f22d08a 100755 --- a/demo.py +++ b/demo.py @@ -35,9 +35,9 @@ def main(): pygame.init() - W, H = 320, 240 + W, H = 640, 360 screen = pygame.display.set_mode((W, H)) - drone = libardrone.ARDrone() + drone = libardrone.ARDrone(frame_size=(W,H)) clock = pygame.time.Clock() running = True while running: @@ -101,7 +101,8 @@ def main(): drone.speed = 1.0 try: - surface = pygame.image.fromstring(drone.image, (W, H), 'RGB') + #surface = pygame.image.fromstring(drone.image, (W, H), 'RGB') + surface = pygame.image.frombuffer(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) bat = drone.navdata.get(0, dict()).get('battery', 0) diff --git a/libardrone.py b/libardrone.py index e58bbc6..23b4aa0 100644 --- a/libardrone.py +++ b/libardrone.py @@ -50,7 +50,7 @@ class ARDrone(object): navdata. """ - def __init__(self): + def __init__(self, frame_size=(360,640)): self.seq_nr = 1 self.timer_t = 0.2 self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) @@ -60,7 +60,8 @@ def __init__(self): 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 = arnetwork.ARDroneNetworkProcess( + nav_pipe_other, video_pipe_other, com_pipe_other, frame_size) self.network_process.start() self.ipc_thread = arnetwork.IPCThread(self) self.ipc_thread.start() @@ -319,6 +320,7 @@ def at(command, seq, params): 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)) + print "SENT: %s" % msg def f2i(f): """Interpret IEEE-754 floating-point value as signed integer.