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)