From dfc660066fb5a852df53d7b002cddf84f57a2f73 Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Sun, 10 Nov 2024 02:26:27 -0500 Subject: [PATCH] initial commit --- config.yaml | 5 +++++ main_2024.py | 62 ++++++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 63 insertions(+), 4 deletions(-) diff --git a/config.yaml b/config.yaml index b0dd9811..19bb573b 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 \ No newline at end of file diff --git a/main_2024.py b/main_2024.py index f6297226..860a6815 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,15 @@ 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"] + + 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 +149,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 +157,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 +280,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 +291,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 = [] @@ -345,6 +375,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 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() @@ -357,7 +400,7 @@ 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() except queue.Empty: geolocation_data = None @@ -375,6 +418,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 @@ -386,8 +439,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()