Modify home location to be part of create for flight interface and co…

…mmunication worker

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()

modules/communications/communications.py

@@ -1,50 +1,60 @@
 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
 from modules.flight_interface.local_global_conversion import detection_in_world_global_from_local
 
 
 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:
         """
         Private constructor, use create() method.
         """
         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

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,22 +35,21 @@ 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
 
     # 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
 

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)

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:
         """

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():

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)