From 06e36693c81fbe782f0d88f723ad2dbb199c3942 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Mon, 8 Jul 2024 15:18:23 -0600 Subject: [PATCH 01/21] Created decision worker --- modules/decision/decision_worker.py | 82 +++++++++++++++++++ .../flight_interface_worker.py | 7 ++ 2 files changed, 89 insertions(+) create mode 100644 modules/decision/decision_worker.py diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py new file mode 100644 index 00000000..3e865e82 --- /dev/null +++ b/modules/decision/decision_worker.py @@ -0,0 +1,82 @@ +""" +Retrieves list of landing pads from cluster estimation and outputs decision to the flight controller + +""" + +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from . import decision +from . import landing_pad_tracking +from . import search_pattern +from .. flight_interface.flight_interface_worker import current_odometry, odometry_mutex + + +def decision_worker( + camera_fov_forwards: float, + camera_fov_sideways: float, + search_height: float, + search_overlap: float, + distance_squared_threshold: float, + small_adjustment: float, + tolerance: float, + cluster_input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController + ) -> None: + """ + Worker process. + + PARAMETERS + ---------- + - camera_fov_forwards and camera_fov_sideways are the measurements for the cameras field of view + - search_height and search overlap are the parameters for the search pattern + - distance_squared_threshold, small adjustment, and tolerance are the initial settings + - cluster_input_queue and output_queue are the data queues. + - conteroller is how the main process communicates to this worker. + """ + + landing_pads = landing_pad_tracking.LandingPadTracking(distance_squared_threshold) + decision_maker = decision.Decision(tolerance) + search = search_pattern.SearchPattern( + camera_fov_forwards, + camera_fov_sideways, + search_height, + search_overlap, + distance_squared_threshold=distance_squared_threshold, + small_adjustment=small_adjustment + ) + + + while not controller.is_exit_requested(): + controller.check_pause() + + #Accesses mutex to get latest odometry data + with odometry_mutex: + curr_state = current_odometry + + if curr_state is None: + break + + input_data = cluster_input_queue.queue.get() + + if input_data is None: + break + + found, best_landing_pads = landing_pads.run(input_data) + + #Runs decision only if there exists a landing pad + if not found: + result, value = search.continue_search(curr_state) + else: + result, value = decision_maker.run(curr_state,best_landing_pads) + + if not result: + break + + output_queue.queue.put(value) + + + + + + \ No newline at end of file diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index a4175dd7..b15fdd3c 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -6,12 +6,15 @@ import os import pathlib import time +import threading from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import flight_interface from ..logger import logger +current_odometry = None +odometry_mutex = threading.Lock() def flight_interface_worker( address: str, @@ -64,4 +67,8 @@ def flight_interface_worker( if not result: continue + with odometry_mutex: + global current_odometry + current_odometry = value + output_queue.queue.put(value) From e7ce8594f9f795bbf7c886a4087cc5c2cfdb6248 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Mon, 8 Jul 2024 15:23:40 -0600 Subject: [PATCH 02/21] Fixed Formatting --- modules/decision/decision_worker.py | 45 ++++++++----------- .../flight_interface_worker.py | 1 + 2 files changed, 20 insertions(+), 26 deletions(-) diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py index 3e865e82..fb6d30e8 100644 --- a/modules/decision/decision_worker.py +++ b/modules/decision/decision_worker.py @@ -8,27 +8,27 @@ from . import decision from . import landing_pad_tracking from . import search_pattern -from .. flight_interface.flight_interface_worker import current_odometry, odometry_mutex +from ..flight_interface.flight_interface_worker import current_odometry, odometry_mutex def decision_worker( - camera_fov_forwards: float, - camera_fov_sideways: float, - search_height: float, - search_overlap: float, - distance_squared_threshold: float, - small_adjustment: float, - tolerance: float, - cluster_input_queue: queue_proxy_wrapper.QueueProxyWrapper, - output_queue: queue_proxy_wrapper.QueueProxyWrapper, - controller: worker_controller.WorkerController - ) -> None: + camera_fov_forwards: float, + camera_fov_sideways: float, + search_height: float, + search_overlap: float, + distance_squared_threshold: float, + small_adjustment: float, + tolerance: float, + cluster_input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, +) -> None: """ Worker process. PARAMETERS ---------- - - camera_fov_forwards and camera_fov_sideways are the measurements for the cameras field of view + - camera_fov_forwards and camera_fov_sideways are the measurements for the cameras field of view - search_height and search overlap are the parameters for the search pattern - distance_squared_threshold, small adjustment, and tolerance are the initial settings - cluster_input_queue and output_queue are the data queues. @@ -43,19 +43,18 @@ def decision_worker( search_height, search_overlap, distance_squared_threshold=distance_squared_threshold, - small_adjustment=small_adjustment + small_adjustment=small_adjustment, ) - while not controller.is_exit_requested(): controller.check_pause() - #Accesses mutex to get latest odometry data + # Accesses mutex to get latest odometry data with odometry_mutex: curr_state = current_odometry if curr_state is None: - break + break input_data = cluster_input_queue.queue.get() @@ -64,19 +63,13 @@ def decision_worker( found, best_landing_pads = landing_pads.run(input_data) - #Runs decision only if there exists a landing pad + # Runs decision only if there exists a landing pad if not found: result, value = search.continue_search(curr_state) else: - result, value = decision_maker.run(curr_state,best_landing_pads) + result, value = decision_maker.run(curr_state, best_landing_pads) if not result: - break + break output_queue.queue.put(value) - - - - - - \ No newline at end of file diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index b15fdd3c..0f75b221 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -16,6 +16,7 @@ current_odometry = None odometry_mutex = threading.Lock() + def flight_interface_worker( address: str, timeout: float, From 7b0cc1507fb929939f228b1f559a87ecddaf7b52 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Fri, 12 Jul 2024 12:38:35 -0600 Subject: [PATCH 03/21] Made requested changes --- modules/decision/decision_worker.py | 32 ++++++++----------- .../flight_interface_worker.py | 16 ++++++---- 2 files changed, 23 insertions(+), 25 deletions(-) diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py index fb6d30e8..dd33b2b7 100644 --- a/modules/decision/decision_worker.py +++ b/modules/decision/decision_worker.py @@ -1,6 +1,5 @@ """ -Retrieves list of landing pads from cluster estimation and outputs decision to the flight controller - +Retrieves list of landing pads from cluster estimation and outputs decision to the flight controller. """ from utilities.workers import queue_proxy_wrapper @@ -8,17 +7,17 @@ from . import decision from . import landing_pad_tracking from . import search_pattern -from ..flight_interface.flight_interface_worker import current_odometry, odometry_mutex def decision_worker( + distance_squared_threshold: float, + tolerance: float, camera_fov_forwards: float, camera_fov_sideways: float, search_height: float, search_overlap: float, - distance_squared_threshold: float, small_adjustment: float, - tolerance: float, + odometry_input_queue: queue_proxy_wrapper.QueueProxyWrapper, cluster_input_queue: queue_proxy_wrapper.QueueProxyWrapper, output_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, @@ -28,11 +27,10 @@ def decision_worker( PARAMETERS ---------- - - camera_fov_forwards and camera_fov_sideways are the measurements for the cameras field of view - - search_height and search overlap are the parameters for the search pattern - - distance_squared_threshold, small adjustment, and tolerance are the initial settings + - camera_fov_forwards, camera_fov_sideways, search_height, search_overlap, distance_squared_threshold, + and small_adjustment are arguments for the constructors below. - cluster_input_queue and output_queue are the data queues. - - conteroller is how the main process communicates to this worker. + - controller is how the main process communicates to this worker. """ landing_pads = landing_pad_tracking.LandingPadTracking(distance_squared_threshold) @@ -48,28 +46,26 @@ def decision_worker( while not controller.is_exit_requested(): controller.check_pause() - - # Accesses mutex to get latest odometry data - with odometry_mutex: - curr_state = current_odometry + + curr_state = odometry_input_queue.queue.get_nowait() if curr_state is None: - break + continue input_data = cluster_input_queue.queue.get() if input_data is None: - break + continue - found, best_landing_pads = landing_pads.run(input_data) + is_found, best_landing_pads = landing_pads.run(input_data) # Runs decision only if there exists a landing pad - if not found: + if not is_found: result, value = search.continue_search(curr_state) else: result, value = decision_maker.run(curr_state, best_landing_pads) if not result: - break + continue output_queue.queue.put(value) diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 0f75b221..55ab2fe2 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -6,15 +6,13 @@ import os import pathlib import time -import threading from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import flight_interface from ..logger import logger -current_odometry = None -odometry_mutex = threading.Lock() + def flight_interface_worker( @@ -55,6 +53,8 @@ def flight_interface_worker( frame = inspect.currentframe() local_logger.error("Worker failed to create class object", frame) return + + odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(maxsize=1) # Get Pylance to stop complaining assert interface is not None @@ -68,8 +68,10 @@ def flight_interface_worker( if not result: continue - with odometry_mutex: - global current_odometry - current_odometry = value - + try: + odometry_queue.queue.get_nowait() + except queue_proxy_wrapper.queue.Empty: + pass + + odometry_queue.queue.put(value) output_queue.queue.put(value) From 30a843468050a5a757f398ce2600836fd5db82ca Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Fri, 12 Jul 2024 12:48:11 -0600 Subject: [PATCH 04/21] Changed get_nowait() to get() to avoid exception --- modules/decision/decision_worker.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py index dd33b2b7..616636ec 100644 --- a/modules/decision/decision_worker.py +++ b/modules/decision/decision_worker.py @@ -47,7 +47,7 @@ def decision_worker( while not controller.is_exit_requested(): controller.check_pause() - curr_state = odometry_input_queue.queue.get_nowait() + curr_state = odometry_input_queue.queue.get() if curr_state is None: continue From 2e590a788ca68312c943d5a1d5ef22e44a21e657 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Mon, 15 Jul 2024 11:55:16 -0600 Subject: [PATCH 05/21] Added an integration test for the decision worker, addressed other comments. --- .../flight_interface_worker.py | 2 + tests/integration/test_decision_worker.py | 147 ++++++++++++++++++ 2 files changed, 149 insertions(+) create mode 100644 tests/integration/test_decision_worker.py diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 55ab2fe2..e254ec36 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -54,6 +54,7 @@ def flight_interface_worker( local_logger.error("Worker failed to create class object", frame) return + #Initalize queue with a maximum size of 1 to only hold latest odometry data odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(maxsize=1) # Get Pylance to stop complaining @@ -68,6 +69,7 @@ def flight_interface_worker( if not result: continue + # Attempt to remove latest odometry data from queue try: odometry_queue.queue.get_nowait() except queue_proxy_wrapper.queue.Empty: diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py new file mode 100644 index 00000000..52b8562f --- /dev/null +++ b/tests/integration/test_decision_worker.py @@ -0,0 +1,147 @@ +""" +Test worker process. +""" + +import multiprocessing as mp +import time + +from modules.decision import decision_worker +from modules import drone_odometry_local +from modules.cluster_estimation import cluster_estimation +from modules import odometry_and_time +from modules.decision import decision +from utilities.workers import worker_controller +from utilities.workers import queue_proxy_wrapper + + +DISTANCE_SQUARED_THRESHOLD = 25 # squared meters + +TOLERANCE = 0.1 # meters + +CAMERA_FOV_FORWARDS = 90 # degrees +CAMERA_FOV_SIDEWAYS = 120 # degrees + +SEARCH_HEIGHT = 10 # meters +SEARCH_OVERLAP = 0.2 + +SMALL_ADJUSTMENT = 0.5 # meters + + +def simulate_cluster_estimation_worker( + min_activation_threshold: int, + min_new_points_to_run: int, + random_state: int, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, +) -> None: + """ + Places list of detected landing pads into the queue. + """ + + result, estimator = cluster_estimation.ClusterEstimation.create( + min_activation_threshold, min_new_points_to_run, random_state + ) + assert result + assert estimator is not None + + result, cluster_pads = estimator.run(input_queue, False) + assert result + assert cluster_pads is not None + + input_queue.queue.put(cluster_pads) + + +def simulate_flight_interface_worker( + timestamp: float, odometry_queue: queue_proxy_wrapper.QueueProxyWrapper +) -> None: + """ + Place odometry data into queue of size 1. + """ + + result, drone_position = drone_odometry_local.DronePositionLocal.create(0.0, 2.0, -1.0) + assert result + assert drone_position is not None + + result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0) + assert result + assert drone_orientation is not None + + result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create( + drone_position, drone_orientation + ) + assert result + assert drone_odometry is not None + + result, drone_odometry_and_time = odometry_and_time.OdometryAndTime.create(drone_odometry) + assert result + assert drone_odometry_and_time is not None + + drone_odometry_and_time.timestamp = timestamp + + odometry_queue.queue.put(drone_odometry_and_time) + + +def main(): + """ + Main function. + """ + controller = worker_controller.WorkerController() + mp_manager = mp.Manager() + + cluster_input_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) + odometry_input_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, maxsize=1) + decision_output_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) + + worker = mp.Process( + target=decision_worker.decision_worker, + args=( + DISTANCE_SQUARED_THRESHOLD, + TOLERANCE, + CAMERA_FOV_FORWARDS, + CAMERA_FOV_SIDEWAYS, + SEARCH_HEIGHT, + SEARCH_OVERLAP, + SMALL_ADJUSTMENT, + odometry_input_queue, + cluster_input_queue, + decision_output_queue, + controller, + ), + ) + + # Starts the decision worker + worker.start() + + # Simulate odometry data + for i in range(1, 5): + simulate_flight_interface_worker(i, odometry_input_queue) + + # Simulate cluster estimation + simulate_cluster_estimation_worker(1, 1, 1, cluster_input_queue) + + time.sleep(1) + + controller.request_exit() + + # Test + try: + while True: + decision_output: decision.Decision = decision_output_queue.queue.get_nowait() + print(f"Decision output: {decision_output}") + assert decision_output is not None + except queue_proxy_wrapper.queue.Empty: + pass + + # Teardown + odometry_input_queue.fill_and_drain_queue() + cluster_input_queue.fill_and_drain_queue() + worker.join() + + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + + print("Done!") From 0dcfb37649a5e109f3d58186aa1d7f9d7b4f5af2 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Fri, 26 Jul 2024 12:15:17 -0600 Subject: [PATCH 06/21] Made Requested Changes --- .../flight_interface/flight_interface_worker.py | 4 ++-- tests/integration/test_decision_worker.py | 16 +++++++++++----- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index e254ec36..81a9cec9 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -54,7 +54,7 @@ def flight_interface_worker( local_logger.error("Worker failed to create class object", frame) return - #Initalize queue with a maximum size of 1 to only hold latest odometry data + # Initalize queue with a maximum size of 1 to only hold latest odometry data odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(maxsize=1) # Get Pylance to stop complaining @@ -69,7 +69,7 @@ def flight_interface_worker( if not result: continue - # Attempt to remove latest odometry data from queue + # Replace any existing odometry data with the latest odometry data try: odometry_queue.queue.get_nowait() except queue_proxy_wrapper.queue.Empty: diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 52b8562f..9e83eb07 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -5,16 +5,16 @@ import multiprocessing as mp import time +from modules.cluster_estimation import cluster_estimation from modules.decision import decision_worker +from modules.decision import decision from modules import drone_odometry_local -from modules.cluster_estimation import cluster_estimation from modules import odometry_and_time -from modules.decision import decision from utilities.workers import worker_controller from utilities.workers import queue_proxy_wrapper -DISTANCE_SQUARED_THRESHOLD = 25 # squared meters +DISTANCE_SQUARED_THRESHOLD = 25 # unit of area TOLERANCE = 0.1 # meters @@ -26,6 +26,7 @@ SMALL_ADJUSTMENT = 0.5 # meters +DECISION_COUNT = 2 def simulate_cluster_estimation_worker( min_activation_threshold: int, @@ -112,7 +113,7 @@ def main(): worker.start() # Simulate odometry data - for i in range(1, 5): + for i in range(0, 5): simulate_flight_interface_worker(i, odometry_input_queue) # Simulate cluster estimation @@ -120,11 +121,16 @@ def main(): time.sleep(1) + for i in range(0, 5): + simulate_flight_interface_worker(i, odometry_input_queue) + + simulate_cluster_estimation_worker(2, 2, 2, cluster_input_queue) + controller.request_exit() # Test try: - while True: + for i in range(0, DECISION_COUNT): decision_output: decision.Decision = decision_output_queue.queue.get_nowait() print(f"Decision output: {decision_output}") assert decision_output is not None From 6cc1a9385e4abf7eb3ef1a293c48f40888f244aa Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Fri, 26 Jul 2024 15:34:09 -0600 Subject: [PATCH 07/21] Updated. --- .../flight_interface_worker.py | 2 -- tests/integration/test_decision_worker.py | 32 ++++++++----------- 2 files changed, 14 insertions(+), 20 deletions(-) diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 81a9cec9..a6e275d7 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -13,8 +13,6 @@ from ..logger import logger - - def flight_interface_worker( address: str, timeout: float, diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 9e83eb07..2f922709 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -6,15 +6,15 @@ import time from modules.cluster_estimation import cluster_estimation -from modules.decision import decision_worker from modules.decision import decision -from modules import drone_odometry_local from modules import odometry_and_time +from modules import drone_odometry_local +from modules.decision import decision_worker from utilities.workers import worker_controller from utilities.workers import queue_proxy_wrapper -DISTANCE_SQUARED_THRESHOLD = 25 # unit of area +DISTANCE_SQUARED_THRESHOLD = 25 # Square root of this is meters TOLERANCE = 0.1 # meters @@ -26,7 +26,8 @@ SMALL_ADJUSTMENT = 0.5 # meters -DECISION_COUNT = 2 + +DECISION_COUNT = 10 def simulate_cluster_estimation_worker( min_activation_threshold: int, @@ -112,30 +113,25 @@ def main(): # Starts the decision worker worker.start() - # Simulate odometry data + # Simulate odometry data and cluster estimation for i in range(0, 5): simulate_flight_interface_worker(i, odometry_input_queue) - - # Simulate cluster estimation - simulate_cluster_estimation_worker(1, 1, 1, cluster_input_queue) + simulate_cluster_estimation_worker(1, 1, 1, cluster_input_queue) time.sleep(1) for i in range(0, 5): - simulate_flight_interface_worker(i, odometry_input_queue) - - simulate_cluster_estimation_worker(2, 2, 2, cluster_input_queue) + simulate_flight_interface_worker(i, odometry_input_queue) + simulate_cluster_estimation_worker(2, 2, 2, cluster_input_queue) controller.request_exit() # Test - try: - for i in range(0, DECISION_COUNT): - decision_output: decision.Decision = decision_output_queue.queue.get_nowait() - print(f"Decision output: {decision_output}") - assert decision_output is not None - except queue_proxy_wrapper.queue.Empty: - pass + for i in range(0, DECISION_COUNT): + decision_output: decision.Decision = decision_output_queue.queue.get_nowait() + print(f"Decision output: {decision_output}") + assert decision_output is not None + # Teardown odometry_input_queue.fill_and_drain_queue() From c42fef019d4925d31e9716424c369f2914a09f23 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Sat, 27 Jul 2024 15:04:50 -0600 Subject: [PATCH 08/21] Updated. --- modules/decision/decision_worker.py | 4 ++-- tests/integration/test_decision_worker.py | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py index 616636ec..b323977f 100644 --- a/modules/decision/decision_worker.py +++ b/modules/decision/decision_worker.py @@ -2,11 +2,11 @@ Retrieves list of landing pads from cluster estimation and outputs decision to the flight controller. """ -from utilities.workers import queue_proxy_wrapper -from utilities.workers import worker_controller from . import decision from . import landing_pad_tracking from . import search_pattern +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller def decision_worker( diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 2f922709..01583208 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -7,9 +7,9 @@ from modules.cluster_estimation import cluster_estimation from modules.decision import decision -from modules import odometry_and_time from modules import drone_odometry_local from modules.decision import decision_worker +from modules import odometry_and_time from utilities.workers import worker_controller from utilities.workers import queue_proxy_wrapper @@ -26,9 +26,11 @@ SMALL_ADJUSTMENT = 0.5 # meters +WORK_COUNT = 5 DECISION_COUNT = 10 + def simulate_cluster_estimation_worker( min_activation_threshold: int, min_new_points_to_run: int, @@ -114,13 +116,13 @@ def main(): worker.start() # Simulate odometry data and cluster estimation - for i in range(0, 5): + for i in range(0, WORK_COUNT): simulate_flight_interface_worker(i, odometry_input_queue) simulate_cluster_estimation_worker(1, 1, 1, cluster_input_queue) time.sleep(1) - for i in range(0, 5): + for i in range(0, WORK_COUNT): simulate_flight_interface_worker(i, odometry_input_queue) simulate_cluster_estimation_worker(2, 2, 2, cluster_input_queue) From e67de0c0c83af8bd10283f286e256ddcc509a529 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Sun, 28 Jul 2024 19:12:08 -0600 Subject: [PATCH 09/21] Updated, fixed imports. --- tests/integration/test_decision_worker.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 01583208..fd367370 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -5,16 +5,16 @@ import multiprocessing as mp import time +from modules import drone_odometry_local +from modules import odometry_and_time from modules.cluster_estimation import cluster_estimation from modules.decision import decision -from modules import drone_odometry_local from modules.decision import decision_worker -from modules import odometry_and_time from utilities.workers import worker_controller from utilities.workers import queue_proxy_wrapper -DISTANCE_SQUARED_THRESHOLD = 25 # Square root of this is meters +DISTANCE_SQUARED_THRESHOLD = 25 # Distance is in meters TOLERANCE = 0.1 # meters @@ -28,8 +28,6 @@ WORK_COUNT = 5 -DECISION_COUNT = 10 - def simulate_cluster_estimation_worker( min_activation_threshold: int, @@ -129,7 +127,7 @@ def main(): controller.request_exit() # Test - for i in range(0, DECISION_COUNT): + for i in range(0, WORK_COUNT * 2): decision_output: decision.Decision = decision_output_queue.queue.get_nowait() print(f"Decision output: {decision_output}") assert decision_output is not None From f3be8e00ff109e2a50d30484660d579404fb4849 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Sun, 28 Jul 2024 19:18:52 -0600 Subject: [PATCH 10/21] fixed formatting --- modules/decision/decision_worker.py | 4 ++-- modules/flight_interface/flight_interface_worker.py | 6 +++--- tests/integration/test_decision_worker.py | 5 ++--- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py index b323977f..58b8878f 100644 --- a/modules/decision/decision_worker.py +++ b/modules/decision/decision_worker.py @@ -27,7 +27,7 @@ def decision_worker( PARAMETERS ---------- - - camera_fov_forwards, camera_fov_sideways, search_height, search_overlap, distance_squared_threshold, + - camera_fov_forwards, camera_fov_sideways, search_height, search_overlap, distance_squared_threshold, and small_adjustment are arguments for the constructors below. - cluster_input_queue and output_queue are the data queues. - controller is how the main process communicates to this worker. @@ -46,7 +46,7 @@ def decision_worker( while not controller.is_exit_requested(): controller.check_pause() - + curr_state = odometry_input_queue.queue.get() if curr_state is None: diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index a6e275d7..67693634 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -51,7 +51,7 @@ def flight_interface_worker( frame = inspect.currentframe() local_logger.error("Worker failed to create class object", frame) return - + # Initalize queue with a maximum size of 1 to only hold latest odometry data odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(maxsize=1) @@ -71,7 +71,7 @@ def flight_interface_worker( try: odometry_queue.queue.get_nowait() except queue_proxy_wrapper.queue.Empty: - pass - + pass + odometry_queue.queue.put(value) output_queue.queue.put(value) diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index fd367370..770b6d2a 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -113,7 +113,7 @@ def main(): # Starts the decision worker worker.start() - # Simulate odometry data and cluster estimation + # Simulate odometry data and cluster estimation for i in range(0, WORK_COUNT): simulate_flight_interface_worker(i, odometry_input_queue) simulate_cluster_estimation_worker(1, 1, 1, cluster_input_queue) @@ -121,7 +121,7 @@ def main(): time.sleep(1) for i in range(0, WORK_COUNT): - simulate_flight_interface_worker(i, odometry_input_queue) + simulate_flight_interface_worker(i, odometry_input_queue) simulate_cluster_estimation_worker(2, 2, 2, cluster_input_queue) controller.request_exit() @@ -132,7 +132,6 @@ def main(): print(f"Decision output: {decision_output}") assert decision_output is not None - # Teardown odometry_input_queue.fill_and_drain_queue() cluster_input_queue.fill_and_drain_queue() From 5a0d5a3e30648fc7f53fa93ba4ed01cb268d5bae Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Sun, 28 Jul 2024 19:26:49 -0600 Subject: [PATCH 11/21] fixed formatting --- tests/integration/test_decision_worker.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 770b6d2a..eb5e306f 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -82,7 +82,7 @@ def simulate_flight_interface_worker( odometry_queue.queue.put(drone_odometry_and_time) -def main(): +def main() -> int: """ Main function. """ From 49ca9c0cf7349c0d8d68657f0e8bf09ca8be7e69 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Mon, 29 Jul 2024 10:24:12 -0600 Subject: [PATCH 12/21] Updated constructor calls, fixed formatting --- modules/decision/decision_worker.py | 6 ++++-- modules/flight_interface/flight_interface_worker.py | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py index 58b8878f..34c950d5 100644 --- a/modules/decision/decision_worker.py +++ b/modules/decision/decision_worker.py @@ -2,11 +2,11 @@ Retrieves list of landing pads from cluster estimation and outputs decision to the flight controller. """ +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller from . import decision from . import landing_pad_tracking from . import search_pattern -from utilities.workers import queue_proxy_wrapper -from utilities.workers import worker_controller def decision_worker( @@ -40,6 +40,8 @@ def decision_worker( camera_fov_sideways, search_height, search_overlap, + current_position_x=0.0, + current_position_y=0.0, distance_squared_threshold=distance_squared_threshold, small_adjustment=small_adjustment, ) diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 67693634..f6660106 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -6,6 +6,7 @@ import os import pathlib import time +import multiprocessing as mp from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller @@ -53,7 +54,7 @@ def flight_interface_worker( return # Initalize queue with a maximum size of 1 to only hold latest odometry data - odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(maxsize=1) + odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(mp.Manager(), maxsize=1) # Get Pylance to stop complaining assert interface is not None From 0ec84aed7d0087a2304ad6cc2349bf4019abc50e Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Mon, 29 Jul 2024 10:29:52 -0600 Subject: [PATCH 13/21] fixed formatting --- modules/flight_interface/flight_interface_worker.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index f6660106..433c0996 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -6,7 +6,7 @@ import os import pathlib import time -import multiprocessing as mp +import multiprocessing as mp from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller From e3478952a398d69dc13f3d20bf1d3b1a95b66f8d Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Mon, 29 Jul 2024 11:04:11 -0600 Subject: [PATCH 14/21] Added TODO --- modules/decision/decision_worker.py | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/decision/decision_worker.py b/modules/decision/decision_worker.py index 34c950d5..6421831f 100644 --- a/modules/decision/decision_worker.py +++ b/modules/decision/decision_worker.py @@ -32,6 +32,7 @@ def decision_worker( - cluster_input_queue and output_queue are the data queues. - controller is how the main process communicates to this worker. """ + # TODO: Need to rework how we get odometry data landing_pads = landing_pad_tracking.LandingPadTracking(distance_squared_threshold) decision_maker = decision.Decision(tolerance) From 65472f959fc93d71612430d1a091bf825a5e6777 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Tue, 30 Jul 2024 11:13:57 -0600 Subject: [PATCH 15/21] Rewrote simulate_cluster_estimation_worker.py --- tests/integration/test_decision_worker.py | 26 +++++++++++------------ 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index eb5e306f..76aedf0f 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -6,6 +6,7 @@ import time from modules import drone_odometry_local +from modules import object_in_world from modules import odometry_and_time from modules.cluster_estimation import cluster_estimation from modules.decision import decision @@ -30,26 +31,23 @@ def simulate_cluster_estimation_worker( - min_activation_threshold: int, - min_new_points_to_run: int, - random_state: int, input_queue: queue_proxy_wrapper.QueueProxyWrapper, ) -> None: """ Places list of detected landing pads into the queue. """ - result, estimator = cluster_estimation.ClusterEstimation.create( - min_activation_threshold, min_new_points_to_run, random_state - ) - assert result - assert estimator is not None + fake_objects = [ + object_in_world.ObjectInWorld.create(1.0,2.0,0.9)[1], + object_in_world.ObjectInWorld.create(4.5,3.0,2.0)[1], + object_in_world.ObjectInWorld.create(7.2,2.9,4.6)[1], + ] + + for obj in fake_objects: + assert obj is not None - result, cluster_pads = estimator.run(input_queue, False) - assert result - assert cluster_pads is not None - input_queue.queue.put(cluster_pads) + input_queue.queue.put(fake_objects) def simulate_flight_interface_worker( @@ -116,13 +114,13 @@ def main() -> int: # Simulate odometry data and cluster estimation for i in range(0, WORK_COUNT): simulate_flight_interface_worker(i, odometry_input_queue) - simulate_cluster_estimation_worker(1, 1, 1, cluster_input_queue) + simulate_cluster_estimation_worker(cluster_input_queue) time.sleep(1) for i in range(0, WORK_COUNT): simulate_flight_interface_worker(i, odometry_input_queue) - simulate_cluster_estimation_worker(2, 2, 2, cluster_input_queue) + simulate_cluster_estimation_worker(cluster_input_queue) controller.request_exit() From bb75213ddd1e03a2d42c7432f2791c79ce06db6f Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Tue, 30 Jul 2024 11:14:36 -0600 Subject: [PATCH 16/21] reformatted --- tests/integration/test_decision_worker.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 76aedf0f..9307aec3 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -38,15 +38,14 @@ def simulate_cluster_estimation_worker( """ fake_objects = [ - object_in_world.ObjectInWorld.create(1.0,2.0,0.9)[1], - object_in_world.ObjectInWorld.create(4.5,3.0,2.0)[1], - object_in_world.ObjectInWorld.create(7.2,2.9,4.6)[1], + object_in_world.ObjectInWorld.create(1.0, 2.0, 0.9)[1], + object_in_world.ObjectInWorld.create(4.5, 3.0, 2.0)[1], + object_in_world.ObjectInWorld.create(7.2, 2.9, 4.6)[1], ] for obj in fake_objects: assert obj is not None - input_queue.queue.put(fake_objects) From a53006a596627da01c29c5d166f95eba265334df Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Tue, 30 Jul 2024 11:18:25 -0600 Subject: [PATCH 17/21] removed import --- tests/integration/test_decision_worker.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 9307aec3..e888e7c8 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -8,7 +8,6 @@ from modules import drone_odometry_local from modules import object_in_world from modules import odometry_and_time -from modules.cluster_estimation import cluster_estimation from modules.decision import decision from modules.decision import decision_worker from utilities.workers import worker_controller From 0c2dea58f8c5c2f86d7d2d2bd9d686503c91a372 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Wed, 31 Jul 2024 11:41:44 -0600 Subject: [PATCH 18/21] Rewrote the cluster_estimation_worker function and made changes to flight_interface_worker --- main_2024.py | 7 ++++++- .../flight_interface_worker.py | 8 ++++--- tests/integration/test_decision_worker.py | 21 ++++++++++++------- .../test_flight_interface_worker.py | 3 +++ 4 files changed, 27 insertions(+), 12 deletions(-) diff --git a/main_2024.py b/main_2024.py index d68adaa0..5892d2cc 100644 --- a/main_2024.py +++ b/main_2024.py @@ -122,6 +122,10 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + odometry_to_data_merge_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, @@ -214,7 +218,7 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, ), input_queues=[], - output_queues=[flight_interface_to_data_merge_queue], + output_queues=[flight_interface_to_data_merge_queue, odometry_to_data_merge_queue], controller=controller, local_logger=main_logger, ) @@ -382,6 +386,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() + odometry_to_data_merge_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() geolocation_to_main_queue.fill_and_drain_queue() diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 433c0996..b840e604 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -20,6 +20,7 @@ def flight_interface_worker( baud_rate: int, period: float, output_queue: queue_proxy_wrapper.QueueProxyWrapper, + odometry_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -30,6 +31,10 @@ def flight_interface_worker( output_queue is the data queue. controller is how the main process communicates to this worker process. """ + if len(odometry_queue) > 1: + print("ERROR: Queue should have a maximum size of 1") + return + # TODO: Error handling worker_name = pathlib.Path(__file__).stem @@ -53,9 +58,6 @@ def flight_interface_worker( local_logger.error("Worker failed to create class object", frame) return - # Initalize queue with a maximum size of 1 to only hold latest odometry data - odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(mp.Manager(), maxsize=1) - # Get Pylance to stop complaining assert interface is not None diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index e888e7c8..6f402bd0 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -36,16 +36,21 @@ def simulate_cluster_estimation_worker( Places list of detected landing pads into the queue. """ - fake_objects = [ - object_in_world.ObjectInWorld.create(1.0, 2.0, 0.9)[1], - object_in_world.ObjectInWorld.create(4.5, 3.0, 2.0)[1], - object_in_world.ObjectInWorld.create(7.2, 2.9, 4.6)[1], - ] + result, object1 = object_in_world.ObjectInWorld.create(1.0, 2.0, 0.9) + assert result + assert object1 is not None + + result, object2 = object_in_world.ObjectInWorld.create(4.5, 3.0, 2.0) + assert result + assert object2 is not None + + result, object3 = object_in_world.ObjectInWorld.create(7.2, 2.9, 4.6) + assert result + assert object3 is not None - for obj in fake_objects: - assert obj is not None + objects = [object1, object2, object3] - input_queue.queue.put(fake_objects) + input_queue.queue.put(objects) def simulate_flight_interface_worker( diff --git a/tests/integration/test_flight_interface_worker.py b/tests/integration/test_flight_interface_worker.py index 10d767fe..9039fa2e 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -16,6 +16,7 @@ FLIGHT_INTERFACE_TIMEOUT = 10.0 # seconds FLIGHT_INTERFACE_BAUD_RATE = 57600 # symbol rate FLIGHT_INTERFACE_WORKER_PERIOD = 0.1 # seconds +QUEUE_MAX_SIZE = 1 # Max items allowed in odometry queue def main() -> int: @@ -28,6 +29,7 @@ def main() -> int: mp_manager = mp.Manager() out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) + odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, QUEUE_MAX_SIZE) worker = mp.Process( target=flight_interface_worker.flight_interface_worker, @@ -37,6 +39,7 @@ def main() -> int: FLIGHT_INTERFACE_BAUD_RATE, FLIGHT_INTERFACE_WORKER_PERIOD, out_queue, + odometry_queue, controller, ), ) From 0560ff0fcd26f74b91b54bca05170da700181888 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Thu, 1 Aug 2024 11:19:17 -0600 Subject: [PATCH 19/21] Updated --- main_2024.py | 9 +++++---- .../flight_interface/flight_interface_worker.py | 14 +++++++------- tests/integration/test_flight_interface_worker.py | 8 +++++--- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/main_2024.py b/main_2024.py index 5892d2cc..6d9ff098 100644 --- a/main_2024.py +++ b/main_2024.py @@ -122,9 +122,10 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - odometry_to_data_merge_queue = queue_proxy_wrapper.QueueProxyWrapper( + # Queue size of latest odometry data must be 1 + flight_interface_to_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, - QUEUE_MAX_SIZE, + 1, ) data_merge_to_geolocation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, @@ -218,7 +219,7 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, ), input_queues=[], - output_queues=[flight_interface_to_data_merge_queue, odometry_to_data_merge_queue], + output_queues=[flight_interface_to_data_merge_queue, flight_interface_to_decision_queue], controller=controller, local_logger=main_logger, ) @@ -386,7 +387,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() - odometry_to_data_merge_queue.fill_and_drain_queue() + flight_interface_to_decision_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() geolocation_to_main_queue.fill_and_drain_queue() diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index b840e604..f766dc97 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -5,8 +5,8 @@ import inspect import os import pathlib +import queue import time -import multiprocessing as mp from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller @@ -20,7 +20,7 @@ def flight_interface_worker( baud_rate: int, period: float, output_queue: queue_proxy_wrapper.QueueProxyWrapper, - odometry_queue: queue_proxy_wrapper.QueueProxyWrapper, + most_recent_odometry_queue: queue_proxy_wrapper.QueueProxyWrapper, controller: worker_controller.WorkerController, ) -> None: """ @@ -31,8 +31,8 @@ def flight_interface_worker( output_queue is the data queue. controller is how the main process communicates to this worker process. """ - if len(odometry_queue) > 1: - print("ERROR: Queue should have a maximum size of 1") + if most_recent_odometry_queue.maxsize != 1: + print("ERROR: most_recent_odometry_queue must have a maximum size of 1") return # TODO: Error handling @@ -72,9 +72,9 @@ def flight_interface_worker( # Replace any existing odometry data with the latest odometry data try: - odometry_queue.queue.get_nowait() - except queue_proxy_wrapper.queue.Empty: + most_recent_odometry_queue.queue.get_nowait() + except queue.Empty: pass - odometry_queue.queue.put(value) + most_recent_odometry_queue.queue.put(value) output_queue.queue.put(value) diff --git a/tests/integration/test_flight_interface_worker.py b/tests/integration/test_flight_interface_worker.py index 9039fa2e..703bc150 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -16,7 +16,7 @@ FLIGHT_INTERFACE_TIMEOUT = 10.0 # seconds FLIGHT_INTERFACE_BAUD_RATE = 57600 # symbol rate FLIGHT_INTERFACE_WORKER_PERIOD = 0.1 # seconds -QUEUE_MAX_SIZE = 1 # Max items allowed in odometry queue +LATEST_ODOMETRY_QUEUE_MAX_SIZE = 1 # Max items allowed in latest odometry queue def main() -> int: @@ -29,7 +29,9 @@ def main() -> int: mp_manager = mp.Manager() out_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager) - odometry_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, QUEUE_MAX_SIZE) + latest_odometry_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, LATEST_ODOMETRY_QUEUE_MAX_SIZE + ) worker = mp.Process( target=flight_interface_worker.flight_interface_worker, @@ -39,7 +41,7 @@ def main() -> int: FLIGHT_INTERFACE_BAUD_RATE, FLIGHT_INTERFACE_WORKER_PERIOD, out_queue, - odometry_queue, + latest_odometry_queue, controller, ), ) From 4de9ee08c80d26dba13cc32c6b7ea7cfda34d7a4 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Wed, 14 Aug 2024 19:23:31 -0600 Subject: [PATCH 20/21] Readded submodules --- requirements-pytorch.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/requirements-pytorch.txt b/requirements-pytorch.txt index 1a8037b1..f9bacd7a 100644 --- a/requirements-pytorch.txt +++ b/requirements-pytorch.txt @@ -1,5 +1,5 @@ # For any non-Jetson computer (i.e. all developers) --extra-index-url https://download.pytorch.org/whl/cu117 -torch==1.13.1+cu117 -torchvision==0.14.1+cu117 +torch==2.0.1+cu117 +torchvision==0.15.2+cu117 ultralytics From 4a4b0f97aae6defbe9da467aba24a28258c9e178 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Mon, 19 Aug 2024 17:35:55 -0600 Subject: [PATCH 21/21] Addressed bugs in landing_pad_tracking --- modules/decision/landing_pad_tracking.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/decision/landing_pad_tracking.py b/modules/decision/landing_pad_tracking.py index c5b0e796..15090cbb 100644 --- a/modules/decision/landing_pad_tracking.py +++ b/modules/decision/landing_pad_tracking.py @@ -54,7 +54,7 @@ def mark_confirmed_positive(self, detection: object_in_world.ObjectInWorld) -> N def run( self, detections: "list[object_in_world.ObjectInWorld]" - ) -> "tuple[bool, object_in_world.ObjectInWorld | None]": + ) -> "tuple[bool, list[object_in_world.ObjectInWorld] | None]": """ Updates the list of unconfirmed positives and returns the a first confirmed positive if one exists, else the unconfirmed positive with the lowest variance. @@ -82,9 +82,9 @@ def run( # If new landing pad, add to list of unconfirmed positives self.__unconfirmed_positives.append(detection) - # If there are confirmed positives, return the first one + # If there are confirmed positives, return the entire list if len(self.__confirmed_positives) > 0: - return True, self.__confirmed_positives[0] + return True, self.__confirmed_positives # If the list is empty, all landing pads have been visited, none are viable if len(self.__unconfirmed_positives) == 0: @@ -93,5 +93,5 @@ def run( # Sort list by variance in ascending order self.__unconfirmed_positives.sort(key=lambda x: x.spherical_variance) - # Return detection with lowest variance - return True, self.__unconfirmed_positives[0] + # Return all detections + return True, self.__unconfirmed_positives