From e54ad5c917f71c9917303cadc7a1e540546862de Mon Sep 17 00:00:00 2001
From: Evan <57083990+Evang264@users.noreply.github.com>
Date: Wed, 30 Oct 2024 14:16:25 -0400
Subject: [PATCH] Move local_global_conversion from computer-vision (#54)

* Move local_global_conversion from computer-vision

* Moved local_global_conversion.py and drone_odometry_local.py from
  computer-vision-python repo to here
* Moved the files to under mavlink/modules

* Move the files under kml/modules instead

Moved
* local_global_conversion.py
* drone_odometry_local.py
from mavlink/modules to kml/modules
---
 kml/modules/drone_odometry_local.py    | 133 +++++++++++++++++++++++++
 kml/modules/local_global_conversion.py | 100 +++++++++++++++++++
 2 files changed, 233 insertions(+)
 create mode 100644 kml/modules/drone_odometry_local.py
 create mode 100644 kml/modules/local_global_conversion.py

diff --git a/kml/modules/drone_odometry_local.py b/kml/modules/drone_odometry_local.py
new file mode 100644
index 0000000..e6f1c33
--- /dev/null
+++ b/kml/modules/drone_odometry_local.py
@@ -0,0 +1,133 @@
+"""
+Drone odometry in local space (origin at home location).
+"""
+
+from ...mavlink.modules import drone_odometry
+
+
+class DronePositionLocal:
+    """
+    Drone position in NED system.
+    """
+
+    __create_key = object()
+
+    @classmethod
+    def create(
+        cls, north: float, east: float, down: float
+    ) -> "tuple[bool, DronePositionLocal | None]":
+        """
+        North, east, down in metres.
+        """
+        return True, DronePositionLocal(cls.__create_key, north, east, down)
+
+    def __init__(
+        self, class_private_create_key: object, north: float, east: float, down: float
+    ) -> None:
+        """
+        Private constructor, use create() method.
+        """
+        assert class_private_create_key is DronePositionLocal.__create_key, "Use create() method"
+
+        self.north = north
+        self.east = east
+        self.down = down
+
+    def __str__(self) -> str:
+        """
+        To string.
+        """
+        return f"DronePositionLocal (NED): {self.north}, {self.east}, {self.down}"
+
+
+class DroneOrientationLocal:
+    """
+    Wrapper for DroneOrientation as it is the same in both local and global space.
+    """
+
+    __create_key = object()
+
+    @classmethod
+    def create_new(
+        cls, yaw: float, pitch: float, roll: float
+    ) -> "tuple[bool, DroneOrientationLocal | None]":
+        """
+        Yaw, pitch, roll in radians.
+        """
+        result, orientation = drone_odometry.DroneOrientation.create(yaw, pitch, roll)
+        if not result:
+            return False, None
+
+        # Get Pylance to stop complaining
+        assert orientation is not None
+
+        return True, DroneOrientationLocal(cls.__create_key, orientation)
+
+    @classmethod
+    def create_wrap(
+        cls, orientation: drone_odometry.DroneOrientation
+    ) -> "tuple[bool, DroneOrientationLocal | None]":
+        """
+        Wrap existing orientation.
+        """
+        return True, DroneOrientationLocal(cls.__create_key, orientation)
+
+    def __init__(
+        self, class_private_create_key: object, orientation: drone_odometry.DroneOrientation
+    ) -> None:
+        """
+        Private constructor, use create() method.
+        """
+        assert class_private_create_key is DroneOrientationLocal.__create_key, "Use create() method"
+
+        self.orientation = orientation
+
+    def __str__(self) -> str:
+        """
+        To string.
+        """
+        # TODO: Update common
+        return f"DroneOrientationLocal (YPR rad): {self.orientation.yaw}, {self.orientation.pitch}, {self.orientation.roll}"
+
+
+class DroneOdometryLocal:
+    """
+    Wrapper for DronePositionLocal and DroneOrientationLocal.
+    """
+
+    __create_key = object()
+
+    @classmethod
+    def create(
+        cls, position: DronePositionLocal, orientation: DroneOrientationLocal
+    ) -> "tuple[bool, DroneOdometryLocal | None]":
+        """
+        Position and orientation in one class.
+        """
+        if position is None:
+            return False, None
+
+        if orientation is None:
+            return False, None
+
+        return True, DroneOdometryLocal(cls.__create_key, position, orientation)
+
+    def __init__(
+        self,
+        class_private_create_key: object,
+        position: DronePositionLocal,
+        orientation: DroneOrientationLocal,
+    ) -> None:
+        """
+        Private constructor, use create() method.
+        """
+        assert class_private_create_key is DroneOdometryLocal.__create_key, "Use create() method"
+
+        self.position = position
+        self.orientation = orientation
+
+    def __str__(self) -> str:
+        """
+        To string.
+        """
+        return f"DroneOdometryLocal: {self.position}, {self.orientation}"
diff --git a/kml/modules/local_global_conversion.py b/kml/modules/local_global_conversion.py
new file mode 100644
index 0000000..d125a5a
--- /dev/null
+++ b/kml/modules/local_global_conversion.py
@@ -0,0 +1,100 @@
+"""
+Conversion between local and global space.
+"""
+
+import pymap3d as pm
+
+from . import drone_odometry_local
+from ...mavlink.modules import drone_odometry
+
+
+def drone_position_global_from_local(
+    home_location: drone_odometry.DronePosition,
+    drone_position_local: drone_odometry_local.DronePositionLocal,
+) -> "tuple[bool, drone_odometry.DronePosition | None]":
+    """
+    Local coordinates to global coordinates.
+    Return: Drone position in WGS 84.
+    """
+    latitude, longitude, altitude = pm.ned2geodetic(
+        drone_position_local.north,
+        drone_position_local.east,
+        drone_position_local.down,
+        home_location.latitude,
+        home_location.longitude,
+        home_location.altitude,
+    )
+
+    result, drone_position = drone_odometry.DronePosition.create(
+        latitude,
+        longitude,
+        altitude,
+    )
+    if not result:
+        return False, None
+
+    # Get Pylance to stop complaining
+    assert drone_position is not None
+
+    return True, drone_position
+
+
+def __drone_position_local_from_global(
+    home_location: drone_odometry.DronePosition, drone_position: drone_odometry.DronePosition
+) -> "tuple[bool, drone_odometry_local.DronePositionLocal | None]":
+    """
+    Global coordinates to local coordinates.
+    Return: Drone position relative to home location (NED system).
+    """
+    north, east, down = pm.geodetic2ned(
+        drone_position.latitude,
+        drone_position.longitude,
+        drone_position.altitude,
+        home_location.latitude,
+        home_location.longitude,
+        home_location.altitude,
+    )
+
+    result, drone_position_local = drone_odometry_local.DronePositionLocal.create(
+        north,
+        east,
+        down,
+    )
+    if not result:
+        return False, None
+
+    # Get Pylance to stop complaining
+    assert drone_position_local is not None
+
+    return True, drone_position_local
+
+
+def drone_odometry_local_from_global(
+    odometry: drone_odometry.DroneOdometry, home_location: drone_odometry.DronePosition
+) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
+    """
+    Converts global odometry to local.
+    """
+    result, drone_position_local = __drone_position_local_from_global(
+        home_location,
+        odometry.position,
+    )
+    if not result:
+        return False, None
+
+    # Get Pylance to stop complaining
+    assert drone_position_local is not None
+
+    result, drone_orientation_local = drone_odometry_local.DroneOrientationLocal.create_wrap(
+        odometry.orientation,
+    )
+    if not result:
+        return False, None
+
+    # Get Pylance to stop complaining
+    assert drone_orientation_local is not None
+
+    return drone_odometry_local.DroneOdometryLocal.create(
+        drone_position_local,
+        drone_orientation_local,
+    )