prelim callback system impl

Author: i25052i

src_pymav/server/common/callback.py

@@ -0,0 +1,81 @@
+from pymavlink import mavutil
+
+class Callback():
+    def __init__(
+            self, 
+            name: str,
+            trigger_message_type: str, 
+            trigger_condition: function = (lambda curr_msg, prev_msg: True), 
+            payload: function = (lambda msg, conn, state: print(msg)),
+            only_once: bool = True
+        ):
+        """
+        A callback instance. Callbacks are composed of two components - the trigger, which controls
+        when and how the callback fires, and the payload, which contains the code to run. 
+        
+        Parameters:
+        `name`: A name to help disambiguate the callback.
+        `trigger_message_type`: mavlink message type that triggers the callback, 
+            e.g. `'GLOBAL_POSITION_INT'`
+        `trigger_condition`: a function that should take in mavlink messages of the designated 
+            trigger type and return a boolean that indicates whether or not the callback should
+            fire on that message. 
+            Also has access to the previous message of that time seen by the callback system. An 
+            example of how this can be used is triggering specifically on the transition between
+            waypoints 2 -> 3. 
+            Functions must be able to handle cases where prev_msg is `None`.
+            If unspecified, a function that will always return `True` will be used - i.e., the 
+            callback will fire on every message of the correct type.
+        `payload`: a function that contains the desired logic to run when the callback fires.
+            Has access to the message it was triggered on, the `mavlink_connection` object, and 
+            a state dictionary owned by the MPS server instance itself (for passing information
+            back to the wider server context.) 
+            If unspecified, a default function will be used that simply prints the message.
+        `only_once`: indicates that the callback should only execute once, i.e. it will be removed
+            after being triggered for the first and only time.
+        """
+
+        self.name = name
+        self.trigger_message_type = trigger_message_type
+        self.trigger_condition = trigger_condition
+        self.payload = payload
+        self.only_once = only_once
+
+class CallbackSystem():
+    def __init__(self, mav_connection: mavutil.mavfile, state: dict):
+        self.conn = mav_connection
+        self.state = state
+
+        self.callbacks: dict[str, list[Callback]] = {}
+        self.prev_messages: dict[str, object] = {}
+    
+    def update_and_check(self, latest_messages: dict):
+        # traverse through all callbacks and check each of their triggers
+        for callback_type, callback_list in self.callbacks.items():
+            prev_msg = self.prev_messages.get(callback_type, None)
+            curr_msg = latest_messages.get(callback_type, None)
+
+            if curr_msg == None:
+                # no point testing
+                continue
+
+            for callback in callback_list:
+                if callback.trigger_condition(curr_msg, prev_msg):
+
+                    if callback.only_once:
+                        # remove callback before firing it
+                        self.callbacks[callback_type].remove(callback)
+
+                    # fire callback
+                    callback.payload(curr_msg, self.conn, self.state)
+
+                    print(f"DEBUG: Fired Callback {callback.name}")
+
+        # update prev_messages
+        self.prev_messages.update(latest_messages)
+
+    def register_callback(self, callback: Callback):
+        self.callbacks[callback.trigger_message_type].append(callback)
+
+    # def unregister_callback(self, name: str, trigger_message_type: int):
+    #     pass

src_pymav/server/features/aeac_water_delivery.py

@@ -1,4 +1,5 @@
 from server.common.wpqueue import WaypointQueue, Waypoint
+from server.common.callback import CallbackSystem, Callback
 from server.operations.rc_channel_cmd import send_rc_channel_value
 from pymavlink import mavutil
 
@@ -14,6 +15,7 @@
 '''
 def generate_water_wps(
     mav_connection: mavutil.mavfile,
+    callback_sys: CallbackSystem,
     current_alt: float,
     deliver_alt: float,
     deliver_duration_secs: int,
@@ -32,6 +34,16 @@ def generate_water_wps(
     )
 
     # TODO how do we send a message here?
+    
+    # TODO test: setting up a callback to trigger on waypoint 2
+    callback_sys.register_callback(Callback(
+        "Water Delivery Callback",
+        'MISSION_CURRENT',
+        lambda curr_msg, prev_msg: (curr_msg.seq == 2),
+        lambda msg, conn, state: send_payload_command(conn, 0, 'TODO'), # TODO !!!
+        True
+    ))
+
     wp_2 = Waypoint(
         "stay",
         "curr_wp",

src_pymav/server/httpserver.py

@@ -18,11 +18,17 @@
 from server.common.wpqueue import WaypointQueue, Waypoint
 from server.common.status import Status
 from server.common.encoders import command_string_to_int, command_int_to_string
+from server.common.callback import CallbackSystem
 
 
 class HTTP_Server:
     def __init__(self, mav_connection):
         self.mav_connection: mavfile = mav_connection
+        self.miscellaneous_state = {}
+
+        self.callback_sys = CallbackSystem(self.mav_connection, self.miscellaneous_state)
+
+        # TODO Handle Camera Protocol via Callbacks?
 
     def serve_forever(self, production=True, HOST="localhost", PORT=9000):
         print("GCOM HTTP Server starting...")
@@ -36,7 +42,7 @@ def index():
 
         @app.route("/queue", methods=["GET"])
         def get_queue():
-            curr = get_status(self.mav_connection)._wpn 
+            curr = get_status(self.mav_connection, self.callback_sys)._wpn 
             wpq = get_current_mission(self.mav_connection)
 
             formatted = []
@@ -56,7 +62,7 @@ def get_queue():
         def post_queue():
             payload = request.get_json()
 
-            ret = get_status(self.mav_connection)
+            ret = get_status(self.mav_connection, self.callback_sys)
             last_altitude = ret.as_dictionary().get("altitude", 50)
 
             wpq = []
@@ -103,7 +109,7 @@ def post_queue():
         def post_insert_wp():
             payload = request.get_json()
 
-            ret: Status = get_status(self.mav_connection)
+            ret: Status = get_status(self.mav_connection, self.callback_sys)
             last_altitude = ret._alt if ret != () else 50
 
             curr = max(ret._wpn, 1)
@@ -165,7 +171,7 @@ def get_clear_queue():
         @app.route("/status", methods=["GET"])
         def get_status_handler():
             print("Status sent to GCOM")
-            s = get_status(self.mav_connection).as_dictionary()
+            s = get_status(self.mav_connection, self.callback_sys).as_dictionary()
             return s, 200
 
         @app.route("/takeoff", methods=["POST"])
@@ -330,7 +336,7 @@ def deliver_water_down():
                 deliver_duration_secs = input["deliver_duration_secs"]
                 curr_lat = input["curr_lat"]
                 curr_lon = input["curr_lon"]
-                wpq = generate_water_wps(self.mav_connection, current_alt, deliver_alt, deliver_duration_secs, curr_lat, curr_lon)
+                wpq = generate_water_wps(self.mav_connection, self.callback_sys, current_alt, deliver_alt, deliver_duration_secs, curr_lat, curr_lon)
 
 
                 if new_mission(self.mav_connection, wpq):

src_pymav/server/operations/get_info.py

@@ -5,14 +5,16 @@
 from server.common.status import Status
 from server.common.wpqueue import WaypointQueue, Waypoint
 from server.common.encoders import command_int_to_string
+from server.common.callback import CallbackSystem
 from server.utilities.request_message_streaming import request_messages
 
+
 """
     Get current status of a drone
     Type of message can be found on https://mavlink.io/en/messages/common.html
 
 """
-def get_status(mav_connection: mavutil.mavfile) -> Status:
+def get_status(mav_connection: mavutil.mavfile, callback_sys: CallbackSystem = None) -> Status:
 
     # trigger an update
     # mav_connection.recv_match(blocking=True)
@@ -57,6 +59,9 @@ def get_status(mav_connection: mavutil.mavfile) -> Status:
     winddirection = math.degrees(math.atan(status_wind.wind_x / status_wind.wind_y)) if status_wind.wind_y != 0 else (0 if status_wind.wind_x > 0 else 180)
     windvelocity = math.sqrt(status_wind.wind_x * status_wind.wind_x + status_wind.wind_y * status_wind.wind_y)
 
+    # trigger / callback mechanism
+    callback_sys.update_and_check(mav_connection.messages)
+
     return Status(
         system_time.time_unix_usec / 1000000, # seconds