bridge.py 6.16 KB
Newer Older
1
#!/usr/bin/env python3
Marlene Böhmer's avatar
Marlene Böhmer committed
2 3 4 5

import logging
import sys
import re
6
import datetime
Marlene Böhmer's avatar
Marlene Böhmer committed
7 8 9 10 11
import time

import prrt

import cflib.crtp
12
from cflib.crazyflie import Crazyflie, State
13
from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
Marlene Böhmer's avatar
Marlene Böhmer committed
14

15
logging.basicConfig(stream=sys.stdout, level=logging.INFO)
16
logger = logging.getLogger(__name__)
Marlene Böhmer's avatar
Marlene Böhmer committed
17 18 19 20 21 22 23 24

MTU = 32
DEFAULT_TARGET_DELAY = 200
PRRT_LOCAL_PORT = 5000


class CrazyflieConnection:
    def __init__(self, uri, receive_callback):
25
        self.uri = uri
Marlene Böhmer's avatar
Marlene Böhmer committed
26 27 28 29 30 31 32 33 34
        self.receive_callback = receive_callback

        self._cf = Crazyflie()

        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)

35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
        logger.info('Connecting to {}'.format(uri))

        # Manually open link of _cf without starting connection setup.
        # This prevents sending messages from the library at this bridging point.
        self._cf.connection_requested.call(uri)
        self._cf.state = State.INITIALIZED
        self._cf.link_uri = uri
        try:
            self._cf.link = cflib.crtp.get_link_driver(
                uri, self._cf._link_quality_cb, self._cf._link_error_cb)

            if not self._cf.link:
                message = 'No driver found or malformed URI: {}'.format(uri)
                logger.warning(message)
                self._cf.connection_failed.call(uri, message)
            else:
                # Add a callback so we can check that any data is coming back from the copter
                self._cf.packet_received.add_callback(self._cf._check_for_initial_packet_cb)
                self._cf.platform.fetch_platform_informations(self._fetched_platform_information)

        except Exception as ex:  # pylint: disable=W0703
            # We want to catch every possible exception here and show it in the user interface
            import traceback

            logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc())
            exception_text = "Couldn't load link driver: %s\n\n%s" % (ex, traceback.format_exc())
            if self._cf.link:
                self._cf.link.close()
                self._cf.link = None
            self._cf.connection_failed.call(uri, exception_text)
Marlene Böhmer's avatar
Marlene Böhmer committed
65 66

        # Variable used to keep main loop occupied until disconnect
67
        self.is_connected = False
Marlene Böhmer's avatar
Marlene Böhmer committed
68

69 70 71 72 73 74
    def _fetched_platform_information(self):
        self._cf.connected_ts = datetime.datetime.now()
        self._cf.connected.call(self.uri)

    def _connected(self, link_uri):
        logger.info('Connected to {}'.format(link_uri))
75
        self.is_connected = True
Marlene Böhmer's avatar
Marlene Böhmer committed
76 77
        self._cf.packet_received.callbacks = [self.receive_callback]
        self._cf.incoming.cb = []
Marlene Böhmer's avatar
Marlene Böhmer committed
78 79 80 81 82

    def send(self, pk):
        self._cf.send_packet(pk)

    def _connection_failed(self, link_uri, msg):
83 84
        """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)"""
        logger.info('Connection to {} failed: {}'.format(link_uri, msg))
Marlene Böhmer's avatar
Marlene Böhmer committed
85 86 87
        self.is_connected = False

    def _connection_lost(self, link_uri, msg):
88
        logger.info('Connection to {} lost: {}'.format(link_uri, msg))
Marlene Böhmer's avatar
Marlene Böhmer committed
89 90

    def _disconnected(self, link_uri):
91
        logger.info('Disconnected from {}'.format(link_uri))
Marlene Böhmer's avatar
Marlene Böhmer committed
92 93 94
        self.is_connected = False

    def close(self):
95
        logger.info('Closing Crazyflie Connection')
Marlene Böhmer's avatar
Marlene Böhmer committed
96
        self._cf.close_link()
Marlene Böhmer's avatar
Marlene Böhmer committed
97 98
        self._cf.packet_received.remove_callback(self.receive_callback)
        self.receive_callback = None
Marlene Böhmer's avatar
Marlene Böhmer committed
99 100 101 102


class ClientConnection:
    def __init__(self, uri):
103 104
        self.uri = uri

Marlene Böhmer's avatar
Marlene Böhmer committed
105 106 107 108 109 110 111 112 113 114
        uri_match = re.search(r'^prrt://((?:[\d]{1,3})\.(?:[\d]{1,3})\.(?:[\d]{1,3})\.(?:[\d]{1,3})):([\d]{1,5})'
                              r'(?:/([\d]{1,9}))?$', uri)
        if not uri_match:
            raise Exception('Invalid PRRT URI')
        address = uri_match.group(1)
        port = int(uri_match.group(2))
        target_delay_us = DEFAULT_TARGET_DELAY
        if uri_match.group(3):
            target_delay_us = int(uri_match.group(3))

115
        logger.info('Open PRRT Link to {}:{} with target delay {}'.format(address, port, target_delay_us))
Marlene Böhmer's avatar
Marlene Böhmer committed
116 117 118 119 120 121 122

        self._prrt_socket = prrt.PrrtSocket(("0.0.0.0", PRRT_LOCAL_PORT), maximum_payload_size=MTU,
                                            target_delay=target_delay_us)
#        self.prrt_socket.coding_configuration = prrt.PrrtCodingConfiguration(1, 1, [0])
        self._prrt_socket.connect((address, port))

    def send(self, pk):
123
        pk_bytes = bytearray([pk.get_header()]) + pk.data
124
        self._prrt_socket.send_sync(pk_bytes)
Marlene Böhmer's avatar
Marlene Böhmer committed
125 126

    def receive(self):
127 128 129
        pk_bytes, _ = self._prrt_socket.receive_asap()
        if len(pk_bytes) > 0:
            pk = CRTPPacket(pk_bytes[0], pk_bytes[1:])
130
            return pk
131 132
        else:
            return None
Marlene Böhmer's avatar
Marlene Böhmer committed
133 134

    def close(self):
135 136 137
        logger.info('Closing Client Connection')
        self._prrt_socket = None
        logger.info('Disconnected from {}'.format(self.uri))
Marlene Böhmer's avatar
Marlene Böhmer committed
138 139 140 141 142 143 144 145


class Bridge:
    def __init__(self, crazyflie_uri, client_uri):
        cflib.crtp.init_drivers(enable_debug_driver=False)

        self._client_connection = ClientConnection(client_uri)
        self._crazyflie_connection = CrazyflieConnection(crazyflie_uri, self._client_connection.send)
Marlene Böhmer's avatar
Marlene Böhmer committed
146
        self.stop_running = False
Marlene Böhmer's avatar
Marlene Böhmer committed
147

148 149 150 151
    def wait_for_crazyflie_connection(self):
        while not self._crazyflie_connection.is_connected:
            time.sleep(1)

Marlene Böhmer's avatar
Marlene Böhmer committed
152
    def run(self):
Marlene Böhmer's avatar
Marlene Böhmer committed
153
        while not self.stop_running and self._crazyflie_connection.is_connected:
Marlene Böhmer's avatar
Marlene Böhmer committed
154 155
            pk = self._client_connection.receive()
            if pk:
156 157
                if pk.port == CRTPPort.LINKCTRL and pk.channel == 3:
                    continue
Marlene Böhmer's avatar
Marlene Böhmer committed
158 159 160 161
                self._crazyflie_connection.send(pk)
        self.stop()

    def stop(self):
Marlene Böhmer's avatar
Marlene Böhmer committed
162
        self.stop_running = True
Marlene Böhmer's avatar
Marlene Böhmer committed
163
        self._crazyflie_connection.close()
Marlene Böhmer's avatar
Marlene Böhmer committed
164
        self._client_connection.close()
Marlene Böhmer's avatar
Marlene Böhmer committed
165 166 167 168 169 170 171 172 173


if __name__ == '__main__':
    serial_uri = 'serial://pi'
    prrt_uri = sys.argv[1]

    bridge = Bridge(serial_uri, prrt_uri)

    try:
174
        bridge.wait_for_crazyflie_connection()
Marlene Böhmer's avatar
Marlene Böhmer committed
175 176
        bridge.run()
    except KeyboardInterrupt:
177
        print('\n')
Marlene Böhmer's avatar
Marlene Böhmer committed
178
        bridge.stop()