diff --git a/config.yaml b/config.yaml index 44bdd6df..b0dd9811 100644 --- a/config.yaml +++ b/config.yaml @@ -34,8 +34,3 @@ 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 29ec29ad..4e24b517 100644 --- a/main_2024.py +++ b/main_2024.py @@ -12,6 +12,7 @@ # Used in type annotation of flight interface output # pylint: disable-next=unused-import from modules import odometry_and_time +from modules.communications import communications_worker from modules.detect_target import detect_target_factory from modules.detect_target import detect_target_worker from modules.flight_interface import flight_interface_worker @@ -19,7 +20,6 @@ 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 @@ -86,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 @@ -112,11 +112,6 @@ 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) @@ -141,19 +136,23 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + flight_interface_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( + communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - cluster_estimation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -238,7 +237,10 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, ), input_queues=[flight_interface_decision_queue], - output_queues=[flight_interface_to_data_merge_queue], + output_queues=[ + flight_interface_to_data_merge_queue, + flight_interface_to_communications_queue, + ], controller=controller, local_logger=main_logger, ) @@ -276,7 +278,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_cluster_estimation_queue], + output_queues=[geolocation_to_communications_queue], controller=controller, local_logger=main_logger, ) @@ -287,21 +289,23 @@ def main() -> int: # Get Pylance to stop complaining assert geolocation_worker_properties is not None - result, cluster_estimation_worker_properties = worker_manager.WorkerProperties.create( + result, communications_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], + target=communications_worker.communications_worker, + work_arguments=(), + input_queues=[ + flight_interface_to_communications_queue, + geolocation_to_communications_queue, + ], + output_queues=[communications_to_main_queue], controller=controller, local_logger=main_logger, ) if not result: - main_logger.error("Failed to create arguments for Cluster Estimation", True) + 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 + assert communications_worker_properties is not None # Create managers worker_managers = [] @@ -371,18 +375,18 @@ def main() -> int: worker_managers.append(geolocation_manager) - result, cluster_estimation_manager = worker_manager.WorkerManager.create( - worker_properties=cluster_estimation_worker_properties, + result, communications_manager = worker_manager.WorkerManager.create( + worker_properties=communications_worker_properties, local_logger=main_logger, ) if not result: - main_logger.error("Failed to create manager for Cluster Estimation", True) + main_logger.error("Failed to create manager for Communications", True) return -1 # Get Pylance to stop complaining - assert cluster_estimation_manager is not None + assert communications_manager is not None - worker_managers.append(cluster_estimation_manager) + worker_managers.append(communications_manager) # Run for manager in worker_managers: @@ -396,16 +400,24 @@ def main() -> int: return -1 try: - cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() + geolocation_data = communications_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("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)) + 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 + ) + if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break @@ -416,10 +428,11 @@ 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_communications_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() - geolocation_to_cluster_estimation_queue.fill_and_drain_queue() + geolocation_to_communications_queue.fill_and_drain_queue() + communications_to_main_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/communications/communications.py b/modules/communications/communications.py new file mode 100644 index 00000000..642973e5 --- /dev/null +++ b/modules/communications/communications.py @@ -0,0 +1,92 @@ +""" +Logs data and forwards it. +""" + +import time + +from .. import detection_in_world +from ..common.modules.logger import logger +from ..common.modules import position_global +from ..common.modules import position_local +from ..common.modules.mavlink import local_global_conversion + + +class Communications: + """ + Currently logs data only. + """ + + __create_key = object() + + @classmethod + def create( + cls, + home_position: position_global.PositionGlobal, + local_logger: logger.Logger, + ) -> "tuple[bool, Communications | None]": + """ + Logs data and forwards it. + + home_location: Take-off location of drone. + + Returns: Success, class object. + """ + + return True, Communications(cls.__create_key, home_position, local_logger) + + def __init__( + self, + class_private_create_key: object, + home_position: position_global.PositionGlobal, + 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_position + self.__logger = local_logger + + def run( + self, detections_in_world: list[detection_in_world.DetectionInWorld] + ) -> tuple[bool, list[detection_in_world.DetectionInWorld] | None]: + + detections_in_world_global = [] + for detection_in_world in detections_in_world: + # TODO: Change this when the conversion interface is changed + north = detection_in_world.centre[0] + east = detection_in_world.centre[1] + down = 0 + + result, drone_position_local = position_local.PositionLocal.create( + north, + east, + down, + ) + + if not result: + self.__logger.warning( + f"Could not convert DetectionInWorld to PositionLocal:\ndetection in world: {detection_in_world}" + ) + return False, None + + result, detection_in_world_global = ( + local_global_conversion.position_global_from_position_local( + self.__home_location, drone_position_local + ) + ) + + if not result: + # Log nothing if at least one of the conversions failed + self.__logger.warning( + f"drone_position_global_from_local conversion failed:\nhome_location: {self.__home_location}\ndrone_position_local: {drone_position_local}" + ) + return False, None + + 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 new file mode 100644 index 00000000..246387ae --- /dev/null +++ b/modules/communications/communications_worker.py @@ -0,0 +1,56 @@ +""" +Logs data and forwards it. +""" + +import os +import pathlib + +from . import communications +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from ..common.modules.logger import logger + + +def communications_worker( + home_position_queue: queue_proxy_wrapper.QueueProxyWrapper, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + + home_position: get home_position for init + """ + + worker_name = pathlib.Path(__file__).stem + process_id = os.getpid() + result, local_logger = logger.Logger.create(f"{worker_name}_{process_id}", True) + if not result: + print("ERROR: Worker failed to create logger") + return + + # Get Pylance to stop complaining + assert local_logger is not None + + local_logger.info("Logger initialized", True) + + # Get home location + home_position = home_position_queue.queue.get() + + result, comm = communications.Communications.create(home_position, 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 + + while not controller.is_exit_requested(): + controller.check_pause() + + result, value = comm.run(input_queue.queue.get()) + if not result: + continue + + output_queue.queue.put(value) diff --git a/modules/communications/log_to_kml.py b/modules/communications/log_to_kml.py new file mode 100644 index 00000000..f62ad82f --- /dev/null +++ b/modules/communications/log_to_kml.py @@ -0,0 +1,56 @@ +""" +Convert log file to KML file. +""" + +import pathlib +import re + +from modules.common.modules.kml.locations_to_kml import locations_to_kml +from modules.common.modules.location_global import LocationGlobal + + +def convert_log_to_kml( + log_file: str, document_name_prefix: str, save_directory: str +) -> "tuple[bool, pathlib.Path | None]": + """Given a log file with a specific format, return a corresponding KML file. + + Args: + log_file (str): Path to the log file + document_name_prefix (str): Prefix name for saved KML file. + save_directory (str): Directory to save the KML file to. + + Returns: + tuple[bool, pathlib.Path | None]: Returns (False, None) if function + failed to execute, otherwise (True, path) where path a pathlib.Path + object pointing to the KML file. + """ + locations = [] + + try: + with open(log_file, "r") as f: + for line in f: + # find all the latitudes and longitudes within the line + latitudes = re.findall(r"latitude: (-?\d+\.\d+)", line) + longitudes = re.findall(r"longitude: (-?\d+\.\d+)", line) + + # we must find equal number of latitude and longitude numbers, + # otherwise that means the log file is improperly formatted or + # the script failed to detect all locations + if len(latitudes) != len(longitudes): + print("Number of latitudes and longitudes found are different.") + print(f"# of altitudes: {len(latitudes)}, # of longitudes: {len(longitudes)}") + return False, None + + latitudes = list(map(float, latitudes)) + longitudes = list(map(float, longitudes)) + + for i in range(len(latitudes)): + success, location = LocationGlobal.create(latitudes[i], longitudes[i]) + if not success: + return False, None + locations.append(location) + + return locations_to_kml(locations, document_name_prefix, save_directory) + except Exception as e: + print(e.with_traceback()) + return False, None 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/detections_and_time.py b/modules/detections_and_time.py index fcedcd9e..75df2e02 100644 --- a/modules/detections_and_time.py +++ b/modules/detections_and_time.py @@ -57,6 +57,12 @@ def __str__(self) -> str: """ return f"cls: {self.label}, conf: {self.confidence}, bounds: {self.x_1} {self.y_1} {self.x_2} {self.y_2}" + def __repr__(self) -> str: + """ + For collections (e.g. list). + """ + return str(self) + def get_centre(self) -> "tuple[float, float]": """ Gets the xy centre of the bounding box. diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index e1b47d86..d497cb39 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -67,6 +67,12 @@ def __init__( self.__home_position = home_position self.__logger = local_logger + def get_home_position(self) -> position_global.PositionGlobal: + """ + Accessor for home position. + """ + return self.__home_position + def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ Returns a possible OdometryAndTime with current timestamp. diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 2ead7bae..41610a73 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -19,6 +19,7 @@ def flight_interface_worker( period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, + communications_output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -53,6 +54,9 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None + home_position = interface.get_home_position() + communications_output_queue.queue.put(home_position) + while not controller.is_exit_requested(): controller.check_pause() 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)