Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
72 changes: 54 additions & 18 deletions examples/mavlink_tunnel.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,10 @@ def main():

# Listen for incoming connections
tcp_sock.listen(1)
tcp_sock.settimeout(0.2)
connection = None
connection_lock = threading.Lock()
stopping = threading.Event()

def handle_Targetted(msg):
'''handle Targetted message'''
Expand All @@ -33,8 +36,17 @@ def handle_Targetted(msg):

if msg.message.target_node == args.node_id:
if msg.message.buffer:
if connection:
connection.sendall(bytes(msg.message.buffer))
with connection_lock:
client = connection
if client is not None:
try:
client.sendall(bytes(msg.message.buffer))
except ConnectionError:
pass # The peer may have disconnected during the send.
except OSError:
with connection_lock:
if connection is client and not stopping.is_set():
raise

def publish_NodeInfo():
msg = dronecan.uavcan.protocol.NodeStatus()
Expand Down Expand Up @@ -67,21 +79,35 @@ def publish_MavlinkMsg(mav_pkt):

def tcp_server_loop():
nonlocal connection
while True:
print(f'Waiting for GCS Connection on tcp:{args.tcp_host}:{args.tcp_port}...')
connection, client_address = tcp_sock.accept()
print("GCS connected")

print(f'Waiting for GCS Connection on tcp:{args.tcp_host}:{args.tcp_port}...')
while not stopping.is_set():
try:
# Receive the data in small chunks and retransmit it
while True:
data = connection.recv(1024)
if data:
client, _ = tcp_sock.accept()
except socket.timeout:
continue

with client:
with connection_lock:
if stopping.is_set():
return
connection = client
print("GCS connected")
try:
while not stopping.is_set():
try:
data = client.recv(1024)
except ConnectionError:
break
except OSError:
if not stopping.is_set():
raise
break
if not data:
break
publish_MavlinkMsg(data)

finally:
# Clean up the connection
connection.close()
finally:
with connection_lock:
connection = None

# Initializing a DroneCAN node instance.
node = dronecan.make_node(args.uri, node_id=args.node_id, bitrate=1000000)
Expand All @@ -100,9 +126,19 @@ def tcp_server_loop():
try:
node.spin()
except KeyboardInterrupt:
tcp_server_loop.join()
if connection:
connection.close()
pass
finally:
stopping.set()
with connection_lock:
if connection is not None:
# Wake the worker if it is waiting for data from a client.
try:
connection.shutdown(socket.SHUT_RDWR)
except OSError:
pass # The peer may have disconnected already.
tcp_server_loop_thread.join()
tcp_sock.close()
node.close()


if __name__ == '__main__':
Expand Down
137 changes: 137 additions & 0 deletions test/test_mavlink_tunnel.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
import json
import os
from pathlib import Path
from queue import Queue
import runpy
import signal
import socket
import struct
import subprocess
import sys
from threading import Thread
from types import SimpleNamespace
import unittest


EXAMPLE = Path(__file__).resolve().parents[1] / 'examples' / 'mavlink_tunnel.py'


class TunnelMessage:
OPTION_LOCK_PORT = 1

def __init__(self):
self.protocol = SimpleNamespace()


class FakeNode:
def periodic(self, *_args):
pass

def add_handler(self, _message_type, callback):
self.handle = callback

def broadcast(self, message):
print(json.dumps({
'payload': bytes(message.buffer).decode('ascii'),
'target': message.target_node,
'protocol': message.protocol.protocol,
}))

def spin(self):
print('node ready', file=sys.stderr)
for line in sys.stdin:
self.handle(SimpleNamespace(message=SimpleNamespace(
target_node=100, buffer=line.rstrip('\n').encode('ascii'),
)))

def close(self):
print('node closed')


@unittest.skipUnless(os.name == 'posix', 'requires sending SIGINT to a subprocess')
class MavlinkTunnelTests(unittest.TestCase):
def start_tunnel(self):
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as reservation:
reservation.bind(('127.0.0.1', 0))
port = reservation.getsockname()[1]
child = subprocess.Popen(
[sys.executable, '-u', __file__, '--tunnel-child', str(port)],
stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True,
)

lines = Queue()

def read_output(stream):
for line in stream:
lines.put(line.rstrip())

readers = [Thread(target=read_output, args=(stream,), daemon=True) for stream in (child.stdout, child.stderr)]
for reader in readers:
reader.start()

def clean_up():
if child.poll() is None:
child.kill()
child.wait(timeout=5)
for reader in readers:
reader.join(timeout=5)
child.stdin.close()
child.stdout.close()
child.stderr.close()

self.addCleanup(clean_up)
self.assertCountEqual([lines.get(timeout=5), lines.get(timeout=5)], [
'node ready', f'Waiting for GCS Connection on tcp:127.0.0.1:{port}...',
])
return child, lines, ('127.0.0.1', port)

def check_forwarding(self, child, lines, client, payload):
self.assertEqual(lines.get(timeout=5), 'GCS connected')
client.sendall(payload.encode('ascii'))
self.assertEqual(json.loads(lines.get(timeout=5)), {'payload': payload, 'target': 10, 'protocol': 1})
child.stdin.write(payload + '\n')
child.stdin.flush()
with client.makefile('rb') as response:
self.assertEqual(response.read(len(payload)), payload.encode('ascii'))

def interrupt(self, child, lines):
child.send_signal(signal.SIGINT)
self.assertEqual(lines.get(timeout=5), 'node closed')
self.assertEqual(child.wait(timeout=5), 0)

def test_interrupt_while_listening(self):
child, lines, _ = self.start_tunnel()
self.interrupt(child, lines)

def test_reconnect_and_interrupt_active_client(self):
child, lines, address = self.start_tunnel()
with socket.create_connection(address, timeout=5) as first:
self.check_forwarding(child, lines, first, 'first')
first.shutdown(socket.SHUT_WR)
self.assertEqual(first.recv(1), b'')
with socket.create_connection(address, timeout=5) as second:
self.check_forwarding(child, lines, second, 'second')
self.interrupt(child, lines)
self.assertEqual(second.recv(1), b'')

def test_reconnect_after_client_reset(self):
child, lines, address = self.start_tunnel()
with socket.create_connection(address, timeout=5) as first:
self.check_forwarding(child, lines, first, 'first')
first.setsockopt(socket.SOL_SOCKET, socket.SO_LINGER, struct.pack('ii', 1, 0))
with socket.create_connection(address, timeout=5) as second:
self.check_forwarding(child, lines, second, 'second')
self.interrupt(child, lines)


if __name__ == '__main__':
if len(sys.argv) > 1 and sys.argv[1] == '--tunnel-child':
sys.modules['dronecan'] = SimpleNamespace(
make_node=lambda *_args, **_kwargs: FakeNode(),
app=SimpleNamespace(node_monitor=SimpleNamespace(NodeMonitor=lambda _node: None)),
uavcan=SimpleNamespace(tunnel=SimpleNamespace(Targetted=TunnelMessage)),
)
sys.argv = [str(EXAMPLE), '--tcp-host', '127.0.0.1', '--tcp-port', sys.argv[2], 'mock-can']
runpy.run_path(str(EXAMPLE), run_name='__main__')
else:
unittest.main()