From caf435db985d567c94fc0b8d0c5bbd35da0d4bce Mon Sep 17 00:00:00 2001 From: Davis Liu Date: Thu, 31 Oct 2024 21:01:14 -0400 Subject: [PATCH 01/11] initial commit --- main_2024.py | 55 ++++++++++++++++-- modules/communications/communications.py | 58 +++++++++++++++++++ .../communications/communications_worker.py | 58 +++++++++++++++++++ modules/flight_interface/flight_interface.py | 8 +-- .../flight_interface_worker.py | 6 +- 5 files changed, 175 insertions(+), 10 deletions(-) create mode 100644 modules/communications/communications.py create mode 100644 modules/communications/communications_worker.py diff --git a/main_2024.py b/main_2024.py index 66ad913d..e7372a8d 100644 --- a/main_2024.py +++ b/main_2024.py @@ -12,6 +12,7 @@ # Used in type annotation of flight interface output # pylint: disable-next=unused-import from modules import odometry_and_time +from modules.communications import communications_worker from modules.detect_target import detect_target_factory from modules.detect_target import detect_target_worker from modules.flight_interface import flight_interface_worker @@ -135,11 +136,19 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + flight_interface_to_communcations_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) + communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -228,7 +237,10 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, ), input_queues=[flight_interface_decision_queue], - output_queues=[flight_interface_to_data_merge_queue], + output_queues=[ + flight_interface_to_data_merge_queue, + flight_interface_to_communcations_queue + ], controller=controller, local_logger=main_logger, ) @@ -266,7 +278,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_main_queue], + output_queues=[geolocation_to_communications_queue], controller=controller, local_logger=main_logger, ) @@ -277,6 +289,24 @@ def main() -> int: # Get Pylance to stop complaining assert geolocation_worker_properties is not None + result, communications_worker_properties = worker_manager.WorkerProperties.create( + count=1, + target=communications_worker.communications_worker, + work_arguments=(), + input_queues=[ + flight_interface_to_communcations_queue, + geolocation_to_communications_queue + ], + output_queues=[communications_to_main_queue], + controller=controller, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create arguments for Video Input", True) + return -1 + + assert communications_worker_properties is not None + # Create managers worker_managers = [] @@ -345,6 +375,19 @@ def main() -> int: worker_managers.append(geolocation_manager) + result, communications_manager = worker_manager.WorkerManager.create( + worker_properties=communications_worker_properties, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create manager for Communications", True) + return -1 + + # Get Pylance to stop complaining + assert communications_manager is not None + + worker_managers.append(communications_manager) + # Run for manager in worker_managers: manager.start_workers() @@ -357,7 +400,7 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_main_queue.queue.get_nowait() + geolocation_data = communications_to_main_queue.queue.get_nowait() except queue.Empty: geolocation_data = None @@ -385,8 +428,10 @@ def main() -> int: video_input_to_detect_target_queue.fill_and_drain_queue() detect_target_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_data_merge_queue.fill_and_drain_queue() + flight_interface_to_communcations_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() - geolocation_to_main_queue.fill_and_drain_queue() + geolocation_to_communications_queue.fill_and_drain_queue() + communications_to_main_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() for manager in worker_managers: diff --git a/modules/communications/communications.py b/modules/communications/communications.py new file mode 100644 index 00000000..be1ee81c --- /dev/null +++ b/modules/communications/communications.py @@ -0,0 +1,58 @@ +import time +from modules.common.logger.modules import logger +from modules.common.mavlink.modules.drone_odometry import DronePosition +from modules.detection_in_world import DetectionInWorld +from modules.flight_interface.local_global_conversion import drone_position_global_from_local + + +class Communications: + """ + """ + __create_key = object() + + @classmethod + def create( + cls, + local_logger: logger.Logger, + ) -> "tuple[bool, Communications | None]": + """ + Logs data and forwards it. + """ + + return True, Communications(cls.__create_key, local_logger) + + def __init__( + self, + class_private_create_key: object, + local_logger: logger.Logger, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is Communications.__create_key, "Use create() method" + + self.__logger = local_logger + + def run( + self, detections_in_world: list[DetectionInWorld], home_location: DronePosition + ) -> tuple[bool, list[DetectionInWorld] | None]: + for detection_in_world in detections_in_world: + if len(detection_in_world.centre) == 3: + result, detection_in_world_global = drone_position_global_from_local( + home_location, + detection_in_world.centre[0], + detection_in_world.centre[1], + detection_in_world.centre[2], + ) + elif len(detection_in_world.centre) == 2: + result, detection_in_world_global = drone_position_global_from_local( + home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 + ) + + if not result: + self.__logger.error("conversion failed") + return False, detections_in_world + + self.__logger.info(time.time() + ": " + str(detection_in_world_global)) + + return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py new file mode 100644 index 00000000..e22aacfa --- /dev/null +++ b/modules/communications/communications_worker.py @@ -0,0 +1,58 @@ +""" +Logs data and forwards it. +""" + +import os +import pathlib + +from modules.communications import communications +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from ..common.logger.modules import logger + + +def communications_worker( + home_location_queue: queue_proxy_wrapper.QueueProxyWrapper, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + + home_location: get home_location for init + """ + # TODO: Error handling + + worker_name = pathlib.Path(__file__).stem + process_id = os.getpid() + result, local_logger = logger.Logger.create(f"{worker_name}_{process_id}", True) + if not result: + print("ERROR: Worker failed to create logger") + return + + # Get Pylance to stop complaining + assert local_logger is not None + + local_logger.info("Logger initialized", True) + + result, comm = communications.Communications.create(local_logger) + if not result: + local_logger.error("Worker failed to create class object", True) + return + + # Get Pylance to stop complaining + assert comm is not None + + home_location = None + + while not controller.is_exit_requested(): + controller.check_pause() + + if not home_location: + home_location = home_location_queue.queue.get() + result, value = comm.run(input_queue.queue.get(), home_location) + if not result: + continue + + output_queue.queue.put(value) diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 9786f413..6f4fa99a 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -67,9 +67,9 @@ def __init__( self.__logger.info(str(self.__home_location), True) - def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": + def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None, drone_odometry.DronePosition]": """ - Returns a possible OdometryAndTime with current timestamp. + Returns a possible OdometryAndTime with current timestamp and home location. """ result, odometry = self.controller.get_odometry() if not result: @@ -83,7 +83,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": self.__home_location, ) if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry_local is not None @@ -97,7 +97,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": self.__logger.info(str(odometry_and_time_object), True) - return True, odometry_and_time_object + return True, odometry_and_time_object, self.__home_location def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 0c81dab9..37454787 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -19,6 +19,7 @@ def flight_interface_worker( period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, + communications_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -53,16 +54,19 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None + home_location_sent = False while not controller.is_exit_requested(): controller.check_pause() time.sleep(period) - result, value = interface.run() + result, value, home_location = interface.run() if not result: continue output_queue.queue.put(value) + if not home_location_sent: + communications_ouutput_queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From 4b65ad5ce1121979fdcdf0d465a6c9bd1c2cf25f Mon Sep 17 00:00:00 2001 From: Ashish Agrahari Date: Fri, 1 Nov 2024 14:06:45 -0400 Subject: [PATCH 02/11] Made fixes --- modules/communications/communications.py | 20 ++++++------------- .../communications/communications_worker.py | 2 +- modules/flight_interface/flight_interface.py | 2 +- .../flight_interface_worker.py | 4 ++-- 4 files changed, 10 insertions(+), 18 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index be1ee81c..9396ca81 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -2,7 +2,7 @@ from modules.common.logger.modules import logger from modules.common.mavlink.modules.drone_odometry import DronePosition from modules.detection_in_world import DetectionInWorld -from modules.flight_interface.local_global_conversion import drone_position_global_from_local +from modules.flight_interface.local_global_conversion import detection_in_world_global_from_local class Communications: @@ -37,22 +37,14 @@ def run( self, detections_in_world: list[DetectionInWorld], home_location: DronePosition ) -> tuple[bool, list[DetectionInWorld] | None]: for detection_in_world in detections_in_world: - if len(detection_in_world.centre) == 3: - result, detection_in_world_global = drone_position_global_from_local( - home_location, - detection_in_world.centre[0], - detection_in_world.centre[1], - detection_in_world.centre[2], - ) - elif len(detection_in_world.centre) == 2: - result, detection_in_world_global = drone_position_global_from_local( - home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 - ) + result, detection_in_world_global = detection_in_world_global_from_local( + home_location, detection_in_world + ) if not result: self.__logger.error("conversion failed") return False, detections_in_world - self.__logger.info(time.time() + ": " + str(detection_in_world_global)) - + self.__logger.info(str(time.time()) + ": " + str(detection_in_world_global)) + return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index e22aacfa..2ba5e1bf 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.communications import communications +from . import communications from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from ..common.logger.modules import logger diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 6f4fa99a..98e744c9 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -73,7 +73,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None, drone_od """ result, odometry = self.controller.get_odometry() if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry is not None diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 37454787..f8119f76 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -19,7 +19,7 @@ def flight_interface_worker( period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, - communications_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, + communications_output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -66,7 +66,7 @@ def flight_interface_worker( output_queue.queue.put(value) if not home_location_sent: - communications_ouutput_queue.put(home_location) + communications_output_queue.queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From 74f5c2ee7b0aad7df6119611be300e4261165b44 Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Fri, 1 Nov 2024 23:16:31 -0400 Subject: [PATCH 03/11] Modify home location to be part of create for flight interface and communication worker --- main_2024.py | 12 +++++----- modules/communications/communications.py | 22 ++++++++++++++----- .../communications/communications_worker.py | 12 +++++----- modules/detection_in_world.py | 6 +++++ modules/flight_interface/flight_interface.py | 18 ++++++++++----- .../flight_interface_worker.py | 8 +++---- .../test_flight_interface_worker.py | 4 ++++ 7 files changed, 53 insertions(+), 29 deletions(-) diff --git a/main_2024.py b/main_2024.py index e7372a8d..6958142e 100644 --- a/main_2024.py +++ b/main_2024.py @@ -136,7 +136,7 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - flight_interface_to_communcations_queue = queue_proxy_wrapper.QueueProxyWrapper( + flight_interface_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -239,7 +239,7 @@ def main() -> int: input_queues=[flight_interface_decision_queue], output_queues=[ flight_interface_to_data_merge_queue, - flight_interface_to_communcations_queue + flight_interface_to_communications_queue, ], controller=controller, local_logger=main_logger, @@ -294,8 +294,8 @@ def main() -> int: target=communications_worker.communications_worker, work_arguments=(), input_queues=[ - flight_interface_to_communcations_queue, - geolocation_to_communications_queue + flight_interface_to_communications_queue, + geolocation_to_communications_queue, ], output_queues=[communications_to_main_queue], controller=controller, @@ -306,7 +306,7 @@ def main() -> int: return -1 assert communications_worker_properties is not None - + # Create managers worker_managers = [] @@ -428,7 +428,7 @@ def main() -> int: video_input_to_detect_target_queue.fill_and_drain_queue() detect_target_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_data_merge_queue.fill_and_drain_queue() - flight_interface_to_communcations_queue.fill_and_drain_queue() + flight_interface_to_communications_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() geolocation_to_communications_queue.fill_and_drain_queue() communications_to_main_queue.fill_and_drain_queue() diff --git a/modules/communications/communications.py b/modules/communications/communications.py index 9396ca81..cea5c122 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,4 +1,5 @@ import time +from ..common.mavlink.modules import drone_odometry from modules.common.logger.modules import logger from modules.common.mavlink.modules.drone_odometry import DronePosition from modules.detection_in_world import DetectionInWorld @@ -6,24 +7,26 @@ class Communications: - """ - """ + """ """ + __create_key = object() @classmethod def create( cls, + home_location: drone_odometry.DronePosition, local_logger: logger.Logger, ) -> "tuple[bool, Communications | None]": """ Logs data and forwards it. """ - return True, Communications(cls.__create_key, local_logger) + return True, Communications(cls.__create_key, home_location, local_logger) def __init__( self, class_private_create_key: object, + home_location: drone_odometry.DronePosition, local_logger: logger.Logger, ) -> None: """ @@ -31,20 +34,27 @@ def __init__( """ assert class_private_create_key is Communications.__create_key, "Use create() method" + self.__home_location = home_location self.__logger = local_logger def run( - self, detections_in_world: list[DetectionInWorld], home_location: DronePosition + self, detections_in_world: list[DetectionInWorld] ) -> tuple[bool, list[DetectionInWorld] | None]: + + detections_in_world_global = [] for detection_in_world in detections_in_world: result, detection_in_world_global = detection_in_world_global_from_local( - home_location, detection_in_world + self.__home_location, detection_in_world ) if not result: + # Log nothing if at least one of the conversions failed self.__logger.error("conversion failed") return False, detections_in_world - self.__logger.info(str(time.time()) + ": " + str(detection_in_world_global)) + detections_in_world_global.append(detection_in_world_global) + + timestamp = time.time() + self.__logger.info(f"{timestamp}: {detections_in_world_global}") return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 2ba5e1bf..d7461563 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -22,7 +22,6 @@ def communications_worker( home_location: get home_location for init """ - # TODO: Error handling worker_name = pathlib.Path(__file__).stem process_id = os.getpid() @@ -36,7 +35,10 @@ def communications_worker( local_logger.info("Logger initialized", True) - result, comm = communications.Communications.create(local_logger) + # Get home location + home_location = home_location_queue.queue.get() + + result, comm = communications.Communications.create(home_location, local_logger) if not result: local_logger.error("Worker failed to create class object", True) return @@ -44,14 +46,10 @@ def communications_worker( # Get Pylance to stop complaining assert comm is not None - home_location = None - while not controller.is_exit_requested(): controller.check_pause() - if not home_location: - home_location = home_location_queue.queue.get() - result, value = comm.run(input_queue.queue.get(), home_location) + result, value = comm.run(input_queue.queue.get()) if not result: continue diff --git a/modules/detection_in_world.py b/modules/detection_in_world.py index a5c22943..cc96e855 100644 --- a/modules/detection_in_world.py +++ b/modules/detection_in_world.py @@ -58,3 +58,9 @@ def __str__(self) -> str: To string. """ return f"{self.__class__}, vertices: {self.vertices.tolist()}, centre: {self.centre}, label: {self.label}, confidence: {self.confidence}" + + def __repr__(self) -> str: + """ + For collections (e.g. list). + """ + return str(self) diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 98e744c9..c940afad 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -47,6 +47,8 @@ def create( # Get Pylance to stop complaining assert home_location is not None + local_logger.info(f"Home location: {home_location}", True) + return True, FlightInterface(cls.__create_key, controller, home_location, local_logger) def __init__( @@ -65,15 +67,19 @@ def __init__( self.__home_location = home_location self.__logger = local_logger - self.__logger.info(str(self.__home_location), True) + def get_home_location(self) -> drone_odometry.DronePosition: + """ + Accessor for home location. + """ + return self.__home_location - def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None, drone_odometry.DronePosition]": + def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ - Returns a possible OdometryAndTime with current timestamp and home location. + Returns a possible OdometryAndTime with current timestamp. """ result, odometry = self.controller.get_odometry() if not result: - return False, None, None + return False, None # Get Pylance to stop complaining assert odometry is not None @@ -83,7 +89,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None, drone_od self.__home_location, ) if not result: - return False, None, None + return False, None # Get Pylance to stop complaining assert odometry_local is not None @@ -97,7 +103,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None, drone_od self.__logger.info(str(odometry_and_time_object), True) - return True, odometry_and_time_object, self.__home_location + return True, odometry_and_time_object def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index f8119f76..b3061cad 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -54,19 +54,19 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None - home_location_sent = False + home_location = interface.get_home_location() + communications_output_queue.queue.put(home_location) + while not controller.is_exit_requested(): controller.check_pause() time.sleep(period) - result, value, home_location = interface.run() + result, value = interface.run() if not result: continue output_queue.queue.put(value) - if not home_location_sent: - communications_output_queue.queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): diff --git a/tests/integration/test_flight_interface_worker.py b/tests/integration/test_flight_interface_worker.py index 17f1983b..aaebbc00 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -103,6 +103,7 @@ def main() -> int: mp_manager = mp.Manager() out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) + home_location_out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) in_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) worker = mp.Process( @@ -114,6 +115,7 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, in_queue, # Added input_queue out_queue, + home_location_out_queue, controller, ), ) @@ -124,6 +126,8 @@ def main() -> int: time.sleep(3) # Test + home_location = home_location_out_queue.queue.get() + assert home_location is not None # Run the apply_decision tests test_result = apply_decision_test(in_queue, out_queue) From c75347e32b72a8585d7339f41173a6c8957deaa8 Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Sat, 2 Nov 2024 00:07:15 -0400 Subject: [PATCH 04/11] combined all logging together --- modules/common | 2 +- modules/communications/communications.py | 53 +++++++++++++++++++----- modules/detections_and_time.py | 6 +++ 3 files changed, 49 insertions(+), 12 deletions(-) diff --git a/modules/common b/modules/common index 07b515df..e87bdb53 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit 07b515df9b56b9a65142d826f720b88f66c1a5f0 +Subproject commit e87bdb5393189f4abf6c9bd2e5bb2e19481cd161 diff --git a/modules/communications/communications.py b/modules/communications/communications.py index cea5c122..ef0cb3a7 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,13 +1,20 @@ +""" +Logs data and forwards it. +""" + import time + +from .. import detection_in_world +from ..common.logger.modules import logger from ..common.mavlink.modules import drone_odometry -from modules.common.logger.modules import logger -from modules.common.mavlink.modules.drone_odometry import DronePosition -from modules.detection_in_world import DetectionInWorld -from modules.flight_interface.local_global_conversion import detection_in_world_global_from_local +from ..common.mavlink.modules import drone_odometry_local +from ..common.mavlink.modules import local_global_conversion class Communications: - """ """ + """ + Currently logs data only. + """ __create_key = object() @@ -19,6 +26,10 @@ def create( ) -> "tuple[bool, Communications | None]": """ Logs data and forwards it. + + home_location: Take-off location of drone. + + Returns: Success, class object. """ return True, Communications(cls.__create_key, home_location, local_logger) @@ -38,19 +49,39 @@ def __init__( self.__logger = local_logger def run( - self, detections_in_world: list[DetectionInWorld] - ) -> tuple[bool, list[DetectionInWorld] | None]: + self, detections_in_world: list[detection_in_world.DetectionInWorld] + ) -> tuple[bool, list[detection_in_world.DetectionInWorld] | None]: detections_in_world_global = [] for detection_in_world in detections_in_world: - result, detection_in_world_global = detection_in_world_global_from_local( - self.__home_location, detection_in_world + # TODO: Change this when the conversion interface is changed + north = detection_in_world.centre[0] + east = detection_in_world.centre[1] + down = 0 + + result, drone_position_local = drone_odometry_local.DronePositionLocal.create( + north, + east, + down, + ) + if not result: + self.__logger.warning( + f"Could not convert DetectionInWorld to DronePositionLocal:\ndetection in world: {detection_in_world}" + ) + return False, None + + result, detection_in_world_global = ( + local_global_conversion.drone_position_global_from_local( + self.__home_location, drone_position_local + ) ) if not result: # Log nothing if at least one of the conversions failed - self.__logger.error("conversion failed") - return False, detections_in_world + self.__logger.warning( + f"drone_position_global_from_local conversion failed:\nhome_location: {self.__home_location}\ndrone_position_local: {drone_position_local}" + ) + return False, None detections_in_world_global.append(detection_in_world_global) diff --git a/modules/detections_and_time.py b/modules/detections_and_time.py index fcedcd9e..75df2e02 100644 --- a/modules/detections_and_time.py +++ b/modules/detections_and_time.py @@ -57,6 +57,12 @@ def __str__(self) -> str: """ return f"cls: {self.label}, conf: {self.confidence}, bounds: {self.x_1} {self.y_1} {self.x_2} {self.y_2}" + def __repr__(self) -> str: + """ + For collections (e.g. list). + """ + return str(self) + def get_centre(self) -> "tuple[float, float]": """ Gets the xy centre of the bounding box. From 223c52df4e61469ae784510a4b8b5a8ca994353d Mon Sep 17 00:00:00 2001 From: Davis Liu Date: Thu, 31 Oct 2024 21:01:14 -0400 Subject: [PATCH 05/11] initial commit --- main_2024.py | 55 ++++++++++++++++-- modules/communications/communications.py | 58 +++++++++++++++++++ .../communications/communications_worker.py | 58 +++++++++++++++++++ modules/flight_interface/flight_interface.py | 6 +- .../flight_interface_worker.py | 6 +- 5 files changed, 174 insertions(+), 9 deletions(-) create mode 100644 modules/communications/communications.py create mode 100644 modules/communications/communications_worker.py diff --git a/main_2024.py b/main_2024.py index f6297226..0fb94405 100644 --- a/main_2024.py +++ b/main_2024.py @@ -12,6 +12,7 @@ # Used in type annotation of flight interface output # pylint: disable-next=unused-import from modules import odometry_and_time +from modules.communications import communications_worker from modules.detect_target import detect_target_factory from modules.detect_target import detect_target_worker from modules.flight_interface import flight_interface_worker @@ -135,11 +136,19 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + flight_interface_to_communcations_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) + communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -228,7 +237,10 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, ), input_queues=[flight_interface_decision_queue], - output_queues=[flight_interface_to_data_merge_queue], + output_queues=[ + flight_interface_to_data_merge_queue, + flight_interface_to_communcations_queue + ], controller=controller, local_logger=main_logger, ) @@ -266,7 +278,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_main_queue], + output_queues=[geolocation_to_communications_queue], controller=controller, local_logger=main_logger, ) @@ -277,6 +289,24 @@ def main() -> int: # Get Pylance to stop complaining assert geolocation_worker_properties is not None + result, communications_worker_properties = worker_manager.WorkerProperties.create( + count=1, + target=communications_worker.communications_worker, + work_arguments=(), + input_queues=[ + flight_interface_to_communcations_queue, + geolocation_to_communications_queue + ], + output_queues=[communications_to_main_queue], + controller=controller, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create arguments for Video Input", True) + return -1 + + assert communications_worker_properties is not None + # Create managers worker_managers = [] @@ -345,6 +375,19 @@ def main() -> int: worker_managers.append(geolocation_manager) + result, communications_manager = worker_manager.WorkerManager.create( + worker_properties=communications_worker_properties, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create manager for Communications", True) + return -1 + + # Get Pylance to stop complaining + assert communications_manager is not None + + worker_managers.append(communications_manager) + # Run for manager in worker_managers: manager.start_workers() @@ -357,7 +400,7 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_main_queue.queue.get_nowait() + geolocation_data = communications_to_main_queue.queue.get_nowait() except queue.Empty: geolocation_data = None @@ -385,8 +428,10 @@ def main() -> int: video_input_to_detect_target_queue.fill_and_drain_queue() detect_target_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_data_merge_queue.fill_and_drain_queue() + flight_interface_to_communcations_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() - geolocation_to_main_queue.fill_and_drain_queue() + geolocation_to_communications_queue.fill_and_drain_queue() + communications_to_main_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() for manager in worker_managers: diff --git a/modules/communications/communications.py b/modules/communications/communications.py new file mode 100644 index 00000000..be1ee81c --- /dev/null +++ b/modules/communications/communications.py @@ -0,0 +1,58 @@ +import time +from modules.common.logger.modules import logger +from modules.common.mavlink.modules.drone_odometry import DronePosition +from modules.detection_in_world import DetectionInWorld +from modules.flight_interface.local_global_conversion import drone_position_global_from_local + + +class Communications: + """ + """ + __create_key = object() + + @classmethod + def create( + cls, + local_logger: logger.Logger, + ) -> "tuple[bool, Communications | None]": + """ + Logs data and forwards it. + """ + + return True, Communications(cls.__create_key, local_logger) + + def __init__( + self, + class_private_create_key: object, + local_logger: logger.Logger, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is Communications.__create_key, "Use create() method" + + self.__logger = local_logger + + def run( + self, detections_in_world: list[DetectionInWorld], home_location: DronePosition + ) -> tuple[bool, list[DetectionInWorld] | None]: + for detection_in_world in detections_in_world: + if len(detection_in_world.centre) == 3: + result, detection_in_world_global = drone_position_global_from_local( + home_location, + detection_in_world.centre[0], + detection_in_world.centre[1], + detection_in_world.centre[2], + ) + elif len(detection_in_world.centre) == 2: + result, detection_in_world_global = drone_position_global_from_local( + home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 + ) + + if not result: + self.__logger.error("conversion failed") + return False, detections_in_world + + self.__logger.info(time.time() + ": " + str(detection_in_world_global)) + + return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py new file mode 100644 index 00000000..e22aacfa --- /dev/null +++ b/modules/communications/communications_worker.py @@ -0,0 +1,58 @@ +""" +Logs data and forwards it. +""" + +import os +import pathlib + +from modules.communications import communications +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from ..common.logger.modules import logger + + +def communications_worker( + home_location_queue: queue_proxy_wrapper.QueueProxyWrapper, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + + home_location: get home_location for init + """ + # TODO: Error handling + + worker_name = pathlib.Path(__file__).stem + process_id = os.getpid() + result, local_logger = logger.Logger.create(f"{worker_name}_{process_id}", True) + if not result: + print("ERROR: Worker failed to create logger") + return + + # Get Pylance to stop complaining + assert local_logger is not None + + local_logger.info("Logger initialized", True) + + result, comm = communications.Communications.create(local_logger) + if not result: + local_logger.error("Worker failed to create class object", True) + return + + # Get Pylance to stop complaining + assert comm is not None + + home_location = None + + while not controller.is_exit_requested(): + controller.check_pause() + + if not home_location: + home_location = home_location_queue.queue.get() + result, value = comm.run(input_queue.queue.get(), home_location) + if not result: + continue + + output_queue.queue.put(value) diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index e1b47d86..bc637886 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -69,7 +69,7 @@ def __init__( def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ - Returns a possible OdometryAndTime with current timestamp. + Returns a possible OdometryAndTime with current timestamp and home location. """ result, odometry = self.controller.get_odometry() if not result: @@ -83,7 +83,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": odometry, ) if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry_local is not None @@ -97,7 +97,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": self.__logger.info(str(odometry_and_time_object), True) - return True, odometry_and_time_object + return True, odometry_and_time_object, self.__home_location def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 2ead7bae..e1fb3f51 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -19,6 +19,7 @@ def flight_interface_worker( period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, + communications_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -53,16 +54,19 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None + home_location_sent = False while not controller.is_exit_requested(): controller.check_pause() time.sleep(period) - result, value = interface.run() + result, value, home_location = interface.run() if not result: continue output_queue.queue.put(value) + if not home_location_sent: + communications_ouutput_queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From 27ebdca1c980b21840bc856cd314c6c4553421be Mon Sep 17 00:00:00 2001 From: Ashish Agrahari Date: Fri, 1 Nov 2024 14:06:45 -0400 Subject: [PATCH 06/11] Made fixes --- modules/communications/communications.py | 20 ++++++------------- .../communications/communications_worker.py | 2 +- modules/flight_interface/flight_interface.py | 2 +- .../flight_interface_worker.py | 4 ++-- 4 files changed, 10 insertions(+), 18 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index be1ee81c..9396ca81 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -2,7 +2,7 @@ from modules.common.logger.modules import logger from modules.common.mavlink.modules.drone_odometry import DronePosition from modules.detection_in_world import DetectionInWorld -from modules.flight_interface.local_global_conversion import drone_position_global_from_local +from modules.flight_interface.local_global_conversion import detection_in_world_global_from_local class Communications: @@ -37,22 +37,14 @@ def run( self, detections_in_world: list[DetectionInWorld], home_location: DronePosition ) -> tuple[bool, list[DetectionInWorld] | None]: for detection_in_world in detections_in_world: - if len(detection_in_world.centre) == 3: - result, detection_in_world_global = drone_position_global_from_local( - home_location, - detection_in_world.centre[0], - detection_in_world.centre[1], - detection_in_world.centre[2], - ) - elif len(detection_in_world.centre) == 2: - result, detection_in_world_global = drone_position_global_from_local( - home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 - ) + result, detection_in_world_global = detection_in_world_global_from_local( + home_location, detection_in_world + ) if not result: self.__logger.error("conversion failed") return False, detections_in_world - self.__logger.info(time.time() + ": " + str(detection_in_world_global)) - + self.__logger.info(str(time.time()) + ": " + str(detection_in_world_global)) + return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index e22aacfa..2ba5e1bf 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.communications import communications +from . import communications from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from ..common.logger.modules import logger diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index bc637886..55fc8007 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -73,7 +73,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ result, odometry = self.controller.get_odometry() if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry is not None diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index e1fb3f51..5a6b4aeb 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -19,7 +19,7 @@ def flight_interface_worker( period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, - communications_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, + communications_output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -66,7 +66,7 @@ def flight_interface_worker( output_queue.queue.put(value) if not home_location_sent: - communications_ouutput_queue.put(home_location) + communications_output_queue.queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From 439c9b0f39a414ed55477226457836a751d98d3d Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Fri, 1 Nov 2024 23:16:31 -0400 Subject: [PATCH 07/11] Modify home location to be part of create for flight interface and communication worker --- main_2024.py | 12 +++++----- modules/communications/communications.py | 22 ++++++++++++++----- .../communications/communications_worker.py | 12 +++++----- modules/detection_in_world.py | 6 +++++ modules/flight_interface/flight_interface.py | 18 ++++++++++----- .../flight_interface_worker.py | 8 +++---- .../test_flight_interface_worker.py | 4 ++++ 7 files changed, 53 insertions(+), 29 deletions(-) diff --git a/main_2024.py b/main_2024.py index 0fb94405..05ea8430 100644 --- a/main_2024.py +++ b/main_2024.py @@ -136,7 +136,7 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - flight_interface_to_communcations_queue = queue_proxy_wrapper.QueueProxyWrapper( + flight_interface_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -239,7 +239,7 @@ def main() -> int: input_queues=[flight_interface_decision_queue], output_queues=[ flight_interface_to_data_merge_queue, - flight_interface_to_communcations_queue + flight_interface_to_communications_queue, ], controller=controller, local_logger=main_logger, @@ -294,8 +294,8 @@ def main() -> int: target=communications_worker.communications_worker, work_arguments=(), input_queues=[ - flight_interface_to_communcations_queue, - geolocation_to_communications_queue + flight_interface_to_communications_queue, + geolocation_to_communications_queue, ], output_queues=[communications_to_main_queue], controller=controller, @@ -306,7 +306,7 @@ def main() -> int: return -1 assert communications_worker_properties is not None - + # Create managers worker_managers = [] @@ -428,7 +428,7 @@ def main() -> int: video_input_to_detect_target_queue.fill_and_drain_queue() detect_target_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_data_merge_queue.fill_and_drain_queue() - flight_interface_to_communcations_queue.fill_and_drain_queue() + flight_interface_to_communications_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() geolocation_to_communications_queue.fill_and_drain_queue() communications_to_main_queue.fill_and_drain_queue() diff --git a/modules/communications/communications.py b/modules/communications/communications.py index 9396ca81..cea5c122 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,4 +1,5 @@ import time +from ..common.mavlink.modules import drone_odometry from modules.common.logger.modules import logger from modules.common.mavlink.modules.drone_odometry import DronePosition from modules.detection_in_world import DetectionInWorld @@ -6,24 +7,26 @@ class Communications: - """ - """ + """ """ + __create_key = object() @classmethod def create( cls, + home_location: drone_odometry.DronePosition, local_logger: logger.Logger, ) -> "tuple[bool, Communications | None]": """ Logs data and forwards it. """ - return True, Communications(cls.__create_key, local_logger) + return True, Communications(cls.__create_key, home_location, local_logger) def __init__( self, class_private_create_key: object, + home_location: drone_odometry.DronePosition, local_logger: logger.Logger, ) -> None: """ @@ -31,20 +34,27 @@ def __init__( """ assert class_private_create_key is Communications.__create_key, "Use create() method" + self.__home_location = home_location self.__logger = local_logger def run( - self, detections_in_world: list[DetectionInWorld], home_location: DronePosition + self, detections_in_world: list[DetectionInWorld] ) -> tuple[bool, list[DetectionInWorld] | None]: + + detections_in_world_global = [] for detection_in_world in detections_in_world: result, detection_in_world_global = detection_in_world_global_from_local( - home_location, detection_in_world + self.__home_location, detection_in_world ) if not result: + # Log nothing if at least one of the conversions failed self.__logger.error("conversion failed") return False, detections_in_world - self.__logger.info(str(time.time()) + ": " + str(detection_in_world_global)) + detections_in_world_global.append(detection_in_world_global) + + timestamp = time.time() + self.__logger.info(f"{timestamp}: {detections_in_world_global}") return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 2ba5e1bf..d7461563 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -22,7 +22,6 @@ def communications_worker( home_location: get home_location for init """ - # TODO: Error handling worker_name = pathlib.Path(__file__).stem process_id = os.getpid() @@ -36,7 +35,10 @@ def communications_worker( local_logger.info("Logger initialized", True) - result, comm = communications.Communications.create(local_logger) + # Get home location + home_location = home_location_queue.queue.get() + + result, comm = communications.Communications.create(home_location, local_logger) if not result: local_logger.error("Worker failed to create class object", True) return @@ -44,14 +46,10 @@ def communications_worker( # Get Pylance to stop complaining assert comm is not None - home_location = None - while not controller.is_exit_requested(): controller.check_pause() - if not home_location: - home_location = home_location_queue.queue.get() - result, value = comm.run(input_queue.queue.get(), home_location) + result, value = comm.run(input_queue.queue.get()) if not result: continue diff --git a/modules/detection_in_world.py b/modules/detection_in_world.py index a5c22943..cc96e855 100644 --- a/modules/detection_in_world.py +++ b/modules/detection_in_world.py @@ -58,3 +58,9 @@ def __str__(self) -> str: To string. """ return f"{self.__class__}, vertices: {self.vertices.tolist()}, centre: {self.centre}, label: {self.label}, confidence: {self.confidence}" + + def __repr__(self) -> str: + """ + For collections (e.g. list). + """ + return str(self) diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 55fc8007..17088f7a 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -45,9 +45,9 @@ def create( return False, None # Get Pylance to stop complaining - assert home_position is not None + assert home_location is not None - local_logger.info(str(home_position), True) + local_logger.info(f"Home location: {home_location}", True) return True, FlightInterface(cls.__create_key, controller, home_position, local_logger) @@ -67,13 +67,19 @@ def __init__( self.__home_position = home_position self.__logger = local_logger + def get_home_location(self) -> drone_odometry.DronePosition: + """ + Accessor for home location. + """ + return self.__home_location + def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ - Returns a possible OdometryAndTime with current timestamp and home location. + Returns a possible OdometryAndTime with current timestamp. """ result, odometry = self.controller.get_odometry() if not result: - return False, None, None + return False, None # Get Pylance to stop complaining assert odometry is not None @@ -83,7 +89,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": odometry, ) if not result: - return False, None, None + return False, None # Get Pylance to stop complaining assert odometry_local is not None @@ -97,7 +103,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": self.__logger.info(str(odometry_and_time_object), True) - return True, odometry_and_time_object, self.__home_location + return True, odometry_and_time_object def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 5a6b4aeb..609694bc 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -54,19 +54,19 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None - home_location_sent = False + home_location = interface.get_home_location() + communications_output_queue.queue.put(home_location) + while not controller.is_exit_requested(): controller.check_pause() time.sleep(period) - result, value, home_location = interface.run() + result, value = interface.run() if not result: continue output_queue.queue.put(value) - if not home_location_sent: - communications_output_queue.queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): diff --git a/tests/integration/test_flight_interface_worker.py b/tests/integration/test_flight_interface_worker.py index 17f1983b..aaebbc00 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -103,6 +103,7 @@ def main() -> int: mp_manager = mp.Manager() out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) + home_location_out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) in_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) worker = mp.Process( @@ -114,6 +115,7 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, in_queue, # Added input_queue out_queue, + home_location_out_queue, controller, ), ) @@ -124,6 +126,8 @@ def main() -> int: time.sleep(3) # Test + home_location = home_location_out_queue.queue.get() + assert home_location is not None # Run the apply_decision tests test_result = apply_decision_test(in_queue, out_queue) From 81f06dc9b849e0a741673fe043d4bc786f21213c Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Sat, 2 Nov 2024 00:07:15 -0400 Subject: [PATCH 08/11] combined all logging together --- modules/communications/communications.py | 53 +++++++++++++++++++----- modules/detections_and_time.py | 6 +++ 2 files changed, 48 insertions(+), 11 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index cea5c122..ef0cb3a7 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,13 +1,20 @@ +""" +Logs data and forwards it. +""" + import time + +from .. import detection_in_world +from ..common.logger.modules import logger from ..common.mavlink.modules import drone_odometry -from modules.common.logger.modules import logger -from modules.common.mavlink.modules.drone_odometry import DronePosition -from modules.detection_in_world import DetectionInWorld -from modules.flight_interface.local_global_conversion import detection_in_world_global_from_local +from ..common.mavlink.modules import drone_odometry_local +from ..common.mavlink.modules import local_global_conversion class Communications: - """ """ + """ + Currently logs data only. + """ __create_key = object() @@ -19,6 +26,10 @@ def create( ) -> "tuple[bool, Communications | None]": """ Logs data and forwards it. + + home_location: Take-off location of drone. + + Returns: Success, class object. """ return True, Communications(cls.__create_key, home_location, local_logger) @@ -38,19 +49,39 @@ def __init__( self.__logger = local_logger def run( - self, detections_in_world: list[DetectionInWorld] - ) -> tuple[bool, list[DetectionInWorld] | None]: + self, detections_in_world: list[detection_in_world.DetectionInWorld] + ) -> tuple[bool, list[detection_in_world.DetectionInWorld] | None]: detections_in_world_global = [] for detection_in_world in detections_in_world: - result, detection_in_world_global = detection_in_world_global_from_local( - self.__home_location, detection_in_world + # TODO: Change this when the conversion interface is changed + north = detection_in_world.centre[0] + east = detection_in_world.centre[1] + down = 0 + + result, drone_position_local = drone_odometry_local.DronePositionLocal.create( + north, + east, + down, + ) + if not result: + self.__logger.warning( + f"Could not convert DetectionInWorld to DronePositionLocal:\ndetection in world: {detection_in_world}" + ) + return False, None + + result, detection_in_world_global = ( + local_global_conversion.drone_position_global_from_local( + self.__home_location, drone_position_local + ) ) if not result: # Log nothing if at least one of the conversions failed - self.__logger.error("conversion failed") - return False, detections_in_world + self.__logger.warning( + f"drone_position_global_from_local conversion failed:\nhome_location: {self.__home_location}\ndrone_position_local: {drone_position_local}" + ) + return False, None detections_in_world_global.append(detection_in_world_global) diff --git a/modules/detections_and_time.py b/modules/detections_and_time.py index fcedcd9e..75df2e02 100644 --- a/modules/detections_and_time.py +++ b/modules/detections_and_time.py @@ -57,6 +57,12 @@ def __str__(self) -> str: """ return f"cls: {self.label}, conf: {self.confidence}, bounds: {self.x_1} {self.y_1} {self.x_2} {self.y_2}" + def __repr__(self) -> str: + """ + For collections (e.g. list). + """ + return str(self) + def get_centre(self) -> "tuple[float, float]": """ Gets the xy centre of the bounding box. From 66c4855b37b7352296c47550c7e1913f34810f46 Mon Sep 17 00:00:00 2001 From: Evan Zhao <57083990+Evang264@users.noreply.github.com> Date: Thu, 14 Nov 2024 17:25:43 -0500 Subject: [PATCH 09/11] Reflect changes in common/ --- modules/common | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/common b/modules/common index a32385ee..a0aac8ce 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit a32385eefab7d6a5468f5b8f2e708860a5f085d6 +Subproject commit a0aac8ce29273a6a1ca397a2229770add760835e From 7ae96e0c866f39e3b4b9b1485351519f5f9a7139 Mon Sep 17 00:00:00 2001 From: Evan Zhao <57083990+Evang264@users.noreply.github.com> Date: Thu, 14 Nov 2024 17:26:15 -0500 Subject: [PATCH 10/11] Add log_to_kml.py --- modules/communications/log_to_kml.py | 56 ++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 modules/communications/log_to_kml.py diff --git a/modules/communications/log_to_kml.py b/modules/communications/log_to_kml.py new file mode 100644 index 00000000..f62ad82f --- /dev/null +++ b/modules/communications/log_to_kml.py @@ -0,0 +1,56 @@ +""" +Convert log file to KML file. +""" + +import pathlib +import re + +from modules.common.modules.kml.locations_to_kml import locations_to_kml +from modules.common.modules.location_global import LocationGlobal + + +def convert_log_to_kml( + log_file: str, document_name_prefix: str, save_directory: str +) -> "tuple[bool, pathlib.Path | None]": + """Given a log file with a specific format, return a corresponding KML file. + + Args: + log_file (str): Path to the log file + document_name_prefix (str): Prefix name for saved KML file. + save_directory (str): Directory to save the KML file to. + + Returns: + tuple[bool, pathlib.Path | None]: Returns (False, None) if function + failed to execute, otherwise (True, path) where path a pathlib.Path + object pointing to the KML file. + """ + locations = [] + + try: + with open(log_file, "r") as f: + for line in f: + # find all the latitudes and longitudes within the line + latitudes = re.findall(r"latitude: (-?\d+\.\d+)", line) + longitudes = re.findall(r"longitude: (-?\d+\.\d+)", line) + + # we must find equal number of latitude and longitude numbers, + # otherwise that means the log file is improperly formatted or + # the script failed to detect all locations + if len(latitudes) != len(longitudes): + print("Number of latitudes and longitudes found are different.") + print(f"# of altitudes: {len(latitudes)}, # of longitudes: {len(longitudes)}") + return False, None + + latitudes = list(map(float, latitudes)) + longitudes = list(map(float, longitudes)) + + for i in range(len(latitudes)): + success, location = LocationGlobal.create(latitudes[i], longitudes[i]) + if not success: + return False, None + locations.append(location) + + return locations_to_kml(locations, document_name_prefix, save_directory) + except Exception as e: + print(e.with_traceback()) + return False, None From 5acada7efea4a9e4344ca234553f1f4e433560a9 Mon Sep 17 00:00:00 2001 From: Evan Zhao <57083990+Evang264@users.noreply.github.com> Date: Thu, 14 Nov 2024 18:04:18 -0500 Subject: [PATCH 11/11] Revert "Merge branch 'main' into log_to_kml" This reverts commit 4b60311fae511e7007c6f429728409578f758051, reversing changes made to 7ae96e0c866f39e3b4b9b1485351519f5f9a7139. --- config.yaml | 5 ---- main_2024.py | 72 +++++++++++++++++++++++++++------------------------- 2 files changed, 38 insertions(+), 39 deletions(-) diff --git a/config.yaml b/config.yaml index 44bdd6df..b0dd9811 100644 --- a/config.yaml +++ b/config.yaml @@ -34,8 +34,3 @@ geolocation: camera_orientation_yaw: 0.0 camera_orientation_pitch: -1.57079632679 camera_orientation_roll: 0.0 - -cluster_estimation: - min_activation_threshold: 25 - min_new_points_to_run: 5 - random_state: 0 diff --git a/main_2024.py b/main_2024.py index 1f357dd7..4e24b517 100644 --- a/main_2024.py +++ b/main_2024.py @@ -20,7 +20,6 @@ from modules.data_merge import data_merge_worker from modules.geolocation import geolocation_worker from modules.geolocation import camera_properties -from modules.cluster_estimation import cluster_estimation_worker from modules.common.modules.logger import logger from modules.common.modules.logger import logger_main_setup from modules.common.modules.read_yaml import read_yaml @@ -113,11 +112,6 @@ def main() -> int: GEOLOCATION_CAMERA_ORIENTATION_YAW = config["geolocation"]["camera_orientation_yaw"] GEOLOCATION_CAMERA_ORIENTATION_PITCH = config["geolocation"]["camera_orientation_pitch"] GEOLOCATION_CAMERA_ORIENTATION_ROLL = config["geolocation"]["camera_orientation_roll"] - - MIN_ACTIVATION_THRESHOLD = config["cluster_estimation"]["min_activation_threshold"] - MIN_NEW_POINTS_TO_RUN = config["cluster_estimation"]["min_new_points_to_run"] - RANDOM_STATE = config["cluster_estimation"]["random_state"] - # pylint: enable=invalid-name except KeyError as exception: main_logger.error(f"Config key(s) not found: {exception}", True) @@ -150,15 +144,15 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( + communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - cluster_estimation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -284,7 +278,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_cluster_estimation_queue], + output_queues=[geolocation_to_communications_queue], controller=controller, local_logger=main_logger, ) @@ -295,21 +289,23 @@ def main() -> int: # Get Pylance to stop complaining assert geolocation_worker_properties is not None - result, cluster_estimation_worker_properties = worker_manager.WorkerProperties.create( + result, communications_worker_properties = worker_manager.WorkerProperties.create( count=1, - target=cluster_estimation_worker.cluster_estimation_worker, - work_arguments=(MIN_ACTIVATION_THRESHOLD, MIN_NEW_POINTS_TO_RUN, RANDOM_STATE), - input_queues=[geolocation_to_cluster_estimation_queue], - output_queues=[cluster_estimation_to_main_queue], + target=communications_worker.communications_worker, + work_arguments=(), + input_queues=[ + flight_interface_to_communications_queue, + geolocation_to_communications_queue, + ], + output_queues=[communications_to_main_queue], controller=controller, local_logger=main_logger, ) if not result: - main_logger.error("Failed to create arguments for Cluster Estimation", True) + main_logger.error("Failed to create arguments for Video Input", True) return -1 - # Get Pylance to stop complaining - assert cluster_estimation_worker_properties is not None + assert communications_worker_properties is not None # Create managers worker_managers = [] @@ -379,18 +375,18 @@ def main() -> int: worker_managers.append(geolocation_manager) - result, cluster_estimation_manager = worker_manager.WorkerManager.create( - worker_properties=cluster_estimation_worker_properties, + result, communications_manager = worker_manager.WorkerManager.create( + worker_properties=communications_worker_properties, local_logger=main_logger, ) if not result: - main_logger.error("Failed to create manager for Cluster Estimation", True) + main_logger.error("Failed to create manager for Communications", True) return -1 # Get Pylance to stop complaining - assert cluster_estimation_manager is not None + assert communications_manager is not None - worker_managers.append(cluster_estimation_manager) + worker_managers.append(communications_manager) # Run for manager in worker_managers: @@ -404,16 +400,24 @@ def main() -> int: return -1 try: - cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() + geolocation_data = communications_to_main_queue.queue.get_nowait() except queue.Empty: - cluster_estimations = None - - if cluster_estimations is not None: - for cluster in cluster_estimations: - main_logger.debug("Cluster in world: " + True) - main_logger.debug("Cluster location x: " + str(cluster.location_x)) - main_logger.debug("Cluster location y: " + str(cluster.location_y)) - main_logger.debug("Cluster spherical variance: " + str(cluster.spherical_variance)) + geolocation_data = None + + if geolocation_data is not None: + for detection_world in geolocation_data: + main_logger.debug("Detection in world:", True) + main_logger.debug( + "geolocation vertices: " + str(detection_world.vertices.tolist()), True + ) + main_logger.debug( + "geolocation centre: " + str(detection_world.centre.tolist()), True + ) + main_logger.debug("geolocation label: " + str(detection_world.label), True) + main_logger.debug( + "geolocation confidence: " + str(detection_world.confidence), True + ) + if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break @@ -426,9 +430,9 @@ def main() -> int: flight_interface_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_communications_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() - geolocation_to_cluster_estimation_queue.fill_and_drain_queue() + geolocation_to_communications_queue.fill_and_drain_queue() + communications_to_main_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() - cluster_estimation_to_main_queue.fill_and_drain_queue() for manager in worker_managers: manager.join_workers()