From 99024a9561fe5ec81da188499989d2da181984c6 Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Tue, 28 May 2024 14:08:31 -0400 Subject: [PATCH 1/7] created data structure to hold local odometry data --- modules/drone_odometry_local.py | 73 +++++++++++++++++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 modules/drone_odometry_local.py diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py new file mode 100644 index 0000000..89782ca --- /dev/null +++ b/modules/drone_odometry_local.py @@ -0,0 +1,73 @@ + +import time + +from .common.mavlink.modules import drone_odometry + +class DronePositionLocal: + """ + Drone position using NED. + """ + + __create_key = object() + + @classmethod + def create( + cls, + north: float, + east: float, + down: float + ) -> "tuple[bool, DronePositionLocal | None]": + return True, DronePositionLocal(north, east, down) + + def __init__( + self, + create_key: object, + north: float, + east: float, + down: float + ) -> None: + + assert create_key is DronePositionLocal.__create_key, "Use create() method" + + self.north = north + self.east = east + self.down = down + + + +class DroneOdometryLocal: + """ + Data structure combining drone's local position, local orientation + and timestamp. + """ + + __create_key = object() + + @classmethod + def create( + cls, + drone_position: DronePositionLocal, + drone_orientation: drone_odometry.DroneOrientation + ) -> "tuple[bool, DroneOdometryLocal | None]": + + if drone_position is None or drone_orientation is None: + return False, None + + timestamp = time.time() + + return True, DroneOdometryLocal(drone_position, drone_orientation, timestamp) + + + def __init__( + self, + create_key: object, + drone_position: DronePositionLocal, + drone_orientation: drone_odometry.DroneOrientation, + timestamp: float + ) -> None: + + assert create_key is DroneOdometryLocal.__create_key, "Use create() method" + + self.drone_position = drone_position + self.drone_orientation = drone_orientation + self.timestamp = timestamp From 848ffd721263d48fd7e5b95ddfae922fe59fc6ec Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Tue, 28 May 2024 14:50:53 -0400 Subject: [PATCH 2/7] added create keys to ensure create method is used --- modules/drone_odometry_local.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py index 89782ca..d6cdeb4 100644 --- a/modules/drone_odometry_local.py +++ b/modules/drone_odometry_local.py @@ -17,7 +17,7 @@ def create( east: float, down: float ) -> "tuple[bool, DronePositionLocal | None]": - return True, DronePositionLocal(north, east, down) + return True, DronePositionLocal(cls.__create_key, north, east, down) def __init__( self, @@ -55,7 +55,7 @@ def create( timestamp = time.time() - return True, DroneOdometryLocal(drone_position, drone_orientation, timestamp) + return True, DroneOdometryLocal(cls.__create_key, drone_position, drone_orientation, timestamp) def __init__( From 453c38d94c38502cd5857f886af8e60ec02a401b Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 29 May 2024 15:01:41 -0400 Subject: [PATCH 3/7] added string representations --- modules/drone_odometry_local.py | 65 +++++++++++++++++---------------- 1 file changed, 33 insertions(+), 32 deletions(-) diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py index d6cdeb4..5b83854 100644 --- a/modules/drone_odometry_local.py +++ b/modules/drone_odometry_local.py @@ -1,8 +1,8 @@ - import time from .common.mavlink.modules import drone_odometry + class DronePositionLocal: """ Drone position using NED. @@ -12,27 +12,20 @@ class DronePositionLocal: @classmethod def create( - cls, - north: float, - east: float, - down: float - ) -> "tuple[bool, DronePositionLocal | None]": + cls, north: float, east: float, down: float + ) -> "tuple[bool, DronePositionLocal | None]": return True, DronePositionLocal(cls.__create_key, north, east, down) - - def __init__( - self, - create_key: object, - north: float, - east: float, - down: float - ) -> None: - + + def __init__(self, create_key: object, north: float, east: float, down: float) -> None: + assert create_key is DronePositionLocal.__create_key, "Use create() method" self.north = north self.east = east self.down = down + def __str__(self) -> str: + return f"{self.__class__}: North: {self.north}, East: {self.east}, Down: {self.down}." class DroneOdometryLocal: @@ -45,29 +38,37 @@ class DroneOdometryLocal: @classmethod def create( - cls, - drone_position: DronePositionLocal, - drone_orientation: drone_odometry.DroneOrientation - ) -> "tuple[bool, DroneOdometryLocal | None]": - - if drone_position is None or drone_orientation is None: + cls, local_position: DronePositionLocal, drone_orientation: drone_odometry.DroneOrientation + ) -> "tuple[bool, DroneOdometryLocal | None]": + + if local_position is None: return False, None - + + if drone_orientation is None: + return False, None + timestamp = time.time() - return True, DroneOdometryLocal(cls.__create_key, drone_position, drone_orientation, timestamp) - + return True, DroneOdometryLocal( + cls.__create_key, local_position, drone_orientation, timestamp + ) def __init__( - self, - create_key: object, - drone_position: DronePositionLocal, - drone_orientation: drone_odometry.DroneOrientation, - timestamp: float - ) -> None: - + self, + create_key: object, + local_position: DronePositionLocal, + drone_orientation: drone_odometry.DroneOrientation, + timestamp: float, + ) -> None: + assert create_key is DroneOdometryLocal.__create_key, "Use create() method" - self.drone_position = drone_position + self.local_position = local_position self.drone_orientation = drone_orientation self.timestamp = timestamp + + def __str__(self) -> str: + return f"{self.__class__},\ + {self.local_position}, \ + DroneOrientation: Roll: {self.drone_orientation.roll}, Pitch: {self.drone_orientation.pitch}, Yaw: {self.drone_orientation.yaw}.\ + Time: {self.timestamp}." From 3cd407e4681dd62392c610855623b71b60c45343 Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 29 May 2024 15:07:30 -0400 Subject: [PATCH 4/7] added module docstrings --- modules/drone_odometry_local.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py index 5b83854..19e47b8 100644 --- a/modules/drone_odometry_local.py +++ b/modules/drone_odometry_local.py @@ -1,3 +1,7 @@ +""" +Data structure for local odometry data (local position, orientation, and timestamp) +""" + import time from .common.mavlink.modules import drone_odometry @@ -14,6 +18,9 @@ class DronePositionLocal: def create( cls, north: float, east: float, down: float ) -> "tuple[bool, DronePositionLocal | None]": + """ + Local position (NED) + """ return True, DronePositionLocal(cls.__create_key, north, east, down) def __init__(self, create_key: object, north: float, east: float, down: float) -> None: @@ -40,6 +47,9 @@ class DroneOdometryLocal: def create( cls, local_position: DronePositionLocal, drone_orientation: drone_odometry.DroneOrientation ) -> "tuple[bool, DroneOdometryLocal | None]": + """ + Combines local odometry data with timestamp + """ if local_position is None: return False, None From cf262018e863fe9a740a3149facd3d9cdbfc8fed Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 29 May 2024 15:30:48 -0400 Subject: [PATCH 5/7] added function docstrings --- modules/drone_odometry_local.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py index 19e47b8..f49ff83 100644 --- a/modules/drone_odometry_local.py +++ b/modules/drone_odometry_local.py @@ -32,6 +32,9 @@ def __init__(self, create_key: object, north: float, east: float, down: float) - self.down = down def __str__(self) -> str: + """ + String representation + """ return f"{self.__class__}: North: {self.north}, East: {self.east}, Down: {self.down}." @@ -78,6 +81,9 @@ def __init__( self.timestamp = timestamp def __str__(self) -> str: + """ + String representation + """ return f"{self.__class__},\ {self.local_position}, \ DroneOrientation: Roll: {self.drone_orientation.roll}, Pitch: {self.drone_orientation.pitch}, Yaw: {self.drone_orientation.yaw}.\ From 45b3600392b1268ff736f4c8ccc820f691e501ce Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 29 May 2024 15:38:59 -0400 Subject: [PATCH 6/7] added init function docstrings --- modules/drone_odometry_local.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py index f49ff83..e57b9b7 100644 --- a/modules/drone_odometry_local.py +++ b/modules/drone_odometry_local.py @@ -24,7 +24,10 @@ def create( return True, DronePositionLocal(cls.__create_key, north, east, down) def __init__(self, create_key: object, north: float, east: float, down: float) -> None: - + """ + Private constructor, use create() method. + """ + assert create_key is DronePositionLocal.__create_key, "Use create() method" self.north = north @@ -73,6 +76,9 @@ def __init__( drone_orientation: drone_odometry.DroneOrientation, timestamp: float, ) -> None: + """ + Private constructor, use create() method. + """ assert create_key is DroneOdometryLocal.__create_key, "Use create() method" From 5a9cef1ff7fbf0ad35fb580f70d38ab3789acfaf Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 29 May 2024 15:42:19 -0400 Subject: [PATCH 7/7] reformatting --- modules/drone_odometry_local.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py index e57b9b7..1320990 100644 --- a/modules/drone_odometry_local.py +++ b/modules/drone_odometry_local.py @@ -27,7 +27,7 @@ def __init__(self, create_key: object, north: float, east: float, down: float) - """ Private constructor, use create() method. """ - + assert create_key is DronePositionLocal.__create_key, "Use create() method" self.north = north