Integrate clock synchronization across swarm agents (#81)

* Started integration of timesync

* Added timesync thread

* Added support for clock synchronization and ping measurement

* Clean up comments and docstrings

* Cleaned up implementation and examples for PR

Author: evan-palmer

examples/arming.py

@@ -55,11 +55,10 @@ def print_message_response_cb(future: Future) -> None:
                 f"({response.target_agent_id}): {response.code}"
             )
     else:
-        for response in responses:
-            print(
-                f"Result of {response.message_type} message sent to "
-                f"({response.target_agent_id}): {response.code}"
-            )
+        print(
+            f"Result of {responses.message_type} message sent to "
+            f"({responses.target_agent_id}): {responses.code}"
+        )
 
     return
 

examples/check_state.py

@@ -69,7 +69,8 @@ def main() -> None:
             agent = mavswarm.get_agent_by_id(agent_id)
 
             if agent is not None:
-                print(f"The current attitude of {agent} is: {agent.attitude}")
+                # print(f"The current attitude of {agent} is: {agent.attitude}")
+                pass
 
             time.sleep(0.5)
 

examples/companion_computer.py

@@ -65,11 +65,10 @@ def print_message_response_cb(future: Future) -> None:
                 f"({response.target_agent_id}): {response.code}"
             )
     else:
-        for response in responses:
-            print(
-                f"Result of {response.message_type} message sent to "
-                f"({response.target_agent_id}): {response.code}"
-            )
+        print(
+            f"Result of {responses.message_type} message sent to "
+            f"({responses.target_agent_id}): {responses.code}"
+        )
 
     return
 

examples/set_flight_mode.py

@@ -57,11 +57,10 @@ def print_message_response_cb(future: Future) -> None:
                 f"({response.target_agent_id}): {response.code}"
             )
     else:
-        for response in responses:
-            print(
-                f"Result of {response.message_type} message sent to "
-                f"({response.target_agent_id}): {response.code}"
-            )
+        print(
+            f"Result of {responses.message_type} message sent to "
+            f"({responses.target_agent_id}): {responses.code}"
+        )
 
     return
 

examples/set_home_position.py

@@ -56,11 +56,10 @@ def print_message_response_cb(future: Future) -> None:
                 f"({response.target_agent_id}): {response.code}"
             )
     else:
-        for response in responses:
-            print(
-                f"Result of {response.message_type} message sent to "
-                f"({response.target_agent_id}): {response.code}"
-            )
+        print(
+            f"Result of {responses.message_type} message sent to "
+            f"({responses.target_agent_id}): {responses.code}"
+        )
 
     return
 

pymavswarm/agent.py

@@ -16,6 +16,8 @@
 
 from __future__ import annotations
 
+from collections import deque
+from statistics import fmean
 from typing import Any
 
 import monotonic
@@ -110,6 +112,10 @@ def __init__(
         self.__hrl_state = swarm_state.Generic(
             "hrl_state", None, optional_context_props=context_props
         )
+        self.__ping = swarm_state.Generic(
+            "ping", 0, optional_context_props=context_props
+        )
+        self.__clock_offset: deque[int] = deque(maxlen=5)
 
         return
 
@@ -328,10 +334,43 @@ def hrl_state(self) -> Generic:
         """
         HRL state that the agent is in.
 
+        :return: hrl state
         :rtype: str
         """
         return self.__hrl_state
 
+    @property
+    def ping(self) -> Generic:
+        """
+        Agent latency [ms].
+
+        :return: latency
+        :rtype: Generic
+        """
+        return self.__ping
+
+    def update_clock_offset(self, offset: int) -> None:
+        """
+        Update the agent clock offset relative to the source clock.
+
+        :param offset: clock offset
+        :type offset: int
+        """
+        self.__clock_offset.append(offset)
+        return
+
+    @property
+    def clock_offset(self) -> int:
+        """
+        Average offset between the source clock and agent clock.
+
+        This property does not implement a watcher interface.
+
+        :return: clock offset
+        :rtype: int
+        """
+        return int(fmean(self.__clock_offset))
+
     def __str__(self) -> str:
         """
         Print agent information in a human-readable format.