Integrating-cluster-estimation-worker

config.yaml

@@ -34,3 +34,9 @@ geolocation:
     camera_orientation_yaw: 0.0
     camera_orientation_pitch: -1.57079632679
     camera_orientation_roll: 0.0
+
+# default values set
+cluster_merge:
+    min_activation_threshold: 0
+    min_new_points_to_run: 0
+    random_state: 0

main_2024.py

@@ -19,6 +19,7 @@
 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.logger.modules import logger
 from modules.common.logger.modules import logger_setup_main
 from modules.common.logger.read_yaml.modules import read_yaml
@@ -85,8 +86,8 @@ def main() -> int:
         VIDEO_INPUT_SAVE_PREFIX = str(pathlib.Path(logging_path, VIDEO_INPUT_SAVE_NAME_PREFIX))
 
         DETECT_TARGET_WORKER_COUNT = config["detect_target"]["worker_count"]
-        detect_target_option_int = config["detect_target"]["option"]
-        DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(detect_target_option_int)
+        DETECT_TARGET_OPTION_INT = config["detect_target"]["option"]
+        DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(DETECT_TARGET_OPTION_INT)
         DETECT_TARGET_DEVICE = "cpu" if args.cpu else config["detect_target"]["device"]
         DETECT_TARGET_MODEL_PATH = config["detect_target"]["model_path"]
         DETECT_TARGET_OVERRIDE_FULL_PRECISION = args.full
@@ -111,14 +112,16 @@ 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_merge"]["min_activation_threshold"]
+        MIN_NEW_POINTS_TO_RUN = config["cluster_merge"]["min_new_points_to_run"]
+        RANDOM_STATE = config["cluster_merge"]["random_state"]
         # pylint: enable=invalid-name
     except KeyError as exception:
         main_logger.error(f"Config key(s) not found: {exception}", True)
         return -1
     except ValueError as exception:
-        main_logger.error(
-            f"Could not convert detect target option into enum: {exception}", True
-        )
+        main_logger.error(f"Could not convert detect target option into enum: {exception}", True)
         return -1
 
     # Setup
@@ -141,14 +144,26 @@ def main() -> int:
         mp_manager,
         QUEUE_MAX_SIZE,
     )
-    geolocation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
+    flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        QUEUE_MAX_SIZE,
+    )
+    geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper(
         mp_manager,
         QUEUE_MAX_SIZE,
     )
     flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper(
         mp_manager,
         QUEUE_MAX_SIZE,
     )
+    geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        QUEUE_MAX_SIZE,
+    )
+    cluster_estimation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        QUEUE_MAX_SIZE,
+    )
 
     result, camera_intrinsics = camera_properties.CameraIntrinsics.create(
         GEOLOCATION_RESOLUTION_X,
@@ -230,6 +245,7 @@ def main() -> int:
             FLIGHT_INTERFACE_WORKER_PERIOD,
         ),
         input_queues=[flight_interface_decision_queue],
+        input_queues=[flight_interface_decision_queue],
         output_queues=[flight_interface_to_data_merge_queue],
         controller=controller,
         local_logger=main_logger,
@@ -268,7 +284,7 @@ def main() -> int:
             camera_extrinsics,
         ),
         input_queues=[data_merge_to_geolocation_queue],
-        output_queues=[geolocation_to_main_queue],
+        output_queues=[geolocation_to_cluster_estimation_queue],
         controller=controller,
         local_logger=main_logger,
     )
@@ -279,6 +295,22 @@ def main() -> int:
     # Get Pylance to stop complaining
     assert geolocation_worker_properties is not None
 
+    result, cluster_estimation_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],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        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
+
     # Create managers
     worker_managers = []
 
@@ -347,6 +379,32 @@ def main() -> int:
 
     worker_managers.append(geolocation_manager)
 
+    result, cluster_estimation_manager = worker_manager.WorkerManager.create(
+        worker_properties=cluster_estimation_worker_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create manager for Flight Interface", True)
+        return -1
+
+    # Get Pylance to stop complaining
+    assert cluster_estimation_manager is not None
+
+    worker_managers.append(cluster_estimation_manager)
+
+    result, cluster_estimation_manager = worker_manager.WorkerManager.create(
+        worker_properties=cluster_estimation_worker_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create manager for Flight Interface", True)
+        return -1
+
+    # Get Pylance to stop complaining
+    assert cluster_estimation_manager is not None
+
+    worker_managers.append(cluster_estimation_manager)
+
     # Run
     for manager in worker_managers:
         manager.start_workers()
@@ -359,7 +417,8 @@ def main() -> int:
                 return -1
 
         try:
-            geolocation_data = geolocation_to_main_queue.queue.get_nowait()
+            geolocation_data = geolocation_to_cluster_estimation_queue.queue.get_nowait()
+            geolocation_data = geolocation_to_cluster_estimation_queue.queue.get_nowait()
         except queue.Empty:
             geolocation_data = None
 
@@ -377,6 +436,16 @@ def main() -> int:
                     "geolocation confidence: " + str(detection_world.confidence), True
                 )
 
+        try:
+            cluster_estimations = cluster_estimation_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("Cluser 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))
         if cv2.waitKey(1) == ord("q"):  # type: ignore
             main_logger.info("Exiting main loop", True)
             break
@@ -388,8 +457,11 @@ def main() -> int:
     detect_target_to_data_merge_queue.fill_and_drain_queue()
     flight_interface_to_data_merge_queue.fill_and_drain_queue()
     data_merge_to_geolocation_queue.fill_and_drain_queue()
-    geolocation_to_main_queue.fill_and_drain_queue()
+    geolocation_to_cluster_estimation_queue.fill_and_drain_queue()
+    flight_interface_decision_queue.fill_and_drain_queue()
+    geolocation_to_cluster_estimation_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()

modules/cluster_estimation/cluster_estimation.py

@@ -1,6 +1,6 @@
 """
 Take in bounding box coordinates from Geolocation and use to estimate landing pad locations.
-Returns an array of classes, each containing the x coordinate, y coordinate, and spherical 
+Returns an array of classes, each containing the x coordinate, y coordinate, and spherical
 covariance of each landing pad estimation.
 """
 
@@ -124,7 +124,7 @@ def __init__(
 
     def run(
         self, detections: "list[detection_in_world.DetectionInWorld]", run_override: bool
-    ) -> "tuple[bool, list[object_in_world.ObjectInWorld] | None]":
+    ) -> "tuple[bool, object_in_world.ObjectInWorld | None]":
         """
         Take in list of landing pad detections and return list of estimated landing pad locations
         if number of detections is sufficient, or if manually forced to run.
@@ -193,14 +193,16 @@ def run(
         # Create output list of remaining valid clusters
         detections_in_world = []
         for cluster in model_output:
-            result, landing_pad = object_in_world.ObjectInWorld.create(
+
+            result, temp_object = object_in_world.ObjectInWorld.create(
                 cluster[0][0],
                 cluster[0][1],
                 cluster[2],
+                0,
             )
 
             if result:
-                detections_in_world.append(landing_pad)
+                detections_in_world.append(temp_object)
 
         return True, detections_in_world
 

modules/flight_interface/flight_interface_worker.py

@@ -17,8 +17,8 @@ def flight_interface_worker(
     timeout: float,
     baud_rate: int,
     period: float,
-    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
     input_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
     controller: worker_controller.WorkerController,
 ) -> None:
     """

modules/object_in_world.py

@@ -12,23 +12,27 @@ class ObjectInWorld:
 
     @classmethod
     def create(
-        cls, location_x: float, location_y: float, spherical_variance: float
+        cls, location_x: float, location_y: float, spherical_variance: float, label: int
     ) -> "tuple[bool, ObjectInWorld | None]":
         """
         location_x, location_y: Location of the object.
         spherical_variance: Uncertainty of the location.
+        label: type of object in real world.
         """
         if spherical_variance < 0.0:
             return False, None
 
-        return True, ObjectInWorld(cls.__create_key, location_x, location_y, spherical_variance)
+        return True, ObjectInWorld(
+            cls.__create_key, location_x, location_y, spherical_variance, label
+        )
 
     def __init__(
         self,
         class_private_create_key: object,
         location_x: float,
         location_y: float,
         spherical_variance: float,
+        label: int,
     ) -> None:
         """
         Private constructor, use create() method.
@@ -38,3 +42,4 @@ def __init__(
         self.location_x = location_x
         self.location_y = location_y
         self.spherical_variance = spherical_variance
+        self.label = label

tests/integration/test_flight_interface_worker.py

@@ -112,8 +112,8 @@ def main() -> int:
             FLIGHT_INTERFACE_TIMEOUT,
             FLIGHT_INTERFACE_BAUD_RATE,
             FLIGHT_INTERFACE_WORKER_PERIOD,
-            out_queue,
             in_queue,  # Added input_queue
+            out_queue,
             controller,
         ),
     )

tests/unit/test_decision.py

@@ -41,7 +41,9 @@ def best_pad_within_tolerance() -> object_in_world.ObjectInWorld:  # type: ignor
     location_x = BEST_PAD_LOCATION_X
     location_y = BEST_PAD_LOCATION_Y
     spherical_variance = 1.0
-    result, pad = object_in_world.ObjectInWorld.create(location_x, location_y, spherical_variance)
+    result, pad = object_in_world.ObjectInWorld.create(
+        location_x, location_y, spherical_variance, 0
+    )
     assert result
     assert pad is not None
 
@@ -56,7 +58,9 @@ def best_pad_outside_tolerance() -> object_in_world.ObjectInWorld:  # type: igno
     location_x = 100.0
     location_y = 200.0
     spherical_variance = 5.0  # variance outside tolerance
-    result, pad = object_in_world.ObjectInWorld.create(location_x, location_y, spherical_variance)
+    result, pad = object_in_world.ObjectInWorld.create(
+        location_x, location_y, spherical_variance, 0
+    )
     assert result
     assert pad is not None
 
@@ -68,15 +72,15 @@ def pads() -> "list[object_in_world.ObjectInWorld]":  # type: ignore
     """
     Create a list of ObjectInWorld instances for the landing pads.
     """
-    result, pad_1 = object_in_world.ObjectInWorld.create(30.0, 40.0, 2.0)
+    result, pad_1 = object_in_world.ObjectInWorld.create(30.0, 40.0, 2.0, 0)
     assert result
     assert pad_1 is not None
 
-    result, pad_2 = object_in_world.ObjectInWorld.create(50.0, 60.0, 3.0)
+    result, pad_2 = object_in_world.ObjectInWorld.create(50.0, 60.0, 3.0, 0)
     assert result
     assert pad_2 is not None
 
-    result, pad_3 = object_in_world.ObjectInWorld.create(70.0, 80.0, 4.0)
+    result, pad_3 = object_in_world.ObjectInWorld.create(70.0, 80.0, 4.0, 0)
     assert result
     assert pad_3 is not None