Commit c120bbed authored by Marlene Böhmer's avatar Marlene Böhmer

Add Bridge Script

parent 7350f04c
#!/usr/bin/env python
import logging
import sys
import re
import time
import prrt
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crtp.crtpstack import CRTPPacket
logging.basicConfig(level=logging.DEBUG)
MTU = 32
DEFAULT_TARGET_DELAY = 200
PRRT_LOCAL_PORT = 5000
class CrazyflieConnection:
def __init__(self, uri, receive_callback):
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)
print('Connecting to %s' % uri)
self._cf.open_link(uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
self._cf.packet_received.add_callback(self.receive_callback)
def send(self, pk):
self._cf.send_packet(pk)
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
def close(self):
print('Closing Crazyflie Connection')
self._cf.close_link()
class ClientConnection:
def __init__(self, uri):
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))
print('Open PRRT Link to {}:{} with target delay {}'.format(address, port, target_delay_us))
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):
print('send to client')
def receive(self):
print('receive from client')
pk = None
return pk
def close(self):
print('Closing Client Connection')
# self._prrt_socket.close()
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)
def run(self):
while self._crazyflie_connection.is_connected:
pk = self._client_connection.receive()
if pk:
self._crazyflie_connection.send(pk)
time.sleep(2)
self.stop()
def stop(self):
self._client_connection.close()
self._crazyflie_connection.close()
if __name__ == '__main__':
serial_uri = 'serial://pi'
prrt_uri = sys.argv[1]
bridge = Bridge(serial_uri, prrt_uri)
try:
bridge.run()
except KeyboardInterrupt:
bridge.stop()
......@@ -11,7 +11,7 @@ import cflib.crtp
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
URI = 'prrt://10.8.0.32:6000/250'
URI = 'prrt://10.8.0.208:5000/250'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment