From fe018af26c62d7e15e9959a2e333c7c084db8090 Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Thu, 14 Nov 2024 16:59:16 -0500 Subject: [PATCH] cluster-estimation-integration-redo-redo (#221) * initial commit * fixed implemented changes * fixed formatting * fixed changes * fixed typo --------- Co-authored-by: Cyuber --- config.yaml | 5 ++++ main_2024.py | 72 ++++++++++++++++++++++++++++++++++++-------------- modules/common | 2 +- 3 files changed, 58 insertions(+), 21 deletions(-) diff --git a/config.yaml b/config.yaml index b0dd9811..44bdd6df 100644 --- a/config.yaml +++ b/config.yaml @@ -34,3 +34,8 @@ 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 f6297226..29ec29ad 100644 --- a/main_2024.py +++ b/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.modules.logger import logger from modules.common.modules.logger import logger_main_setup from modules.common.modules.read_yaml import read_yaml @@ -111,6 +112,11 @@ 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) @@ -139,7 +145,7 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -147,6 +153,10 @@ def main() -> int: 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, @@ -266,7 +276,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, ) @@ -277,6 +287,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 Cluster Estimation", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_worker_properties is not None + # Create managers worker_managers = [] @@ -345,6 +371,19 @@ 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 Cluster Estimation", 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() @@ -357,24 +396,16 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_main_queue.queue.get_nowait() + cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() except queue.Empty: - 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 - ) - + 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)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break @@ -386,8 +417,9 @@ 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() + cluster_estimation_to_main_queue.fill_and_drain_queue() for manager in worker_managers: manager.join_workers() 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