From 28800690ed42eff3981124d0e5cfbe120092108f Mon Sep 17 00:00:00 2001 From: Xierumeng <22666760+Xierumeng@users.noreply.github.com> Date: Mon, 11 Nov 2024 18:33:05 -0800 Subject: [PATCH] Consolidate coordinate classes (#59) * Consolidate coordinate classes * Linting and formatting --- ...ocations_to_kml.py => locations_to_kml.py} | 19 +-- modules/kml/named_location_global.py | 56 --------- modules/location_global.py | 43 +++++++ modules/location_local.py | 49 +++++++- modules/position_global.py | 53 ++++++++ modules/position_global_relative_altitude.py | 117 ++++++++++++++++++ modules/position_local.py | 54 +++++++- .../test_flight_controller_mission_ended.py | 79 ++++++++---- tests/unit/test_ground_locations_to_kml.py | 28 ++--- 9 files changed, 389 insertions(+), 109 deletions(-) rename modules/kml/{ground_locations_to_kml.py => locations_to_kml.py} (70%) delete mode 100644 modules/kml/named_location_global.py create mode 100644 modules/position_global_relative_altitude.py diff --git a/modules/kml/ground_locations_to_kml.py b/modules/kml/locations_to_kml.py similarity index 70% rename from modules/kml/ground_locations_to_kml.py rename to modules/kml/locations_to_kml.py index 55f9085..76a32b9 100644 --- a/modules/kml/ground_locations_to_kml.py +++ b/modules/kml/locations_to_kml.py @@ -7,14 +7,14 @@ import simplekml -from . import named_location_global +from .. import location_global -def ground_locations_to_kml( - ground_locations: list[named_location_global.NamedLocationGlobal], +def named_locations_to_kml( + named_locations: list[location_global.NamedLocationGlobal], document_name_prefix: str, save_directory: pathlib.Path, -) -> "tuple[bool, pathlib.Path | None]": +) -> tuple[True, pathlib.Path] | tuple[False, None]: """ Generates a KML file from a list of ground locations. @@ -26,10 +26,10 @@ def ground_locations_to_kml( """ kml = simplekml.Kml() - for i, ground_location in enumerate(ground_locations): - ground_location_name = f"Point {i + 1}: {ground_location.name}" - latitude = ground_location.location.latitude - longitude = ground_location.location.longitude + for i, named_location in enumerate(named_locations): + ground_location_name = f"Point {i + 1}: {named_location.name}" + latitude = named_location.latitude + longitude = named_location.longitude # Coordinates are in the order: longitude, latitude, optional height kml.newpoint(name=ground_location_name, coords=[(longitude, latitude)]) @@ -39,9 +39,10 @@ def ground_locations_to_kml( try: kml.save(str(kml_file_path)) - return True, kml_file_path # Required for catching library exceptions # pylint: disable-next=broad-exception-caught except Exception as exception: print(f"Error while saving KML file: {exception}") return False, None + + return True, kml_file_path diff --git a/modules/kml/named_location_global.py b/modules/kml/named_location_global.py deleted file mode 100644 index df6217c..0000000 --- a/modules/kml/named_location_global.py +++ /dev/null @@ -1,56 +0,0 @@ -""" -Location local with name. -""" - -from .. import location_global - - -class NamedLocationGlobal: - """ - Location local with name. - """ - - __create_key = object() - - @classmethod - def create( - cls, name: str, latitude: float, longitude: float - ) -> "tuple[True, NamedLocationGlobal] | tuple[False, None]": - """ - Name: Name of location. Cannot be empty. - Latitude: Decimal degrees. - Longitude: Decimal degrees. - - Return: Success, object. - """ - if name == "": - return False, None - - result, location = location_global.LocationGlobal.create(latitude, longitude) - if not result: - return False, None - - return True, NamedLocationGlobal(cls.__create_key, name, location) - - def __init__( - self, class_private_create_key: object, name: str, location: location_global.LocationGlobal - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is NamedLocationGlobal.__create_key, "Use create() method." - - self.name = name - self.location = location - - def __str__(self) -> str: - """ - To string. - """ - return f"{self.__class__}: {self.name}, location: {self.location}" - - def __repr__(self) -> str: - """ - For collections (e.g. list). - """ - return str(self) diff --git a/modules/location_global.py b/modules/location_global.py index 7fcb12b..12e4ff5 100644 --- a/modules/location_global.py +++ b/modules/location_global.py @@ -1,5 +1,7 @@ """ Location on ground in WGS 84. + +Class with name also available. """ @@ -42,3 +44,44 @@ def __repr__(self) -> str: For collections (e.g. list). """ return str(self) + + +class NamedLocationGlobal(LocationGlobal): + """ + Named LocationGlobal. + """ + + __create_key = object() + + @classmethod + # Additional argument for name + # pylint: disable-next=arguments-differ + def create( + cls, name: str, latitude: float, longitude: float + ) -> "tuple[True, NamedLocationGlobal] | tuple[False, None]": + """ + name: Can be empty. + latitude: Decimal degrees. + longitude: Decimal degrees. + + Return: Success, object. + """ + return True, NamedLocationGlobal(cls.__create_key, name, latitude, longitude) + + def __init__( + self, class_private_create_key: object, name: str, latitude: float, longitude: float + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is NamedLocationGlobal.__create_key, "Use create() method." + + super().__init__(super()._LocationGlobal__create_key, latitude, longitude) + + self.name = name + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}: name: {self.name}, latitude: {self.latitude}, longitude: {self.longitude}" diff --git a/modules/location_local.py b/modules/location_local.py index d894a20..9130543 100644 --- a/modules/location_local.py +++ b/modules/location_local.py @@ -1,11 +1,13 @@ """ Location on the ground in local Euclidean space (origin at home position global). + +Class with name also available. """ class LocationLocal: """ - Location in NED system, with down = 0.0 . + Location in NED system relative to home position, with down = 0.0 . """ __create_key = object() @@ -13,8 +15,8 @@ class LocationLocal: @classmethod def create(cls, north: float, east: float) -> "tuple[True, LocationLocal] | tuple[False, None]": """ - North: Metres. - East: Metres. + north: Metres. + east: Metres. Return: Success, object. """ @@ -40,3 +42,44 @@ def __repr__(self) -> str: For collections (e.g. list). """ return str(self) + + +class NamedLocationLocal(LocationLocal): + """ + Named LocationLocal. + """ + + __create_key = object() + + @classmethod + # Additional argument for name + # pylint: disable-next=arguments-differ + def create( + cls, name: str, north: float, east: float + ) -> "tuple[True, NamedLocationLocal] | tuple[False, None]": + """ + name: Can be empty. + north: Metres. + east: Metres. + + Return: Success, object. + """ + return True, NamedLocationLocal(cls.__create_key, name, north, east) + + def __init__( + self, class_private_create_key: object, name: str, north: float, east: float + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is NamedLocationLocal.__create_key, "Use create() method." + + super().__init__(super()._LocationLocal__create_key, north, east) + + self.name = name + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}: name: {self.name}, north: {self.north}, east: {self.east}" diff --git a/modules/position_global.py b/modules/position_global.py index ea4f822..a34357c 100644 --- a/modules/position_global.py +++ b/modules/position_global.py @@ -1,5 +1,7 @@ """ 3D position in WGS 84. + +Class with name also available. """ @@ -46,3 +48,54 @@ def __repr__(self) -> str: For collections (e.g. list). """ return str(self) + + +class NamedPositionGlobal(PositionGlobal): + """ + Named PositionGlobal. + """ + + __create_key = object() + + @classmethod + # Additional argument for name + # pylint: disable-next=arguments-differ + def create( + cls, + name: str, + latitude: float, + longitude: float, + altitude: float, + ) -> "tuple[True, NamedPositionGlobal] | tuple[False, None]": + """ + name: Can be empty. + latitude: Decimal degrees. + longitude: Decimal degrees. + altitude: Metres above mean sea level (MSL). Can be negative. + + Return: Success, object. + """ + return True, NamedPositionGlobal(cls.__create_key, name, latitude, longitude, altitude) + + def __init__( + self, + class_private_create_key: object, + name: str, + latitude: float, + longitude: float, + altitude: float, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is NamedPositionGlobal.__create_key, "Use create() method." + + super().__init__(super()._PositionGlobal__create_key, latitude, longitude, altitude) + + self.name = name + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}: name: {self.name}, latitude: {self.latitude}, longitude: {self.longitude}, altitude: {self.altitude}" diff --git a/modules/position_global_relative_altitude.py b/modules/position_global_relative_altitude.py new file mode 100644 index 0000000..8243cc9 --- /dev/null +++ b/modules/position_global_relative_altitude.py @@ -0,0 +1,117 @@ +""" +3D position in WGS 84. + +Class with name also available. +""" + + +class PositionGlobalRelativeAltitude: + """ + WGS 84 following ISO 6709 (latitude before longitude). + + Relative altitude to home position. + """ + + __create_key = object() + + @classmethod + def create( + cls, latitude: float, longitude: float, relative_altitude: float + ) -> "tuple[True, PositionGlobalRelativeAltitude] | tuple[False, None]": + """ + latitude: Decimal degrees. + longitude: Decimal degrees. + relative_altitude: Metres above home position. Can be negative. + + Return: Success, object. + """ + return True, PositionGlobalRelativeAltitude( + cls.__create_key, latitude, longitude, relative_altitude + ) + + def __init__( + self, + class_private_create_key: object, + latitude: float, + longitude: float, + relative_altitude: float, + ) -> None: + """ + Private constructor, use create() method. + """ + assert ( + class_private_create_key is PositionGlobalRelativeAltitude.__create_key + ), "Use create() method." + + self.latitude = latitude + self.longitude = longitude + self.relative_altitude = relative_altitude + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}: latitude: {self.latitude}, longitude: {self.longitude}, relative altitude: {self.relative_altitude}" + + def __repr__(self) -> str: + """ + For collections (e.g. list). + """ + return str(self) + + +class NamedPositionGlobalRelativeAltitude(PositionGlobalRelativeAltitude): + """ + Named PositionGlobalRelativeAltitude. + """ + + __create_key = object() + + @classmethod + # Additional argument for name + # pylint: disable-next=arguments-differ + def create( + cls, + name: str, + latitude: float, + longitude: float, + relative_altitude: float, + ) -> "tuple[True, NamedPositionGlobalRelativeAltitude] | tuple[False, None]": + """ + name: Can be empty. + latitude: Decimal degrees. + longitude: Decimal degrees. + relative_altitude: Metres above home position. Can be negative. + + Return: Success, object. + """ + return True, NamedPositionGlobalRelativeAltitude( + cls.__create_key, name, latitude, longitude, relative_altitude + ) + + def __init__( + self, + class_private_create_key: object, + name: str, + latitude: float, + longitude: float, + relative_altitude: float, + ) -> None: + """ + Private constructor, use create() method. + """ + assert ( + class_private_create_key is NamedPositionGlobalRelativeAltitude.__create_key + ), "Use create() method." + + super().__init__( + super()._PositionGlobal__create_key, latitude, longitude, relative_altitude + ) + + self.name = name + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}: name: {self.name}, latitude: {self.latitude}, longitude: {self.longitude}, relative_altitude: {self.relative_altitude}" diff --git a/modules/position_local.py b/modules/position_local.py index d46e690..44dafee 100644 --- a/modules/position_local.py +++ b/modules/position_local.py @@ -5,7 +5,7 @@ class PositionLocal: """ - Position in NED system. + Position in NED system relative to home position. """ __create_key = object() @@ -15,9 +15,9 @@ def create( cls, north: float, east: float, down: float ) -> "tuple[True, PositionLocal] | tuple[False, None]": """ - North: Metres. - East: Metres. - Down: Metres. Allowed to be positive, which is below the home position. + north: Metres. + east: Metres. + down: Metres. Allowed to be positive, which is below the home position. Return: Success, object. """ @@ -46,3 +46,49 @@ def __repr__(self) -> str: For collections (e.g. list). """ return str(self) + + +class NamedPositionLocal(PositionLocal): + """ + Named PositionLocal. + """ + + __create_key = object() + + @classmethod + # Additional argument for name + # pylint: disable-next=arguments-differ + def create( + cls, + name: str, + north: float, + east: float, + down: float, + ) -> "tuple[True, NamedPositionLocal] | tuple[False, None]": + """ + name: Can be empty. + north: Metres. + east: Metres. + down: Metres. Allowed to be positive, which is below the home position. + + Return: Success, object. + """ + return True, NamedPositionLocal(cls.__create_key, name, north, east, down) + + def __init__( + self, class_private_create_key: object, name: str, north: float, east: float, down: float + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is NamedPositionLocal.__create_key, "Use create() method." + + super().__init__(super()._PositionLocal__create_key, north, east, down) + + self.name = name + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}: name: {self.name}, north: {self.north}, east: {self.east}, down: {self.down}" diff --git a/tests/integration/test_flight_controller_mission_ended.py b/tests/integration/test_flight_controller_mission_ended.py index 7f6eaa2..91bb207 100644 --- a/tests/integration/test_flight_controller_mission_ended.py +++ b/tests/integration/test_flight_controller_mission_ended.py @@ -6,6 +6,7 @@ from pymavlink import mavutil +from modules import position_global_relative_altitude from modules.mavlink import dronekit from modules.mavlink import flight_controller @@ -26,23 +27,17 @@ # TODO: This function is to be removed when Dronekit-Python interfaces are # moved from pathing repository. def upload_mission( - controller: flight_controller.FlightController, waypoints: "list[tuple[float, float, float]]" + controller: flight_controller.FlightController, + waypoints: list[position_global_relative_altitude.PositionGlobalRelativeAltitude], ) -> bool: """ Add a takeoff command and waypoint following commands to the drone's command sequence, and upload them. - Parameters - ---------- - controller: "flight_controller.FlightController" - waypoints: "list[tuple[float, float, float]]" - The three values in the tuple represent latitude (decimal degrees), - longitude (decimal degrees), and altitude (metres) respectively. - - Returns - ------- - bool - Returns if the mission is successfully uploaded or not. + controller: Flight controller. + waypoints: List of waypoints. + + Return: If the mission is successfully uploaded or not. """ # Clear existing mission controller.drone.commands.download() @@ -70,7 +65,7 @@ def upload_mission( controller.drone.commands.add(takeoff_command) # Add waypoints to the mission - for point in waypoints: + for waypoint in waypoints: command = dronekit.Command( 0, 0, @@ -83,9 +78,9 @@ def upload_mission( ACCEPT_RADIUS, 0, 0, - point[0], - point[1], - point[2], + waypoint.latitude, + waypoint.longitude, + waypoint.relative_altitude, ) controller.drone.commands.add(command) @@ -93,10 +88,13 @@ def upload_mission( # Upload the mission to the drone try: controller.drone.commands.upload() - return True except dronekit.TimeoutError: return False + print("Mission uploaded.") + + return True + def main() -> int: """ @@ -111,12 +109,47 @@ def main() -> int: assert controller is not None # List of waypoints for the drone to travel - waypoints = [ - (43.4731, -80.5419, ALTITUDE), - (43.4723, -80.5380, ALTITUDE), - (43.4735, -80.5371, ALTITUDE), - (43.4743, -80.5400, ALTITUDE), - ] + result, waypoint_1 = position_global_relative_altitude.PositionGlobalRelativeAltitude.create( + 43.4731, -80.5419, ALTITUDE + ) + if not result: + print("Failed to create waypoint.") + return -1 + + # Get Pylance to stop complaining + assert waypoint_1 is not None + + result, waypoint_2 = position_global_relative_altitude.PositionGlobalRelativeAltitude.create( + 43.4723, -80.5380, ALTITUDE + ) + if not result: + print("Failed to create waypoint.") + return -1 + + # Get Pylance to stop complaining + assert waypoint_2 is not None + + result, waypoint_3 = position_global_relative_altitude.PositionGlobalRelativeAltitude.create( + 43.4735, -80.5371, ALTITUDE + ) + if not result: + print("Failed to create waypoint.") + return -1 + + # Get Pylance to stop complaining + assert waypoint_3 is not None + + result, waypoint_4 = position_global_relative_altitude.PositionGlobalRelativeAltitude.create( + 43.4743, -80.5400, ALTITUDE + ) + if not result: + print("Failed to create waypoint.") + return -1 + + # Get Pylance to stop complaining + assert waypoint_4 is not None + + waypoints = [waypoint_1, waypoint_2, waypoint_3, waypoint_4] # Upload mission result = upload_mission(controller, waypoints) diff --git a/tests/unit/test_ground_locations_to_kml.py b/tests/unit/test_ground_locations_to_kml.py index 51184ea..c71620e 100644 --- a/tests/unit/test_ground_locations_to_kml.py +++ b/tests/unit/test_ground_locations_to_kml.py @@ -6,8 +6,8 @@ import pytest -from modules.kml import ground_locations_to_kml -from modules.kml import named_location_global +from modules import location_global +from modules.kml import locations_to_kml PARENT_DIRECTORY = pathlib.Path("tests", "unit", "kml_documents") @@ -20,37 +20,37 @@ @pytest.fixture -def locations() -> "list[named_location_global.NamedLocationGlobal]": # type: ignore +def locations() -> list[location_global.NamedLocationGlobal]: # type: ignore """ List of LocationGround. """ - result, location_san_francisco = named_location_global.NamedLocationGlobal.create( + result, waypoint_san_francisco = location_global.NamedLocationGlobal.create( "San Francisco", 37.7749, -122.4194 ) assert result - assert location_san_francisco is not None + assert waypoint_san_francisco is not None - result, location_los_angeles = named_location_global.NamedLocationGlobal.create( + result, waypoint_los_angeles = location_global.NamedLocationGlobal.create( "Los Angeles", 34.0522, -118.2437 ) assert result - assert location_los_angeles is not None + assert waypoint_los_angeles is not None - result, location_new_york_city = named_location_global.NamedLocationGlobal.create( + result, waypoint_new_york_city = location_global.NamedLocationGlobal.create( "New York City", 40.7128, -74.0060 ) assert result - assert location_new_york_city is not None + assert waypoint_new_york_city is not None yield [ - location_san_francisco, - location_los_angeles, - location_new_york_city, + waypoint_san_francisco, + waypoint_los_angeles, + waypoint_new_york_city, ] def test_locations_to_kml_with_save_path( - locations: "list[named_location_global.NamedLocationGlobal]", tmp_path: pathlib.Path + locations: list[location_global.NamedLocationGlobal], tmp_path: pathlib.Path ) -> None: """ Basic test case to save KML to the correct path when provided. @@ -61,7 +61,7 @@ def test_locations_to_kml_with_save_path( # the KML files are cleaned after the tests are run tmp_path.mkdir(parents=True, exist_ok=True) - result, actual_kml_file_path = ground_locations_to_kml.ground_locations_to_kml( + result, actual_kml_file_path = locations_to_kml.named_locations_to_kml( locations, actual_kml_document_name, tmp_path,