Added support for local and relative frames (#109)

* Added support for local and relative frames

* Removed unused variables

* Remove broken attempt at using frames in collision avoidance

* Remove stale print statement

* Fixed unit tests

* Cleaned up clock synchronization error

* Updated stale docstring

* Update stale docstrings

* Resolve stale argument names

Author: evan-palmer

examples/collision_prevention.py

@@ -150,7 +150,9 @@ def main() -> None:
     # Command the agent to the target location
     # We don't need to specify the target agents because these are captured from the
     # config file
-    future = mavswarm.goto(config_file=args.config, retry=True)
+    future = mavswarm.goto(
+        config_file=args.config, retry=True, frame=MavSwarm.GLOBAL_RELATIVE_FRAME
+    )
     future.add_done_callback(print_message_response_cb)
 
     while not future.done():

examples/goto.py

@@ -146,7 +146,9 @@ def main() -> None:
     # Command the agent to the target location
     # We don't need to specify the target agents because these are captured from the
     # config file
-    future = mavswarm.goto(config_file=args.config, retry=True)
+    future = mavswarm.goto(
+        config_file=args.config, retry=True, frame=MavSwarm.GLOBAL_RELATIVE_FRAME
+    )
     future.add_done_callback(print_message_response_cb)
 
     while not future.done():

pymavswarm/agent.py

@@ -16,12 +16,11 @@
 
 from __future__ import annotations
 
-from collections import deque
 from datetime import datetime
-from statistics import fmean
 from typing import Any
 
 import monotonic
+from pymavlink import mavutil
 
 import pymavswarm.state as swarm_state
 from pymavswarm.mission import SwarmMission
@@ -81,23 +80,16 @@ def __init__(
         self.__gps_info = swarm_state.GPSInfo(
             0.0, 0.0, 0, 0, optional_context_props=context_props
         )
-        self.__position = swarm_state.Position(
-            0.0, 0.0, 0.0, optional_context_props=context_props
-        )
+        self.__position = swarm_state.Position(optional_context_props=context_props)
         self.__ekf = swarm_state.EKFStatus(
             0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, optional_context_props=context_props
         )
         self.__telemetry = swarm_state.Telemetry(
             0.0, optional_context_props=context_props
         )
-        self.__velocity = swarm_state.Velocity(
-            0.0, 0.0, 0.0, optional_context_props=context_props
-        )
-        self.__prev_velocity = swarm_state.Velocity(
-            0.0, 0.0, 0.0, optional_context_props=context_props
-        )
+        self.__velocity = swarm_state.Velocity(optional_context_props=context_props)
         self.__acceleration = swarm_state.Acceleration(
-            0.0, 0.0, 0.0, optional_context_props=context_props
+            optional_context_props=context_props
         )
         self.__armed = swarm_state.Generic(
             "armed", False, optional_context_props=context_props
@@ -129,23 +121,23 @@ def __init__(
         self.__last_params_read = swarm_state.ParameterList(
             max_length=max_params_stored, optional_context_props=context_props
         )
-        self.__home_position = swarm_state.Position(
-            0.0, 0.0, 0.0, optional_context_props=context_props
+        self.__home_position = swarm_state.Vector(
+            0.0,
+            0.0,
+            0.0,
+            mavutil.mavlink.MAV_FRAME_GLOBAL,
+            0.0,
+            optional_context_props=context_props,
         )
         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.__last_position_message_timestamp = swarm_state.Generic(
-            "time_boot_ms", 0, optional_context_props=context_props
+        self.__clock_offset = swarm_state.Generic(
+            "clock_offset", 0, optional_context_props=context_props
         )
-        self.__clock_offset: deque[int] = deque(maxlen=5)
-
-        # Initialize the clock offset with a value so that we have something to access
-        # at boot
-        self.__clock_offset.append(0)
 
         return
 
@@ -249,16 +241,6 @@ def velocity(self) -> swarm_state.Velocity:
         """
         return self.__velocity
 
-    @property
-    def previous_velocity(self) -> swarm_state.Velocity:
-        """
-        Velocity of the agent at the previous measurement.
-
-        return: agent's previous velocity
-        :rtype: Velocity
-        """
-        return self.__prev_velocity
-
     @property
     def acceleration(self) -> swarm_state.Acceleration:
         """
@@ -370,7 +352,7 @@ def last_params_read(self) -> swarm_state.ParameterList:
         return self.__last_params_read
 
     @property
-    def home_position(self) -> swarm_state.Position:
+    def home_position(self) -> swarm_state.Vector:
         """
         Home position of the agent.
 
@@ -400,36 +382,14 @@ def ping(self) -> Generic:
         return self.__ping
 
     @property
-    def last_position_message_timestamp(self) -> Generic:
-        """
-        Most recent time that the agent sent the position message in the global clock.
-
-        :return: time since boot [ms]
-        :rtype: Generic
+    def clock_offset(self) -> Generic:
         """
-        return self.__last_position_message_timestamp
-
-    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.
+        Clock offset from the global clock.
 
         :return: clock offset
-        :rtype: int
+        :rtype: Generic
         """
-        return int(fmean(self.__clock_offset))
+        return self.__clock_offset
 
     def compute_reachable_set(
         self,
@@ -438,11 +398,14 @@ def compute_reachable_set(
         reach_time: float,
         initial_step_size: float = 0.5,
         reach_timeout: float = 0.001,
-        uses_lat_lon: bool = True,
     ) -> tuple[HyperRectangle, float]:
         """
         Compute the current reachable set of the agent.
 
+        In the current state of the system, the reachable set is only computed using
+        the global frame. In the future, if transformations between frames is
+        implemented, reachable set computation for multiple frames may be supported.
+
         :param position_error: 3D position error to account for in the measurement
         :type position_error: float
         :param velocity_error: 3D velocity error to account for in the measurement
@@ -460,103 +423,71 @@ def compute_reachable_set(
             from the start time
         :rtype: tuple[HyperRectangle, float]
         """
-        if uses_lat_lon:
-            rect = HyperRectangle(
-                [
-                    # When computing the reachable state for the GPS position, we pass
-                    # the origin as the position. After computing what the reachable
-                    # change, we correct the original lat/lon position. This
-                    # helps us eliminate *some* error that occurs during
-                    # conversions.
-                    Interval(-position_error, position_error),
-                    Interval(-position_error, position_error),
-                    Interval(
-                        self.position.z - position_error,
-                        self.position.z + position_error,
-                    ),
-                    Interval(
-                        self.velocity.x - velocity_error,
-                        self.velocity.x + velocity_error,
-                    ),
-                    Interval(
-                        self.velocity.y - velocity_error,
-                        self.velocity.y + velocity_error,
-                    ),
-                    Interval(
-                        self.velocity.z - velocity_error,
-                        self.velocity.z + velocity_error,
-                    ),
-                ]
-            )
-        else:
-            rect = HyperRectangle(
-                [
-                    Interval(
-                        self.position.x - position_error,
-                        self.position.x + position_error,
-                    ),
-                    Interval(
-                        self.position.y - position_error,
-                        self.position.y + position_error,
-                    ),
-                    Interval(
-                        self.position.z - position_error,
-                        self.position.z + position_error,
-                    ),
-                    Interval(
-                        self.velocity.x - velocity_error,
-                        self.velocity.x + velocity_error,
-                    ),
-                    Interval(
-                        self.velocity.y - velocity_error,
-                        self.velocity.y + velocity_error,
-                    ),
-                    Interval(
-                        self.velocity.z - velocity_error,
-                        self.velocity.z + velocity_error,
-                    ),
-                ]
-            )
+        # When computing the reachable state for the GPS position, we pass
+        # the origin as the position. After computing what the reachable
+        # change, we correct the original lat/lon position. This
+        # helps us eliminate *some* error that occurs during
+        # conversions.
+        rect = HyperRectangle(
+            [
+                Interval(-position_error, position_error),
+                Interval(-position_error, position_error),
+                Interval(
+                    self.position.global_frame.z - position_error,
+                    self.position.global_frame.z + position_error,
+                ),
+                Interval(
+                    self.velocity.global_frame.x - velocity_error,
+                    self.velocity.global_frame.x + velocity_error,
+                ),
+                Interval(
+                    self.velocity.global_frame.y - velocity_error,
+                    self.velocity.global_frame.y + velocity_error,
+                ),
+                Interval(
+                    self.velocity.global_frame.z - velocity_error,
+                    self.velocity.global_frame.z + velocity_error,
+                ),
+            ]
+        )
 
         # Compute the sender's reachable state
         (
             reachable_state,
             reach_time_elapsed,
         ) = SafetyChecker.face_lifting_iterative_improvement(
             rect,
-            self.last_position_message_timestamp.value,
+            self.position.global_frame.timestamp,
             (
-                self.acceleration.x,
-                self.acceleration.y,
-                self.acceleration.z,
+                self.acceleration.global_frame.x,
+                self.acceleration.global_frame.y,
+                self.acceleration.global_frame.z,
             ),
             reach_time,
             initial_step_size=initial_step_size,
             timeout=reach_timeout,
         )
 
-        if uses_lat_lon:
-            # Correct the latitude using the reachable positions
-            reachable_state.intervals[0].interval_min = latitude_conversion(
-                self.position.x,
-                reachable_state.intervals[0].interval_min,
-            )
-            reachable_state.intervals[0].interval_max = latitude_conversion(
-                self.position.x,
-                reachable_state.intervals[0].interval_max,
-            )
-
-            # Correct the longitude using the reachable positions
-            reachable_state.intervals[1].interval_min = longitude_conversion(
-                self.position.x,
-                self.position.y,
-                reachable_state.intervals[1].interval_min,
-            )
-            reachable_state.intervals[1].interval_max = longitude_conversion(
-                self.position.x,
-                self.position.y,
-                reachable_state.intervals[1].interval_max,
-            )
+        # Correct the positions to use latitude and longitude
+        reachable_state.intervals[0].interval_min = latitude_conversion(
+            self.position.global_frame.x,
+            reachable_state.intervals[0].interval_min,
+        )
+        reachable_state.intervals[0].interval_max = latitude_conversion(
+            self.position.global_frame.x,
+            reachable_state.intervals[0].interval_max,
+        )
+
+        reachable_state.intervals[1].interval_min = longitude_conversion(
+            self.position.global_frame.x,
+            self.position.global_frame.y,
+            reachable_state.intervals[1].interval_min,
+        )
+        reachable_state.intervals[1].interval_max = longitude_conversion(
+            self.position.global_frame.x,
+            self.position.global_frame.y,
+            reachable_state.intervals[1].interval_max,
+        )
 
         return reachable_state, reach_time_elapsed