From 4ef1755962485cafaa2b1c993e0f2e48f4e441e3 Mon Sep 17 00:00:00 2001 From: Davis Liu Date: Thu, 31 Oct 2024 21:01:14 -0400 Subject: [PATCH 01/31] initial commit --- main_2024.py | 11 +++- modules/communications/communications.py | 58 +++++++++++++++++++ .../communications/communications_worker.py | 58 +++++++++++++++++++ modules/flight_interface/flight_interface.py | 6 +- .../flight_interface_worker.py | 6 +- 5 files changed, 134 insertions(+), 5 deletions(-) create mode 100644 modules/communications/communications.py create mode 100644 modules/communications/communications_worker.py diff --git a/main_2024.py b/main_2024.py index 29ec29ad..d1c9a172 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 @@ -141,6 +142,10 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + flight_interface_to_communcations_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, @@ -238,7 +243,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_communcations_queue + ], controller=controller, local_logger=main_logger, ) @@ -416,6 +424,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() data_merge_to_geolocation_queue.fill_and_drain_queue() geolocation_to_cluster_estimation_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() diff --git a/modules/communications/communications.py b/modules/communications/communications.py new file mode 100644 index 00000000..be1ee81c --- /dev/null +++ b/modules/communications/communications.py @@ -0,0 +1,58 @@ +import time +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 drone_position_global_from_local + + +class Communications: + """ + """ + __create_key = object() + + @classmethod + def create( + cls, + local_logger: logger.Logger, + ) -> "tuple[bool, Communications | None]": + """ + Logs data and forwards it. + """ + + return True, Communications(cls.__create_key, local_logger) + + def __init__( + self, + class_private_create_key: object, + local_logger: logger.Logger, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is Communications.__create_key, "Use create() method" + + self.__logger = local_logger + + def run( + self, detections_in_world: list[DetectionInWorld], home_location: DronePosition + ) -> tuple[bool, list[DetectionInWorld] | None]: + for detection_in_world in detections_in_world: + if len(detection_in_world.centre) == 3: + result, detection_in_world_global = drone_position_global_from_local( + home_location, + detection_in_world.centre[0], + detection_in_world.centre[1], + detection_in_world.centre[2], + ) + elif len(detection_in_world.centre) == 2: + result, detection_in_world_global = drone_position_global_from_local( + home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 + ) + + if not result: + self.__logger.error("conversion failed") + return False, detections_in_world + + self.__logger.info(time.time() + ": " + str(detection_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..e22aacfa --- /dev/null +++ b/modules/communications/communications_worker.py @@ -0,0 +1,58 @@ +""" +Logs data and forwards it. +""" + +import os +import pathlib + +from modules.communications import communications +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from ..common.logger.modules import logger + + +def communications_worker( + home_location_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_location: get home_location for init + """ + # TODO: Error handling + + 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) + + result, comm = communications.Communications.create(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) + if not result: + continue + + output_queue.queue.put(value) diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index e1b47d86..bc637886 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -69,7 +69,7 @@ def __init__( def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ - Returns a possible OdometryAndTime with current timestamp. + Returns a possible OdometryAndTime with current timestamp and home location. """ result, odometry = self.controller.get_odometry() if not result: @@ -83,7 +83,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": odometry, ) if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry_local is not None @@ -97,7 +97,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": self.__logger.info(str(odometry_and_time_object), True) - return True, odometry_and_time_object + return True, odometry_and_time_object, self.__home_location def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 2ead7bae..e1fb3f51 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_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -53,16 +54,19 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None + home_location_sent = False while not controller.is_exit_requested(): controller.check_pause() time.sleep(period) - result, value = interface.run() + result, value, home_location = interface.run() if not result: continue output_queue.queue.put(value) + if not home_location_sent: + communications_ouutput_queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From 99f2d28f4105557cad6f64e572dc5547847cbc12 Mon Sep 17 00:00:00 2001 From: Ashish Agrahari Date: Fri, 1 Nov 2024 14:06:45 -0400 Subject: [PATCH 02/31] Made fixes --- modules/communications/communications.py | 20 ++++++------------- .../communications/communications_worker.py | 2 +- modules/flight_interface/flight_interface.py | 2 +- .../flight_interface_worker.py | 4 ++-- 4 files changed, 10 insertions(+), 18 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index be1ee81c..9396ca81 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -2,7 +2,7 @@ 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 drone_position_global_from_local +from modules.flight_interface.local_global_conversion import detection_in_world_global_from_local class Communications: @@ -37,22 +37,14 @@ def run( self, detections_in_world: list[DetectionInWorld], home_location: DronePosition ) -> tuple[bool, list[DetectionInWorld] | None]: for detection_in_world in detections_in_world: - if len(detection_in_world.centre) == 3: - result, detection_in_world_global = drone_position_global_from_local( - home_location, - detection_in_world.centre[0], - detection_in_world.centre[1], - detection_in_world.centre[2], - ) - elif len(detection_in_world.centre) == 2: - result, detection_in_world_global = drone_position_global_from_local( - home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 - ) + result, detection_in_world_global = detection_in_world_global_from_local( + home_location, detection_in_world + ) if not result: self.__logger.error("conversion failed") return False, detections_in_world - self.__logger.info(time.time() + ": " + str(detection_in_world_global)) - + self.__logger.info(str(time.time()) + ": " + str(detection_in_world_global)) + return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index e22aacfa..2ba5e1bf 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.communications import communications +from . import communications from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from ..common.logger.modules import logger diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index bc637886..55fc8007 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -73,7 +73,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ result, odometry = self.controller.get_odometry() if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry is not None diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index e1fb3f51..5a6b4aeb 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -19,7 +19,7 @@ def flight_interface_worker( period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, - communications_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, + communications_output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -66,7 +66,7 @@ def flight_interface_worker( output_queue.queue.put(value) if not home_location_sent: - communications_ouutput_queue.put(home_location) + communications_output_queue.queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From b50d71435a309a86435a1d543a2232c90e4ada40 Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Fri, 1 Nov 2024 23:16:31 -0400 Subject: [PATCH 03/31] Modify home location to be part of create for flight interface and communication worker --- main_2024.py | 6 ++--- modules/communications/communications.py | 22 ++++++++++++++----- .../communications/communications_worker.py | 12 +++++----- modules/flight_interface/flight_interface.py | 18 ++++++++++----- .../flight_interface_worker.py | 8 +++---- .../test_flight_interface_worker.py | 4 ++++ 6 files changed, 44 insertions(+), 26 deletions(-) diff --git a/main_2024.py b/main_2024.py index d1c9a172..a4d1534b 100644 --- a/main_2024.py +++ b/main_2024.py @@ -142,7 +142,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, ) @@ -245,7 +245,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, @@ -424,7 +424,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_cluster_estimation_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() diff --git a/modules/communications/communications.py b/modules/communications/communications.py index 9396ca81..cea5c122 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,4 +1,5 @@ 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 @@ -6,24 +7,26 @@ 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: """ @@ -31,20 +34,27 @@ def __init__( """ 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 diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 2ba5e1bf..d7461563 100644 --- a/modules/communications/communications_worker.py +++ b/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,7 +35,10 @@ 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 @@ -44,14 +46,10 @@ def communications_worker( # 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 diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 55fc8007..17088f7a 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -45,9 +45,9 @@ def create( return False, None # Get Pylance to stop complaining - assert home_position is not None + assert home_location is not None - local_logger.info(str(home_position), True) + local_logger.info(f"Home location: {home_location}", True) return True, FlightInterface(cls.__create_key, controller, home_position, local_logger) @@ -67,13 +67,19 @@ def __init__( self.__home_position = home_position self.__logger = local_logger + 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]": """ - 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]": odometry, ) 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]": 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: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 5a6b4aeb..609694bc 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/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(): 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) From 39c55f0e865cd0e5793c36091ec605a4d0fc6dd0 Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Sat, 2 Nov 2024 00:07:15 -0400 Subject: [PATCH 04/31] combined all logging together --- modules/communications/communications.py | 53 +++++++++++++++++++----- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index cea5c122..ef0cb3a7 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,13 +1,20 @@ +""" +Logs data and forwards it. +""" + import time + +from .. import detection_in_world +from ..common.logger.modules import logger 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 +from ..common.mavlink.modules import drone_odometry_local +from ..common.mavlink.modules import local_global_conversion class Communications: - """ """ + """ + Currently logs data only. + """ __create_key = object() @@ -19,6 +26,10 @@ def create( ) -> "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_location, local_logger) @@ -38,19 +49,39 @@ def __init__( self.__logger = local_logger def run( - self, detections_in_world: list[DetectionInWorld] - ) -> tuple[bool, list[DetectionInWorld] | None]: + 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: - result, detection_in_world_global = detection_in_world_global_from_local( - self.__home_location, detection_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 = drone_odometry_local.DronePositionLocal.create( + north, + east, + down, + ) + if not result: + self.__logger.warning( + f"Could not convert DetectionInWorld to DronePositionLocal:\ndetection in world: {detection_in_world}" + ) + return False, None + + result, detection_in_world_global = ( + local_global_conversion.drone_position_global_from_local( + self.__home_location, drone_position_local + ) ) 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.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) From 1bd2cb6cdf8fb4236c946272e78d0b0ce7ea5076 Mon Sep 17 00:00:00 2001 From: Evan Zhao <57083990+Evang264@users.noreply.github.com> Date: Thu, 14 Nov 2024 17:26:15 -0500 Subject: [PATCH 05/31] Add log_to_kml.py --- config.yaml | 5 -- main_2024.py | 72 +++++++++++++++------------- modules/communications/log_to_kml.py | 56 ++++++++++++++++++++++ 3 files changed, 94 insertions(+), 39 deletions(-) create mode 100644 modules/communications/log_to_kml.py 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 a4d1534b..05ea8430 100644 --- a/main_2024.py +++ b/main_2024.py @@ -20,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 @@ -113,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) @@ -150,15 +144,15 @@ def main() -> int: 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, ) @@ -284,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, ) @@ -295,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 = [] @@ -379,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: @@ -404,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 @@ -426,9 +430,9 @@ def main() -> int: 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/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 From 89a72217d5b582b4b7689a075572b74916c278ab Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 20 Nov 2024 15:58:10 -0500 Subject: [PATCH 06/31] Refactor for common changes --- modules/common | 2 +- modules/communications/log_to_kml.py | 2 +- modules/flight_interface/flight_interface.py | 10 +++++----- modules/flight_interface/flight_interface_worker.py | 4 ++-- tests/integration/test_flight_interface_worker.py | 8 ++++---- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/modules/common b/modules/common index a0aac8ce..a256a497 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit a0aac8ce29273a6a1ca397a2229770add760835e +Subproject commit a256a49778d1154e03683c3b5e2fe6cb215d00e7 diff --git a/modules/communications/log_to_kml.py b/modules/communications/log_to_kml.py index f62ad82f..fa851020 100644 --- a/modules/communications/log_to_kml.py +++ b/modules/communications/log_to_kml.py @@ -5,7 +5,7 @@ import pathlib import re -from modules.common.modules.kml.locations_to_kml import locations_to_kml +from modules.common.modules.kml.kml_conversion import locations_to_kml from modules.common.modules.location_global import LocationGlobal diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 17088f7a..ef976278 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -45,9 +45,9 @@ def create( return False, None # Get Pylance to stop complaining - assert home_location is not None + assert home_position is not None - local_logger.info(f"Home location: {home_location}", True) + local_logger.info(f"Home position: {home_position}", True) return True, FlightInterface(cls.__create_key, controller, home_position, local_logger) @@ -67,11 +67,11 @@ def __init__( self.__home_position = home_position self.__logger = local_logger - def get_home_location(self) -> drone_odometry.DronePosition: + def get_home_position(self) -> position_global.PositionGlobal: """ - Accessor for home location. + Accessor for home position. """ - return self.__home_location + return self.__home_position def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 609694bc..41610a73 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -54,8 +54,8 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None - home_location = interface.get_home_location() - communications_output_queue.queue.put(home_location) + 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 aaebbc00..af52cc9d 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -103,7 +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) + home_position_out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) in_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) worker = mp.Process( @@ -115,7 +115,7 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, in_queue, # Added input_queue out_queue, - home_location_out_queue, + home_position_out_queue, controller, ), ) @@ -126,8 +126,8 @@ def main() -> int: time.sleep(3) # Test - home_location = home_location_out_queue.queue.get() - assert home_location is not None + home_position = home_position_out_queue.queue.get() + assert home_position is not None # Run the apply_decision tests test_result = apply_decision_test(in_queue, out_queue) From 0e8263d2b6de034327a3448336475651e20858a8 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 16:52:03 -0500 Subject: [PATCH 07/31] Fixed communications module imports and implemented object position local-global conversion. --- modules/communications/communications.py | 48 +++++++++---------- .../communications/communications_worker.py | 12 ++--- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index ef0cb3a7..cdabc8d6 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -4,11 +4,11 @@ import time -from .. import detection_in_world -from ..common.logger.modules import logger -from ..common.mavlink.modules import drone_odometry -from ..common.mavlink.modules import drone_odometry_local -from ..common.mavlink.modules import local_global_conversion +from .. import object_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: @@ -21,23 +21,23 @@ class Communications: @classmethod def create( cls, - home_location: drone_odometry.DronePosition, + 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. + home_position: Take-off position of drone. Returns: Success, class object. """ - return True, Communications(cls.__create_key, home_location, local_logger) + return True, Communications(cls.__create_key, home_position, local_logger) def __init__( self, class_private_create_key: object, - home_location: drone_odometry.DronePosition, + home_position: position_global.PositionGlobal, local_logger: logger.Logger, ) -> None: """ @@ -45,47 +45,47 @@ def __init__( """ assert class_private_create_key is Communications.__create_key, "Use create() method" - self.__home_location = home_location + self.__home_position = 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]: + self, objects_in_world: list[object_in_world.ObjectInWorld], + ) -> tuple[bool, list[object_in_world.ObjectInWorld] | None]: - detections_in_world_global = [] - for detection_in_world in detections_in_world: + objects_in_world_global = [] + for object_in_world in objects_in_world: # TODO: Change this when the conversion interface is changed - north = detection_in_world.centre[0] - east = detection_in_world.centre[1] + north = object_in_world.location_x + east = object_in_world.location_y down = 0 - result, drone_position_local = drone_odometry_local.DronePositionLocal.create( + result, object_position_local = position_local.PositionLocal.create( north, east, down, ) if not result: self.__logger.warning( - f"Could not convert DetectionInWorld to DronePositionLocal:\ndetection in world: {detection_in_world}" + f"Could not convert ObjectInWorld to PositionLocal:\object in world: {object_in_world}" ) return False, None - result, detection_in_world_global = ( + result, object_in_world_global = ( local_global_conversion.drone_position_global_from_local( - self.__home_location, drone_position_local + self.__home_position, object_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}" + f"drone_position_global_from_local conversion failed:\nhome_position: {self.__home_position}\ndrone_position_local: {object_position_local}" ) return False, None - detections_in_world_global.append(detection_in_world_global) + objects_in_world_global.append(object_in_world_global) timestamp = time.time() - self.__logger.info(f"{timestamp}: {detections_in_world_global}") + self.__logger.info(f"{timestamp}: {objects_in_world_global}") - return True, detections_in_world + return True, objects_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index d7461563..54aea36f 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -8,11 +8,11 @@ from . import communications from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller -from ..common.logger.modules import logger +from ..common.modules.logger import logger def communications_worker( - home_location_queue: queue_proxy_wrapper.QueueProxyWrapper, + home_position_queue: queue_proxy_wrapper.QueueProxyWrapper, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, @@ -20,7 +20,7 @@ def communications_worker( """ Worker process. - home_location: get home_location for init + home_position: get home_position for init """ worker_name = pathlib.Path(__file__).stem @@ -35,10 +35,10 @@ def communications_worker( local_logger.info("Logger initialized", True) - # Get home location - home_location = home_location_queue.queue.get() + # Get home position + home_position = home_position_queue.queue.get() - result, comm = communications.Communications.create(home_location, local_logger) + result, comm = communications.Communications.create(home_position, local_logger) if not result: local_logger.error("Worker failed to create class object", True) return From de1800244655bee5e317b4b284b2afeb7e9c9f30 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 16:52:40 -0500 Subject: [PATCH 08/31] Added communications logger for logging cluster estimation --- config.yaml | 5 ++++ main_2024.py | 76 ++++++++++++++++++++++++++++++++-------------------- 2 files changed, 52 insertions(+), 29 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 05ea8430..da8423ce 100644 --- a/main_2024.py +++ b/main_2024.py @@ -20,6 +20,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 @@ -112,6 +113,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) @@ -144,19 +150,22 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( + cluster_estimation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) + communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - result, camera_intrinsics = camera_properties.CameraIntrinsics.create( GEOLOCATION_RESOLUTION_X, GEOLOCATION_RESOLUTION_Y, @@ -278,7 +287,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_communications_queue], + output_queues=[geolocation_to_cluster_estimation_queue], controller=controller, local_logger=main_logger, ) @@ -289,13 +298,29 @@ 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_communications_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 + result, communications_worker_properties = worker_manager.WorkerProperties.create( count=1, target=communications_worker.communications_worker, work_arguments=(), input_queues=[ flight_interface_to_communications_queue, - geolocation_to_communications_queue, + cluster_estimation_to_communications_queue, ], output_queues=[communications_to_main_queue], controller=controller, @@ -375,18 +400,18 @@ def main() -> int: worker_managers.append(geolocation_manager) - result, communications_manager = worker_manager.WorkerManager.create( - worker_properties=communications_worker_properties, + 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 Communications", True) + main_logger.error("Failed to create manager for Cluster Estimation", True) return -1 # Get Pylance to stop complaining - assert communications_manager is not None + assert cluster_estimation_manager is not None - worker_managers.append(communications_manager) + worker_managers.append(cluster_estimation_manager) # Run for manager in worker_managers: @@ -400,24 +425,16 @@ def main() -> int: return -1 try: - geolocation_data = communications_to_main_queue.queue.get_nowait() + cluster_estimations = cluster_estimation_to_communications_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 @@ -430,7 +447,8 @@ def main() -> int: 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_communications_queue.fill_and_drain_queue() + geolocation_to_cluster_estimation_queue.fill_and_drain_queue() + cluster_estimation_to_communications_queue.fill_and_drain_queue() communications_to_main_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() From a0b05602348049daf3641b7ecc7fb44fd91ac4ba Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 16:58:16 -0500 Subject: [PATCH 09/31] Formatted python --- modules/communications/communications.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index cdabc8d6..a4300c35 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -49,7 +49,8 @@ def __init__( self.__logger = local_logger def run( - self, objects_in_world: list[object_in_world.ObjectInWorld], + self, + objects_in_world: list[object_in_world.ObjectInWorld], ) -> tuple[bool, list[object_in_world.ObjectInWorld] | None]: objects_in_world_global = [] From 11733479c9ba5b6eb1244ab51105728696820e2a Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:20:20 -0500 Subject: [PATCH 10/31] Added communcations worker manager --- main_2024.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/main_2024.py b/main_2024.py index da8423ce..9bff1a2d 100644 --- a/main_2024.py +++ b/main_2024.py @@ -327,7 +327,7 @@ def main() -> int: local_logger=main_logger, ) if not result: - main_logger.error("Failed to create arguments for Video Input", True) + main_logger.error("Failed to create arguments for Communications Worker", True) return -1 assert communications_worker_properties is not None @@ -412,6 +412,19 @@ def main() -> int: assert cluster_estimation_manager is not None worker_managers.append(cluster_estimation_manager) + + 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 Communications Worker", True) + return -1 + + # Get Pylance to stop complaining + assert communications_manager is not None + + worker_managers.append(communications_manager) # Run for manager in worker_managers: @@ -431,7 +444,7 @@ def main() -> int: if cluster_estimations is not None: for cluster in cluster_estimations: - main_logger.debug("Cluster in world: " + True) + 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)) From 7cf466751fc7c6cdb8f881c7a65650a88159de43 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:21:12 -0500 Subject: [PATCH 11/31] Cluster estimations takes from communication to main queue --- main_2024.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main_2024.py b/main_2024.py index 9bff1a2d..b91f8b91 100644 --- a/main_2024.py +++ b/main_2024.py @@ -438,7 +438,7 @@ def main() -> int: return -1 try: - cluster_estimations = cluster_estimation_to_communications_queue.queue.get_nowait() + cluster_estimations = communications_to_main_queue.queue.get_nowait() except queue.Empty: cluster_estimations = None From 16cc61966cc1ae2c6d1844c1fd31f7c7f69cfeea Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:43:48 -0500 Subject: [PATCH 12/31] Fixed communications worker --- main_2024.py | 6 +++--- modules/communications/communications.py | 2 +- modules/communications/communications_worker.py | 1 + 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/main_2024.py b/main_2024.py index b91f8b91..5a949aa1 100644 --- a/main_2024.py +++ b/main_2024.py @@ -87,8 +87,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 @@ -418,7 +418,7 @@ def main() -> int: local_logger=main_logger, ) if not result: - main_logger.error("Failed to create manager for Communications Worker", True) + main_logger.error("Failed to create manager for Communications", True) return -1 # Get Pylance to stop complaining diff --git a/modules/communications/communications.py b/modules/communications/communications.py index a4300c35..b4bbdc63 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -72,7 +72,7 @@ def run( return False, None result, object_in_world_global = ( - local_global_conversion.drone_position_global_from_local( + local_global_conversion.position_global_from_position_local( self.__home_position, object_position_local ) ) diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 54aea36f..84b2a938 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -37,6 +37,7 @@ def communications_worker( # Get home position home_position = home_position_queue.queue.get() + local_logger.info(f"Home position received: {home_position}", True) result, comm = communications.Communications.create(home_position, local_logger) if not result: From 8ff6035d689f8c8f8be3d86b0e0a814517d065f0 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:43:59 -0500 Subject: [PATCH 13/31] Reformatted kml conversion --- main_2024.py | 2 +- modules/communications/log_to_kml.py | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/main_2024.py b/main_2024.py index 5a949aa1..e2fa0bb8 100644 --- a/main_2024.py +++ b/main_2024.py @@ -412,7 +412,7 @@ def main() -> int: assert cluster_estimation_manager is not None worker_managers.append(cluster_estimation_manager) - + result, communications_manager = worker_manager.WorkerManager.create( worker_properties=communications_worker_properties, local_logger=main_logger, diff --git a/modules/communications/log_to_kml.py b/modules/communications/log_to_kml.py index fa851020..256d18b7 100644 --- a/modules/communications/log_to_kml.py +++ b/modules/communications/log_to_kml.py @@ -5,8 +5,8 @@ import pathlib import re -from modules.common.modules.kml.kml_conversion import locations_to_kml -from modules.common.modules.location_global import LocationGlobal +from modules.common.modules.kml import kml_conversion +from modules.common.modules import location_global def convert_log_to_kml( @@ -45,12 +45,14 @@ def convert_log_to_kml( longitudes = list(map(float, longitudes)) for i in range(len(latitudes)): - success, location = LocationGlobal.create(latitudes[i], longitudes[i]) + success, location = location_global.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) + return kml_conversion.locations_to_kml(locations, document_name_prefix, save_directory) except Exception as e: print(e.with_traceback()) return False, None From 7562a889c66faf203a0f99f8a9248cc6887485c2 Mon Sep 17 00:00:00 2001 From: Davis Liu Date: Thu, 31 Oct 2024 21:01:14 -0400 Subject: [PATCH 14/31] initial commit --- main_2024.py | 11 +++- modules/communications/communications.py | 58 +++++++++++++++++++ .../communications/communications_worker.py | 58 +++++++++++++++++++ modules/flight_interface/flight_interface.py | 6 +- .../flight_interface_worker.py | 6 +- 5 files changed, 134 insertions(+), 5 deletions(-) create mode 100644 modules/communications/communications.py create mode 100644 modules/communications/communications_worker.py diff --git a/main_2024.py b/main_2024.py index 29ec29ad..d1c9a172 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 @@ -141,6 +142,10 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + flight_interface_to_communcations_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, @@ -238,7 +243,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_communcations_queue + ], controller=controller, local_logger=main_logger, ) @@ -416,6 +424,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() data_merge_to_geolocation_queue.fill_and_drain_queue() geolocation_to_cluster_estimation_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() diff --git a/modules/communications/communications.py b/modules/communications/communications.py new file mode 100644 index 00000000..be1ee81c --- /dev/null +++ b/modules/communications/communications.py @@ -0,0 +1,58 @@ +import time +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 drone_position_global_from_local + + +class Communications: + """ + """ + __create_key = object() + + @classmethod + def create( + cls, + local_logger: logger.Logger, + ) -> "tuple[bool, Communications | None]": + """ + Logs data and forwards it. + """ + + return True, Communications(cls.__create_key, local_logger) + + def __init__( + self, + class_private_create_key: object, + local_logger: logger.Logger, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is Communications.__create_key, "Use create() method" + + self.__logger = local_logger + + def run( + self, detections_in_world: list[DetectionInWorld], home_location: DronePosition + ) -> tuple[bool, list[DetectionInWorld] | None]: + for detection_in_world in detections_in_world: + if len(detection_in_world.centre) == 3: + result, detection_in_world_global = drone_position_global_from_local( + home_location, + detection_in_world.centre[0], + detection_in_world.centre[1], + detection_in_world.centre[2], + ) + elif len(detection_in_world.centre) == 2: + result, detection_in_world_global = drone_position_global_from_local( + home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 + ) + + if not result: + self.__logger.error("conversion failed") + return False, detections_in_world + + self.__logger.info(time.time() + ": " + str(detection_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..e22aacfa --- /dev/null +++ b/modules/communications/communications_worker.py @@ -0,0 +1,58 @@ +""" +Logs data and forwards it. +""" + +import os +import pathlib + +from modules.communications import communications +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from ..common.logger.modules import logger + + +def communications_worker( + home_location_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_location: get home_location for init + """ + # TODO: Error handling + + 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) + + result, comm = communications.Communications.create(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) + if not result: + continue + + output_queue.queue.put(value) diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index e1b47d86..bc637886 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -69,7 +69,7 @@ def __init__( def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ - Returns a possible OdometryAndTime with current timestamp. + Returns a possible OdometryAndTime with current timestamp and home location. """ result, odometry = self.controller.get_odometry() if not result: @@ -83,7 +83,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": odometry, ) if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry_local is not None @@ -97,7 +97,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": self.__logger.info(str(odometry_and_time_object), True) - return True, odometry_and_time_object + return True, odometry_and_time_object, self.__home_location def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 2ead7bae..e1fb3f51 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_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -53,16 +54,19 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None + home_location_sent = False while not controller.is_exit_requested(): controller.check_pause() time.sleep(period) - result, value = interface.run() + result, value, home_location = interface.run() if not result: continue output_queue.queue.put(value) + if not home_location_sent: + communications_ouutput_queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From 5599ccc8d66a783deae26b22ce47e32f1765dbe3 Mon Sep 17 00:00:00 2001 From: Ashish Agrahari Date: Fri, 1 Nov 2024 14:06:45 -0400 Subject: [PATCH 15/31] Made fixes --- modules/communications/communications.py | 20 ++++++------------- .../communications/communications_worker.py | 2 +- modules/flight_interface/flight_interface.py | 2 +- .../flight_interface_worker.py | 4 ++-- 4 files changed, 10 insertions(+), 18 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index be1ee81c..9396ca81 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -2,7 +2,7 @@ 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 drone_position_global_from_local +from modules.flight_interface.local_global_conversion import detection_in_world_global_from_local class Communications: @@ -37,22 +37,14 @@ def run( self, detections_in_world: list[DetectionInWorld], home_location: DronePosition ) -> tuple[bool, list[DetectionInWorld] | None]: for detection_in_world in detections_in_world: - if len(detection_in_world.centre) == 3: - result, detection_in_world_global = drone_position_global_from_local( - home_location, - detection_in_world.centre[0], - detection_in_world.centre[1], - detection_in_world.centre[2], - ) - elif len(detection_in_world.centre) == 2: - result, detection_in_world_global = drone_position_global_from_local( - home_location, detection_in_world.centre[0], detection_in_world.centre[1], 0 - ) + result, detection_in_world_global = detection_in_world_global_from_local( + home_location, detection_in_world + ) if not result: self.__logger.error("conversion failed") return False, detections_in_world - self.__logger.info(time.time() + ": " + str(detection_in_world_global)) - + self.__logger.info(str(time.time()) + ": " + str(detection_in_world_global)) + return True, detections_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index e22aacfa..2ba5e1bf 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.communications import communications +from . import communications from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from ..common.logger.modules import logger diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index bc637886..55fc8007 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -73,7 +73,7 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ result, odometry = self.controller.get_odometry() if not result: - return False, None + return False, None, None # Get Pylance to stop complaining assert odometry is not None diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index e1fb3f51..5a6b4aeb 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -19,7 +19,7 @@ def flight_interface_worker( period: float, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, - communications_ouutput_queue: queue_proxy_wrapper.QueueProxyWrapper, + communications_output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -66,7 +66,7 @@ def flight_interface_worker( output_queue.queue.put(value) if not home_location_sent: - communications_ouutput_queue.put(home_location) + communications_output_queue.queue.put(home_location) # Check for decision commands if not input_queue.queue.empty(): From 9d3b588a579af86333eaa55fc935bed5c0486688 Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Fri, 1 Nov 2024 23:16:31 -0400 Subject: [PATCH 16/31] Modify home location to be part of create for flight interface and communication worker --- main_2024.py | 6 ++--- modules/communications/communications.py | 22 ++++++++++++++----- .../communications/communications_worker.py | 12 +++++----- modules/flight_interface/flight_interface.py | 18 ++++++++++----- .../flight_interface_worker.py | 8 +++---- .../test_flight_interface_worker.py | 4 ++++ 6 files changed, 44 insertions(+), 26 deletions(-) diff --git a/main_2024.py b/main_2024.py index d1c9a172..a4d1534b 100644 --- a/main_2024.py +++ b/main_2024.py @@ -142,7 +142,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, ) @@ -245,7 +245,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, @@ -424,7 +424,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_cluster_estimation_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() diff --git a/modules/communications/communications.py b/modules/communications/communications.py index 9396ca81..cea5c122 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,4 +1,5 @@ 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 @@ -6,24 +7,26 @@ 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: """ @@ -31,20 +34,27 @@ def __init__( """ 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 diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 2ba5e1bf..d7461563 100644 --- a/modules/communications/communications_worker.py +++ b/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,7 +35,10 @@ 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 @@ -44,14 +46,10 @@ def communications_worker( # 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 diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 55fc8007..17088f7a 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -45,9 +45,9 @@ def create( return False, None # Get Pylance to stop complaining - assert home_position is not None + assert home_location is not None - local_logger.info(str(home_position), True) + local_logger.info(f"Home location: {home_location}", True) return True, FlightInterface(cls.__create_key, controller, home_position, local_logger) @@ -67,13 +67,19 @@ def __init__( self.__home_position = home_position self.__logger = local_logger + 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]": """ - 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]": odometry, ) 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]": 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: """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 5a6b4aeb..609694bc 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/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(): 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) From 2501414262a19f09233373efc340f71f3c3ff8fd Mon Sep 17 00:00:00 2001 From: Maxwell Lou Date: Sat, 2 Nov 2024 00:07:15 -0400 Subject: [PATCH 17/31] combined all logging together --- modules/communications/communications.py | 53 +++++++++++++++++++----- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index cea5c122..ef0cb3a7 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -1,13 +1,20 @@ +""" +Logs data and forwards it. +""" + import time + +from .. import detection_in_world +from ..common.logger.modules import logger 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 +from ..common.mavlink.modules import drone_odometry_local +from ..common.mavlink.modules import local_global_conversion class Communications: - """ """ + """ + Currently logs data only. + """ __create_key = object() @@ -19,6 +26,10 @@ def create( ) -> "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_location, local_logger) @@ -38,19 +49,39 @@ def __init__( self.__logger = local_logger def run( - self, detections_in_world: list[DetectionInWorld] - ) -> tuple[bool, list[DetectionInWorld] | None]: + 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: - result, detection_in_world_global = detection_in_world_global_from_local( - self.__home_location, detection_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 = drone_odometry_local.DronePositionLocal.create( + north, + east, + down, + ) + if not result: + self.__logger.warning( + f"Could not convert DetectionInWorld to DronePositionLocal:\ndetection in world: {detection_in_world}" + ) + return False, None + + result, detection_in_world_global = ( + local_global_conversion.drone_position_global_from_local( + self.__home_location, drone_position_local + ) ) 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.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) From e4029e73118998e370d852ccd95cc66476e57f00 Mon Sep 17 00:00:00 2001 From: Evan Zhao <57083990+Evang264@users.noreply.github.com> Date: Thu, 14 Nov 2024 17:26:15 -0500 Subject: [PATCH 18/31] Add log_to_kml.py --- config.yaml | 5 -- main_2024.py | 72 +++++++++++++++------------- modules/communications/log_to_kml.py | 56 ++++++++++++++++++++++ 3 files changed, 94 insertions(+), 39 deletions(-) create mode 100644 modules/communications/log_to_kml.py 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 a4d1534b..05ea8430 100644 --- a/main_2024.py +++ b/main_2024.py @@ -20,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 @@ -113,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) @@ -150,15 +144,15 @@ def main() -> int: 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, ) @@ -284,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, ) @@ -295,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 = [] @@ -379,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: @@ -404,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 @@ -426,9 +430,9 @@ def main() -> int: 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/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 From 9ede6b49492463a4506bd2de2bbf3ecc453c847a Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 20 Nov 2024 15:58:10 -0500 Subject: [PATCH 19/31] Refactor for common changes --- modules/common | 2 +- modules/communications/log_to_kml.py | 2 +- modules/flight_interface/flight_interface.py | 10 +++++----- modules/flight_interface/flight_interface_worker.py | 4 ++-- tests/integration/test_flight_interface_worker.py | 8 ++++---- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/modules/common b/modules/common index a0aac8ce..a256a497 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit a0aac8ce29273a6a1ca397a2229770add760835e +Subproject commit a256a49778d1154e03683c3b5e2fe6cb215d00e7 diff --git a/modules/communications/log_to_kml.py b/modules/communications/log_to_kml.py index f62ad82f..fa851020 100644 --- a/modules/communications/log_to_kml.py +++ b/modules/communications/log_to_kml.py @@ -5,7 +5,7 @@ import pathlib import re -from modules.common.modules.kml.locations_to_kml import locations_to_kml +from modules.common.modules.kml.kml_conversion import locations_to_kml from modules.common.modules.location_global import LocationGlobal diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 17088f7a..ef976278 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -45,9 +45,9 @@ def create( return False, None # Get Pylance to stop complaining - assert home_location is not None + assert home_position is not None - local_logger.info(f"Home location: {home_location}", True) + local_logger.info(f"Home position: {home_position}", True) return True, FlightInterface(cls.__create_key, controller, home_position, local_logger) @@ -67,11 +67,11 @@ def __init__( self.__home_position = home_position self.__logger = local_logger - def get_home_location(self) -> drone_odometry.DronePosition: + def get_home_position(self) -> position_global.PositionGlobal: """ - Accessor for home location. + Accessor for home position. """ - return self.__home_location + return self.__home_position def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 609694bc..41610a73 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -54,8 +54,8 @@ def flight_interface_worker( # Get Pylance to stop complaining assert interface is not None - home_location = interface.get_home_location() - communications_output_queue.queue.put(home_location) + 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 aaebbc00..af52cc9d 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -103,7 +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) + home_position_out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) in_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) worker = mp.Process( @@ -115,7 +115,7 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, in_queue, # Added input_queue out_queue, - home_location_out_queue, + home_position_out_queue, controller, ), ) @@ -126,8 +126,8 @@ def main() -> int: time.sleep(3) # Test - home_location = home_location_out_queue.queue.get() - assert home_location is not None + home_position = home_position_out_queue.queue.get() + assert home_position is not None # Run the apply_decision tests test_result = apply_decision_test(in_queue, out_queue) From 2be3b1d451c3d904ee1b195b9cab76c35f8208d7 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 16:52:03 -0500 Subject: [PATCH 20/31] Fixed communications module imports and implemented object position local-global conversion. --- modules/communications/communications.py | 48 +++++++++---------- .../communications/communications_worker.py | 12 ++--- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index ef0cb3a7..cdabc8d6 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -4,11 +4,11 @@ import time -from .. import detection_in_world -from ..common.logger.modules import logger -from ..common.mavlink.modules import drone_odometry -from ..common.mavlink.modules import drone_odometry_local -from ..common.mavlink.modules import local_global_conversion +from .. import object_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: @@ -21,23 +21,23 @@ class Communications: @classmethod def create( cls, - home_location: drone_odometry.DronePosition, + 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. + home_position: Take-off position of drone. Returns: Success, class object. """ - return True, Communications(cls.__create_key, home_location, local_logger) + return True, Communications(cls.__create_key, home_position, local_logger) def __init__( self, class_private_create_key: object, - home_location: drone_odometry.DronePosition, + home_position: position_global.PositionGlobal, local_logger: logger.Logger, ) -> None: """ @@ -45,47 +45,47 @@ def __init__( """ assert class_private_create_key is Communications.__create_key, "Use create() method" - self.__home_location = home_location + self.__home_position = 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]: + self, objects_in_world: list[object_in_world.ObjectInWorld], + ) -> tuple[bool, list[object_in_world.ObjectInWorld] | None]: - detections_in_world_global = [] - for detection_in_world in detections_in_world: + objects_in_world_global = [] + for object_in_world in objects_in_world: # TODO: Change this when the conversion interface is changed - north = detection_in_world.centre[0] - east = detection_in_world.centre[1] + north = object_in_world.location_x + east = object_in_world.location_y down = 0 - result, drone_position_local = drone_odometry_local.DronePositionLocal.create( + result, object_position_local = position_local.PositionLocal.create( north, east, down, ) if not result: self.__logger.warning( - f"Could not convert DetectionInWorld to DronePositionLocal:\ndetection in world: {detection_in_world}" + f"Could not convert ObjectInWorld to PositionLocal:\object in world: {object_in_world}" ) return False, None - result, detection_in_world_global = ( + result, object_in_world_global = ( local_global_conversion.drone_position_global_from_local( - self.__home_location, drone_position_local + self.__home_position, object_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}" + f"drone_position_global_from_local conversion failed:\nhome_position: {self.__home_position}\ndrone_position_local: {object_position_local}" ) return False, None - detections_in_world_global.append(detection_in_world_global) + objects_in_world_global.append(object_in_world_global) timestamp = time.time() - self.__logger.info(f"{timestamp}: {detections_in_world_global}") + self.__logger.info(f"{timestamp}: {objects_in_world_global}") - return True, detections_in_world + return True, objects_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index d7461563..54aea36f 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -8,11 +8,11 @@ from . import communications from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller -from ..common.logger.modules import logger +from ..common.modules.logger import logger def communications_worker( - home_location_queue: queue_proxy_wrapper.QueueProxyWrapper, + home_position_queue: queue_proxy_wrapper.QueueProxyWrapper, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, @@ -20,7 +20,7 @@ def communications_worker( """ Worker process. - home_location: get home_location for init + home_position: get home_position for init """ worker_name = pathlib.Path(__file__).stem @@ -35,10 +35,10 @@ def communications_worker( local_logger.info("Logger initialized", True) - # Get home location - home_location = home_location_queue.queue.get() + # Get home position + home_position = home_position_queue.queue.get() - result, comm = communications.Communications.create(home_location, local_logger) + result, comm = communications.Communications.create(home_position, local_logger) if not result: local_logger.error("Worker failed to create class object", True) return From dea241350982e0911ec17275a9e949114d5245bc Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 16:52:40 -0500 Subject: [PATCH 21/31] Added communications logger for logging cluster estimation --- config.yaml | 5 ++++ main_2024.py | 76 ++++++++++++++++++++++++++++++++-------------------- 2 files changed, 52 insertions(+), 29 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 05ea8430..da8423ce 100644 --- a/main_2024.py +++ b/main_2024.py @@ -20,6 +20,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 @@ -112,6 +113,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) @@ -144,19 +150,22 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( + cluster_estimation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) + communications_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) - result, camera_intrinsics = camera_properties.CameraIntrinsics.create( GEOLOCATION_RESOLUTION_X, GEOLOCATION_RESOLUTION_Y, @@ -278,7 +287,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_communications_queue], + output_queues=[geolocation_to_cluster_estimation_queue], controller=controller, local_logger=main_logger, ) @@ -289,13 +298,29 @@ 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_communications_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 + result, communications_worker_properties = worker_manager.WorkerProperties.create( count=1, target=communications_worker.communications_worker, work_arguments=(), input_queues=[ flight_interface_to_communications_queue, - geolocation_to_communications_queue, + cluster_estimation_to_communications_queue, ], output_queues=[communications_to_main_queue], controller=controller, @@ -375,18 +400,18 @@ def main() -> int: worker_managers.append(geolocation_manager) - result, communications_manager = worker_manager.WorkerManager.create( - worker_properties=communications_worker_properties, + 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 Communications", True) + main_logger.error("Failed to create manager for Cluster Estimation", True) return -1 # Get Pylance to stop complaining - assert communications_manager is not None + assert cluster_estimation_manager is not None - worker_managers.append(communications_manager) + worker_managers.append(cluster_estimation_manager) # Run for manager in worker_managers: @@ -400,24 +425,16 @@ def main() -> int: return -1 try: - geolocation_data = communications_to_main_queue.queue.get_nowait() + cluster_estimations = cluster_estimation_to_communications_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 @@ -430,7 +447,8 @@ def main() -> int: 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_communications_queue.fill_and_drain_queue() + geolocation_to_cluster_estimation_queue.fill_and_drain_queue() + cluster_estimation_to_communications_queue.fill_and_drain_queue() communications_to_main_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() From 76ab64fff4c6b1ccd42fc15dae65d5037ba7d16f Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 16:58:16 -0500 Subject: [PATCH 22/31] Formatted python --- modules/communications/communications.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index cdabc8d6..a4300c35 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -49,7 +49,8 @@ def __init__( self.__logger = local_logger def run( - self, objects_in_world: list[object_in_world.ObjectInWorld], + self, + objects_in_world: list[object_in_world.ObjectInWorld], ) -> tuple[bool, list[object_in_world.ObjectInWorld] | None]: objects_in_world_global = [] From 194f0d8909b2d8f4cf0f557495ff164fd2a361f4 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:20:20 -0500 Subject: [PATCH 23/31] Added communcations worker manager --- main_2024.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/main_2024.py b/main_2024.py index da8423ce..9bff1a2d 100644 --- a/main_2024.py +++ b/main_2024.py @@ -327,7 +327,7 @@ def main() -> int: local_logger=main_logger, ) if not result: - main_logger.error("Failed to create arguments for Video Input", True) + main_logger.error("Failed to create arguments for Communications Worker", True) return -1 assert communications_worker_properties is not None @@ -412,6 +412,19 @@ def main() -> int: assert cluster_estimation_manager is not None worker_managers.append(cluster_estimation_manager) + + 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 Communications Worker", True) + return -1 + + # Get Pylance to stop complaining + assert communications_manager is not None + + worker_managers.append(communications_manager) # Run for manager in worker_managers: @@ -431,7 +444,7 @@ def main() -> int: if cluster_estimations is not None: for cluster in cluster_estimations: - main_logger.debug("Cluster in world: " + True) + 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)) From df2d4de24651e4cdbf3480f82f78b939b7a1a165 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:21:12 -0500 Subject: [PATCH 24/31] Cluster estimations takes from communication to main queue --- main_2024.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main_2024.py b/main_2024.py index 9bff1a2d..b91f8b91 100644 --- a/main_2024.py +++ b/main_2024.py @@ -438,7 +438,7 @@ def main() -> int: return -1 try: - cluster_estimations = cluster_estimation_to_communications_queue.queue.get_nowait() + cluster_estimations = communications_to_main_queue.queue.get_nowait() except queue.Empty: cluster_estimations = None From e214595a09a2cbd0babbd9fc47d5db123c600360 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:43:48 -0500 Subject: [PATCH 25/31] Fixed communications worker --- main_2024.py | 6 +++--- modules/communications/communications.py | 2 +- modules/communications/communications_worker.py | 1 + 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/main_2024.py b/main_2024.py index b91f8b91..5a949aa1 100644 --- a/main_2024.py +++ b/main_2024.py @@ -87,8 +87,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 @@ -418,7 +418,7 @@ def main() -> int: local_logger=main_logger, ) if not result: - main_logger.error("Failed to create manager for Communications Worker", True) + main_logger.error("Failed to create manager for Communications", True) return -1 # Get Pylance to stop complaining diff --git a/modules/communications/communications.py b/modules/communications/communications.py index a4300c35..b4bbdc63 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -72,7 +72,7 @@ def run( return False, None result, object_in_world_global = ( - local_global_conversion.drone_position_global_from_local( + local_global_conversion.position_global_from_position_local( self.__home_position, object_position_local ) ) diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 54aea36f..84b2a938 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -37,6 +37,7 @@ def communications_worker( # Get home position home_position = home_position_queue.queue.get() + local_logger.info(f"Home position received: {home_position}", True) result, comm = communications.Communications.create(home_position, local_logger) if not result: From 92c3299260f04b8b37de35d9138d46dd9094ec9b Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 21 Nov 2024 17:43:59 -0500 Subject: [PATCH 26/31] Reformatted kml conversion --- main_2024.py | 2 +- modules/communications/log_to_kml.py | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/main_2024.py b/main_2024.py index 5a949aa1..e2fa0bb8 100644 --- a/main_2024.py +++ b/main_2024.py @@ -412,7 +412,7 @@ def main() -> int: assert cluster_estimation_manager is not None worker_managers.append(cluster_estimation_manager) - + result, communications_manager = worker_manager.WorkerManager.create( worker_properties=communications_worker_properties, local_logger=main_logger, diff --git a/modules/communications/log_to_kml.py b/modules/communications/log_to_kml.py index fa851020..256d18b7 100644 --- a/modules/communications/log_to_kml.py +++ b/modules/communications/log_to_kml.py @@ -5,8 +5,8 @@ import pathlib import re -from modules.common.modules.kml.kml_conversion import locations_to_kml -from modules.common.modules.location_global import LocationGlobal +from modules.common.modules.kml import kml_conversion +from modules.common.modules import location_global def convert_log_to_kml( @@ -45,12 +45,14 @@ def convert_log_to_kml( longitudes = list(map(float, longitudes)) for i in range(len(latitudes)): - success, location = LocationGlobal.create(latitudes[i], longitudes[i]) + success, location = location_global.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) + return kml_conversion.locations_to_kml(locations, document_name_prefix, save_directory) except Exception as e: print(e.with_traceback()) return False, None From c7ca9c7caa5c5c5f477730ec9c728fc427d69f18 Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 22 Nov 2024 17:02:44 -0500 Subject: [PATCH 27/31] Fixed logging and types in communications modules --- modules/communications/communications.py | 17 +++++++---------- modules/communications/communications_worker.py | 6 ++++-- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index b4bbdc63..321f97ed 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -5,9 +5,9 @@ import time from .. import object_in_world -from ..common.modules.logger import logger from ..common.modules import position_global from ..common.modules import position_local +from ..common.modules.logger import logger from ..common.modules.mavlink import local_global_conversion @@ -23,7 +23,7 @@ def create( cls, home_position: position_global.PositionGlobal, local_logger: logger.Logger, - ) -> "tuple[bool, Communications | None]": + ) -> "tuple[True, Communications] | tuple[False, None]": """ Logs data and forwards it. @@ -51,14 +51,13 @@ def __init__( def run( self, objects_in_world: list[object_in_world.ObjectInWorld], - ) -> tuple[bool, list[object_in_world.ObjectInWorld] | None]: + ) -> tuple[True, list[object_in_world.ObjectInWorld]] | tuple[False, None]: objects_in_world_global = [] for object_in_world in objects_in_world: - # TODO: Change this when the conversion interface is changed north = object_in_world.location_x east = object_in_world.location_y - down = 0 + down = 0.0 result, object_position_local = position_local.PositionLocal.create( north, @@ -67,7 +66,7 @@ def run( ) if not result: self.__logger.warning( - f"Could not convert ObjectInWorld to PositionLocal:\object in world: {object_in_world}" + f"Could not convert ObjectInWorld to PositionLocal:\nobject in world: {object_in_world}" ) return False, None @@ -76,17 +75,15 @@ def run( self.__home_position, object_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_position: {self.__home_position}\ndrone_position_local: {object_position_local}" + f"drone_position_global_from_local conversion failed:\nhome_position: {self.__home_position}\nobject_position_local: {object_position_local}" ) return False, None objects_in_world_global.append(object_in_world_global) - timestamp = time.time() - self.__logger.info(f"{timestamp}: {objects_in_world_global}") + self.__logger.info(f"{time.time()}: {objects_in_world_global}") return True, objects_in_world diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 84b2a938..501c6416 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -20,7 +20,9 @@ def communications_worker( """ Worker process. - home_position: get home_position for init + home_position_queue contains home positions for creating communications object. + input_queue and output_queue are data queues. + controller is how the main process communicates to this worker process. """ worker_name = pathlib.Path(__file__).stem @@ -36,7 +38,7 @@ def communications_worker( local_logger.info("Logger initialized", True) # Get home position - home_position = home_position_queue.queue.get() + home_position = home_position_queue.queue.get() local_logger.info(f"Home position received: {home_position}", True) result, comm = communications.Communications.create(home_position, local_logger) From ce99100a8bae8d5a4587ddaebc89134a2dda3a3d Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 22 Nov 2024 17:09:16 -0500 Subject: [PATCH 28/31] Added timeout to communications worker --- config.yaml | 3 +++ main_2024.py | 4 +++- modules/communications/communications_worker.py | 9 ++++++++- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/config.yaml b/config.yaml index 44bdd6df..87ed1293 100644 --- a/config.yaml +++ b/config.yaml @@ -39,3 +39,6 @@ cluster_estimation: min_activation_threshold: 25 min_new_points_to_run: 5 random_state: 0 + +communications: + timeout: 10.0 # seconds diff --git a/main_2024.py b/main_2024.py index e2fa0bb8..ae41a107 100644 --- a/main_2024.py +++ b/main_2024.py @@ -117,6 +117,8 @@ def main() -> int: 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"] + + COMMUNICATIONS_TIMEOUT = config["communications"]["timeout"] # pylint: enable=invalid-name except KeyError as exception: @@ -317,7 +319,7 @@ def main() -> int: result, communications_worker_properties = worker_manager.WorkerProperties.create( count=1, target=communications_worker.communications_worker, - work_arguments=(), + work_arguments=(COMMUNICATIONS_TIMEOUT,), input_queues=[ flight_interface_to_communications_queue, cluster_estimation_to_communications_queue, diff --git a/modules/communications/communications_worker.py b/modules/communications/communications_worker.py index 501c6416..ede6f67b 100644 --- a/modules/communications/communications_worker.py +++ b/modules/communications/communications_worker.py @@ -4,6 +4,7 @@ import os import pathlib +import queue from . import communications from utilities.workers import queue_proxy_wrapper @@ -12,6 +13,7 @@ def communications_worker( + timeout: float, home_position_queue: queue_proxy_wrapper.QueueProxyWrapper, input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, @@ -38,7 +40,12 @@ def communications_worker( local_logger.info("Logger initialized", True) # Get home position - home_position = home_position_queue.queue.get() + try: + home_position = home_position_queue.queue.get(timeout=timeout) + except queue.Empty: + local_logger.error("Home position queue timed out on startup", True) + return + local_logger.info(f"Home position received: {home_position}", True) result, comm = communications.Communications.create(home_position, local_logger) From 55755ca5bd317e1c863eda5d59b611ddd1cb385e Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 22 Nov 2024 19:06:13 -0500 Subject: [PATCH 29/31] Reworded logs from communications --- modules/communications/communications.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index 321f97ed..c9de1a40 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -78,7 +78,7 @@ def run( 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_position: {self.__home_position}\nobject_position_local: {object_position_local}" + f"position_global_from_local conversion failed:\nhome_position: {self.__home_position}\nobject_position_local: {object_position_local}" ) return False, None From affadfbda485b12f55305717c1979d4fd4e0da0b Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 22 Nov 2024 19:06:51 -0500 Subject: [PATCH 30/31] Reworded logs from communications --- modules/communications/communications.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/communications/communications.py b/modules/communications/communications.py index c9de1a40..6700d16b 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -78,7 +78,7 @@ def run( if not result: # Log nothing if at least one of the conversions failed self.__logger.warning( - f"position_global_from_local conversion failed:\nhome_position: {self.__home_position}\nobject_position_local: {object_position_local}" + f"position_global_from_position_local conversion failed:\nhome_position: {self.__home_position}\nobject_position_local: {object_position_local}" ) return False, None From 9e8df82a1c16af62e8200701b7ee6f2c3db95f0f Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 27 Nov 2024 17:19:15 -0500 Subject: [PATCH 31/31] Updated cluster estimation logging --- config.yaml | 2 +- main_2024.py | 6 +-- modules/communications/communications.py | 1 + modules/communications/log_to_kml.py | 58 ------------------------ 4 files changed, 3 insertions(+), 64 deletions(-) delete mode 100644 modules/communications/log_to_kml.py diff --git a/config.yaml b/config.yaml index 87ed1293..c10d0c4e 100644 --- a/config.yaml +++ b/config.yaml @@ -41,4 +41,4 @@ cluster_estimation: random_state: 0 communications: - timeout: 10.0 # seconds + timeout: 30.0 # seconds diff --git a/main_2024.py b/main_2024.py index 24af6d1b..9c59908d 100644 --- a/main_2024.py +++ b/main_2024.py @@ -445,11 +445,7 @@ def main() -> int: 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)) + main_logger.debug(f"Clusters: {cluster_estimations}") if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break diff --git a/modules/communications/communications.py b/modules/communications/communications.py index 6700d16b..b1127475 100644 --- a/modules/communications/communications.py +++ b/modules/communications/communications.py @@ -55,6 +55,7 @@ def run( objects_in_world_global = [] for object_in_world in objects_in_world: + # We assume detected objects are on the ground north = object_in_world.location_x east = object_in_world.location_y down = 0.0 diff --git a/modules/communications/log_to_kml.py b/modules/communications/log_to_kml.py deleted file mode 100644 index 256d18b7..00000000 --- a/modules/communications/log_to_kml.py +++ /dev/null @@ -1,58 +0,0 @@ -""" -Convert log file to KML file. -""" - -import pathlib -import re - -from modules.common.modules.kml import kml_conversion -from modules.common.modules import location_global - - -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 = location_global.LocationGlobal.create( - latitudes[i], longitudes[i] - ) - if not success: - return False, None - locations.append(location) - - return kml_conversion.locations_to_kml(locations, document_name_prefix, save_directory) - except Exception as e: - print(e.with_traceback()) - return False, None