From 2f82a1bb9d1904cb8c6f571e4df776054d5c092a Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 29 May 2024 15:27:01 -0400 Subject: [PATCH] added multiprocessing worker and conversion methods --- modules/drone_odometry_local.py | 81 +++++++++++-------- modules/flight_interface/conversions.py | 48 +++++++++++ modules/flight_interface/flight_interface.py | 43 +++++----- .../flight_interface_worker.py | 36 +++++++++ 4 files changed, 157 insertions(+), 51 deletions(-) create mode 100644 modules/flight_interface/conversions.py create mode 100644 modules/flight_interface/flight_interface_worker.py diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py index 89782ca..f49ff83 100644 --- a/modules/drone_odometry_local.py +++ b/modules/drone_odometry_local.py @@ -1,8 +1,12 @@ +""" +Data structure for local odometry data (local position, orientation, and timestamp) +""" import time from .common.mavlink.modules import drone_odometry + class DronePositionLocal: """ Drone position using NED. @@ -12,27 +16,26 @@ class DronePositionLocal: @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: - + 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: + assert create_key is DronePositionLocal.__create_key, "Use create() method" self.north = north self.east = east self.down = down + def __str__(self) -> str: + """ + String representation + """ + return f"{self.__class__}: North: {self.north}, East: {self.east}, Down: {self.down}." class DroneOdometryLocal: @@ -45,29 +48,43 @@ 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]": + """ + Combines local odometry data with timestamp + """ + + if local_position is None: return False, None - + + if drone_orientation is None: + return False, None + timestamp = time.time() - return True, DroneOdometryLocal(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: + """ + 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}.\ + Time: {self.timestamp}." diff --git a/modules/flight_interface/conversions.py b/modules/flight_interface/conversions.py new file mode 100644 index 0000000..58f12d9 --- /dev/null +++ b/modules/flight_interface/conversions.py @@ -0,0 +1,48 @@ +import pymap3d as pymap + +from .. import drone_odometry_local +from ..common.mavlink.modules import drone_odometry + + +def position_global_to_local( + global_position, home_location +) -> "tuple[bool, drone_odometry_local.DronePositionLocal | None]": + """ + Converts global position (geodetic) to local position (NED) + """ + north, east, down = pymap.geodetic2ned( + global_position.latitude, + global_position.longitude, + global_position.altitude, + home_location.latitude, + home_location.longitude, + home_location.altitude, + ) + + result, local_position = drone_odometry_local.DronePositionLocal.create(north, east, down) + if not result: + return False, None + + return True, local_position + + +def position_local_to_global( + local_position, home_location +) -> "tuple[bool, drone_odometry.DronePosition | None]": + """ + Converts local position (NED) to global position (geodetic) + """ + latitude, longitude, altitude = pymap.ned2geodetic( + local_position.north, + local_position.east, + local_position.down, + home_location.latitude, + home_location.longitude, + home_location.altitude, + ) + + result, global_position = drone_odometry.DronePosition.create(latitude, longitude, altitude) + if not result: + return False, None + + return True, global_position diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 15bd99e..42417a6 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -1,34 +1,38 @@ - -from . import conversions # to be implemented +from . import conversions from .. import drone_odometry_local from ..common.mavlink.modules import drone_odometry from ..common.mavlink.modules import flight_controller + class FlightInterface: + """ + Create flight controller and sets home location + """ __create_key = object() - def create( - cls, - address: str, - timeout_home: float - ) -> "tuple[bool, FlightInterface | None]": + @classmethod + def create(cls, address: str, timeout_home: float) -> "tuple[bool, FlightInterface | None]": + """ + address: TCP address or port. + timeout_home: Timeout for home location in seconds. + """ result, controller = flight_controller.FlightController.create(address) if not result: return False, None - + result, home_location = controller.get_home_location(timeout_home) if not result: return False, None - + return True, FlightInterface(cls.__create_key, controller, home_location) def __init__( - self, + self, create_key: object, controller: flight_controller.FlightController, - home_location: drone_odometry.DroneLocation, + home_location: drone_odometry.DroneLocation, ) -> None: assert create_key is FlightInterface.__create_key, "Use create() method" @@ -36,18 +40,19 @@ def __init__( self.home_location = home_location def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]": - - result, drone_odometry = self.controller.get_odometry() + """ + Returns local drone odometry with timestamp + """ + result, odometry = self.controller.get_odometry() if not result: return False, None - drone_position = drone_odometry.position - - result, drone_position_local = conversions.global_to_local(drone_position, self.home_location) + global_position = odometry.position + + result, local_position = conversions.global_to_local(global_position, self.home_location) if not result: return False, None - drone_orientation = drone_odometry.orientation + drone_orientation = odometry.orientation - return drone_odometry_local.DroneOdometryLocal.create(drone_position_local, drone_orientation) - \ No newline at end of file + return drone_odometry_local.DroneOdometryLocal.create(local_position, drone_orientation) diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py new file mode 100644 index 0000000..398904d --- /dev/null +++ b/modules/flight_interface/flight_interface_worker.py @@ -0,0 +1,36 @@ +import time + +from . import flight_interface +from ..worker import queue_wrapper +from ..worker import worker_controller + + +def flight_interface_worker( + address: str, + timeout: float, + period: float, + output_queue: queue_wrapper.QueueWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + + address, timeout is initial setting. + period is minimum period between loops. + output_queue is the data queue. + controller is how the main process communicates to this worker process. + """ + result, interface = flight_interface.FlightInterface.create(address, timeout) + if not result: + return + + while not controller.is_exit_requested(): + controller.check_pause() + + time.sleep(period) + + result, value = interface.run() + if not result: + continue + + output_queue.queue.put(value)