PyMavRouter/example_with_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 demonstrates the multithreading features of MavRouter.
# It shows how to run the router in the background and dynamically
# manage devices (add, remove, send messages) while it is running.

import time
from mavrouter import MavDevice, MavRouter, mavutil

# ---------------------------------------------------------
# Step 1: Define the devices and callbacks
# ---------------------------------------------------------

# We create just a few devices for this example
drone = MavDevice(address="/dev/ttyUSB0", listen_to=True)
gcs = MavDevice(address="192.168.1.10:14610", listen_to=True, is_input=False)
admin = MavDevice(address="127.0.0.1:14550", listen_to=True, is_input=False)

# Simple callback: Drone sends to GCS and Admin
def callback_drone(msg):
    gcs.send_msg(msg)
    admin.send_msg(msg)
drone.set_callback(callback_drone)

# Simple callback: GCS sends to Drone
def callback_gcs(msg):
    drone.send_msg(msg)
gcs.set_callback(callback_gcs)

# Admin has no callback for this example (default is null_callback)


# ---------------------------------------------------------
# Step 2: Initialize the router and add initial devices
# ---------------------------------------------------------

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

router.add_device(drone)
router.add_device(gcs)
router.add_device(admin)


# ---------------------------------------------------------
# Step 3: Run router in a background thread
# ---------------------------------------------------------

print("Starting router in the background...")
# This starts the routing loop but does not block the script!
router.start_routing_in_thread()


# ---------------------------------------------------------
# Step 4: Perform dynamic actions in the main thread
# ---------------------------------------------------------

try:
    print("Main script: Waiting 20 seconds...")
    time.sleep(20)

    # After 20 seconds, we remove the GCS from the router.
    # The GCS is disconnected and will no longer receive or route messages.
    print("Main script: Removing the GCS device...")
    router.remove_device(gcs)

    print("Main script: Waiting 10 seconds...")
    time.sleep(10)

    # We send a dummy MAVLink message (STATUSTEXT) to send to the admin
    # The send method is thread-safe, so it is safe to call it here
    # while the background thread is running
    print("Main script: Sending a manual text message to the Admin...")
    text_msg = "Hello Admin, GCS has been disconnected!"
    admin.master.mav.statustext_send(
        mavutil.mavlink.MAV_SEVERITY_INFO,
        text_msg.encode('utf-8')
    )

    print("Main script: Waiting 15 seconds...")
    time.sleep(15)

except Exception as e:
    print(f"Error caught: {e}")

except KeyboardInterrupt:
    print("Script interrupted by user.")

finally:
    # ---------------------------------------------------------
    # Step 5: Safely stop the routing thread
    # ---------------------------------------------------------
    print("Main script: Stopping the routing thread...")
    # This sets the stop event and waits for the thread to close cleanly
    router.stop_routing_in_thread()
    print("Main script: Done. Bye!")