PyMavRouter/example_no_thread.py at main · noahredon/PyMavRouter · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
# This script is an example of how to route MAVLink messages between multiple drones,
# ground control stations (GCS), an admin, and a log server.

from mavrouter import MavDevice, MavRouter

# ---------------------------------------------------------
# Step 1: Define all the devices (Drones, GCS, Admin, Logs)
# ---------------------------------------------------------

# Drones connected via serial ports (USB)
# listen_to=True means we want to receive messages from them
drone_team_a = MavDevice(address="/dev/ttyUSB0", listen_to=True)
drone_team_b = MavDevice(address="/dev/ttyUSB1", listen_to=True)

# Ground Control Stations connected via network (UDP/TCP)
# is_input=False means we are creating a new network endpoint for them to connect to
gcs_team_a = MavDevice(address="192.168.1.10:14610", listen_to=True, is_input=False)
gcs_team_b = MavDevice(address="192.168.2.20:14620", listen_to=True, is_input=False)

# Admin network and Logging server
admin = MavDevice(address="127.0.0.1:14550", listen_to=True, is_input=False)
# listen_to=False because the log server only receives data; it never sends data back
log_server = MavDevice(address="127.0.0.1:14560", listen_to=False, is_input=False)


# ---------------------------------------------------------
# Step 2: Define routing rules (where should messages go?)
# ---------------------------------------------------------

# When Drone A sends a message, forward it to GCS A, the Admin, and the Log Server
def callback_drone_a(msg):
    gcs_team_a.send_msg(msg)
    admin.send_msg(msg)
    log_server.send_msg(msg)
drone_team_a.set_callback(callback_drone_a)

# When Drone B sends a message, forward it to GCS B, the Admin, and the Log Server
def callback_drone_b(msg):
    gcs_team_b.send_msg(msg)
    admin.send_msg(msg)
    log_server.send_msg(msg)
drone_team_b.set_callback(callback_drone_b)

# When GCS A sends a message, forward it to Drone A, the Admin, and the Log Server
def callback_gcs_a(msg):
    drone_team_a.send_msg(msg)
    admin.send_msg(msg)
    log_server.send_msg(msg)
gcs_team_a.set_callback(callback_gcs_a)

# When GCS B sends a message, forward it to Drone B, the Admin, and the Log Server
def callback_gcs_b(msg):
    drone_team_b.send_msg(msg)
    admin.send_msg(msg)
    log_server.send_msg(msg)
gcs_team_b.set_callback(callback_gcs_b)

# The Admin has special rules: it checks the 'target_system' ID of the message
def callback_admin(msg):
    # If the message is meant for Drone 1 (Team A), send it only to Team A
    if hasattr(msg, 'target_system') and msg.target_system == 1:
        drone_team_a.send_msg(msg)
        gcs_team_a.send_msg(msg)
    # If the message is meant for Drone 2 (Team B), send it only to Team B
    elif hasattr(msg, 'target_system') and msg.target_system == 2:
        drone_team_b.send_msg(msg)
        gcs_team_b.send_msg(msg)
    # If there is no specific target, broadcast the message to everyone
    else:
        drone_team_a.send_msg(msg)
        gcs_team_a.send_msg(msg)
        drone_team_b.send_msg(msg)
        gcs_team_b.send_msg(msg)

    # Always save admin messages to the log server and print to the console
    log_server.send_msg(msg)
    print(f"New msg from admin: {msg.get_type()}")
admin.set_callback(callback_admin)


# ---------------------------------------------------------
# Step 3: Start the Router
# ---------------------------------------------------------

# Create the router
router = MavRouter(dialect="ardupilotmega", mavlink_version=2.0, forward_bad_data=False)

# Add all our devices to the router so it can manage them
for device in [drone_team_a, drone_team_b, gcs_team_a, gcs_team_b, admin, log_server]:
    router.add_device(device)

# Start the infinite loop to process messages (Press Ctrl+C to stop)
router.route_loop()