From 5772af4eed51e45de06b1329ed3be3158c4c2ccf Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Wed, 7 Jan 2015 17:51:55 +0100 Subject: server2 --- libardrone/libardrone.py | 690 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 690 insertions(+) create mode 100644 libardrone/libardrone.py (limited to 'libardrone/libardrone.py') diff --git a/libardrone/libardrone.py b/libardrone/libardrone.py new file mode 100644 index 0000000..348ce0f --- /dev/null +++ b/libardrone/libardrone.py @@ -0,0 +1,690 @@ +# 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 + +__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 and 262155 and 196614 and 458753: "Undefined" +ctrl_state_dict={0:0, 131072:1, 393216:2, 393217:3, 262144:4, 524288:5, 458752:6, 196608:7, 262153:8, 196613:9, 262155:10, 196614:11, 458753: 12} + + +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, 0x88) + + 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 set_max_alt(self, alt): + self.at(at_config_ids , self.config_ids_string) + self.at(at_config, "control:altitude_max", alt) + + def set_max_vz(self, speed): + self.at(at_config_ids , self.config_ids_string) + self.at(at_config, "control:control_vz_max", speed) + + def set_max_angle(self, eul): + self.at(at_config_ids , self.config_ids_string) + self.at(at_config, "control:euler_angle_max", eul) + + def set_max_rotspeed(self, speed): + self.at(at_config_ids , self.config_ids_string) + self.at(at_config, "control:control_yaw", speed) + + 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_freely(self, hov, x,y, alt, rot): + self.at(at_pcmd, hov, x, y, alt, rot) + + 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.""" + # Enter emergency mode + self.at(at_ref, False, True) + self.at(at_ref, False, False) + # Leave emergency mode + self.at(at_ref, False, True) + + 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 set_camera_view(self, downward): + """ + Set which video camera is used. If 'downward' is true, + downward camera will be viewed - otherwise frontwards. + """ + channel = None + if downward: + channel = 0 + else: + channel = 1 + self.set_video_channel(self.config_ids_string, channel) + + 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_video_channel(self, config_ids_string, channel): + self.at(at_config_ids , config_ids_string) + self.at(at_config, "video:video_channel", channel) + + 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.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.encode("utf-8"), ("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 + try: + values['ctrl_state'] = ctrl_state_dict[values['ctrl_state']] + except KeyError: + values['ctrl_state'] = -1 + # 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() + -- cgit v1.2.3