From 4df05c8637514e614e57cbffd462c8f80047e4db Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Wed, 9 Oct 2024 12:21:06 -0400 Subject: [PATCH 01/16] redo of integrating cluster estimation worker into main --- config.yaml | 5 +++++ main_2024.py | 62 ++++++++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 60 insertions(+), 7 deletions(-) diff --git a/config.yaml b/config.yaml index b0dd9811..62534d32 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_merge: + min_activation_threshold: 0 + min_new_points_to_run: 0 + random_state: 0 \ No newline at end of file diff --git a/main_2024.py b/main_2024.py index 66ad913d..fe4d3278 100644 --- a/main_2024.py +++ b/main_2024.py @@ -19,6 +19,7 @@ from modules.data_merge import data_merge_worker from modules.geolocation import geolocation_worker from modules.geolocation import camera_properties +from modules.cluster_estimation import cluster_estimation_worker from modules.common.logger.modules import logger from modules.common.logger.modules import logger_setup_main from modules.common.logger.read_yaml.modules import read_yaml @@ -85,8 +86,8 @@ def main() -> int: VIDEO_INPUT_SAVE_PREFIX = str(pathlib.Path(logging_path, VIDEO_INPUT_SAVE_NAME_PREFIX)) DETECT_TARGET_WORKER_COUNT = config["detect_target"]["worker_count"] - detect_target_option_int = config["detect_target"]["option"] - DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(detect_target_option_int) + DETECT_TARGET_OPTION_INT = config["detect_target"]["option"] + DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(DETECT_TARGET_OPTION_INT) DETECT_TARGET_DEVICE = "cpu" if args.cpu else config["detect_target"]["device"] DETECT_TARGET_MODEL_PATH = config["detect_target"]["model_path"] DETECT_TARGET_OVERRIDE_FULL_PRECISION = args.full @@ -111,6 +112,10 @@ 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_merge"]["min_activation_threshold"] + MIN_NEW_POINTS_TO_RUN = config["cluster_merge"]["min_new_points_to_run"] + RANDOM_STATE = config["cluster_merge"]["random_state"] # pylint: enable=invalid-name except KeyError as exception: main_logger.error(f"Config key(s) not found: {exception}", True) @@ -139,7 +144,7 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -147,6 +152,10 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + cluster_estimation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) result, camera_intrinsics = camera_properties.CameraIntrinsics.create( GEOLOCATION_RESOLUTION_X, @@ -266,7 +275,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_main_queue], + output_queues=[geolocation_to_cluster_estimation_queue], controller=controller, local_logger=main_logger, ) @@ -277,6 +286,22 @@ def main() -> int: # Get Pylance to stop complaining assert geolocation_worker_properties is not None + result, cluster_estimation_worker_properties = worker_manager.WorkerProperties.create( + count=1, + target=cluster_estimation_worker.cluster_estimation_worker, + work_arguments=(MIN_ACTIVATION_THRESHOLD, MIN_NEW_POINTS_TO_RUN, RANDOM_STATE), + input_queues=[geolocation_to_cluster_estimation_queue], + output_queues=[cluster_estimation_to_main_queue], + controller=controller, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create arguments for Video Input", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_worker_properties is not None + # Create managers worker_managers = [] @@ -345,6 +370,19 @@ def main() -> int: worker_managers.append(geolocation_manager) + result, cluster_estimation_manager = worker_manager.WorkerManager.create( + worker_properties=cluster_estimation_worker_properties, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create manager for Flight Interface", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_manager is not None + + worker_managers.append(cluster_estimation_manager) + # Run for manager in worker_managers: manager.start_workers() @@ -357,7 +395,7 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_main_queue.queue.get_nowait() + geolocation_data = geolocation_to_cluster_estimation_queue.queue.get_nowait() except queue.Empty: geolocation_data = None @@ -374,7 +412,16 @@ def main() -> int: main_logger.debug( "geolocation confidence: " + str(detection_world.confidence), True ) - + try: + cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() + except queue.Empty: + cluster_estimations = None + if cluster_estimations is not None: + for cluster in cluster_estimations: + main_logger.debug("Cluser in world: True") + main_logger.debug("Cluster location x: " + str(cluster.location_x)) + main_logger.debug("Cluster location y: " + str(cluster.location_y)) + main_logger.debug("Cluster spherical variance: " + str(cluster.spherical_variance)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break @@ -386,8 +433,9 @@ def main() -> int: detect_target_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_data_merge_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() - geolocation_to_main_queue.fill_and_drain_queue() + geolocation_to_cluster_estimation_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() + cluster_estimation_to_main_queue.fill_and_drain_queue() for manager in worker_managers: manager.join_workers() From 9ec46f24a9c33987a58db1a60ae4a775a1fd2fcc Mon Sep 17 00:00:00 2001 From: Cyuber Date: Fri, 11 Oct 2024 19:06:27 -0400 Subject: [PATCH 02/16] implemented changes from review --- config.yaml | 4 ++-- main_2024.py | 37 +++++++++---------------------------- modules/object_in_world.py | 6 ++++++ 3 files changed, 17 insertions(+), 30 deletions(-) diff --git a/config.yaml b/config.yaml index 62534d32..de8fe916 100644 --- a/config.yaml +++ b/config.yaml @@ -35,7 +35,7 @@ geolocation: camera_orientation_pitch: -1.57079632679 camera_orientation_roll: 0.0 -cluster_merge: +cluster_estimation: min_activation_threshold: 0 min_new_points_to_run: 0 - random_state: 0 \ No newline at end of file + random_state: 0 diff --git a/main_2024.py b/main_2024.py index fe4d3278..c18bd0c0 100644 --- a/main_2024.py +++ b/main_2024.py @@ -296,7 +296,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 Cluster Estimation", True) return -1 # Get Pylance to stop complaining @@ -375,7 +375,7 @@ def main() -> int: local_logger=main_logger, ) if not result: - main_logger.error("Failed to create manager for Flight Interface", True) + main_logger.error("Failed to create manager for Cluster Estimation", True) return -1 # Get Pylance to stop complaining @@ -395,33 +395,14 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_cluster_estimation_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 - ) - try: - cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() + cluster_estimation_data = cluster_estimation_to_main_queue.queue.get_nowait() except queue.Empty: - cluster_estimations = None - if cluster_estimations is not None: - for cluster in cluster_estimations: - main_logger.debug("Cluser in world: True") - main_logger.debug("Cluster location x: " + str(cluster.location_x)) - main_logger.debug("Cluster location y: " + str(cluster.location_y)) - main_logger.debug("Cluster spherical variance: " + str(cluster.spherical_variance)) + cluster_estimation_data = None + + if cluster_estimation_data is not None: + for object_in_world in cluster_estimation_data: + main_logger.debug("Cluster in world: ", True) + main_logger.debug(object_in_world.__str__) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break diff --git a/modules/object_in_world.py b/modules/object_in_world.py index 1ce9f65e..31f7ef01 100644 --- a/modules/object_in_world.py +++ b/modules/object_in_world.py @@ -38,3 +38,9 @@ def __init__( self.location_x = location_x self.location_y = location_y self.spherical_variance = spherical_variance + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}, location x: {self.location_x}, location y: {self.location_y}, spherical variance: {self.spherical_variance}, label: {self.label}" From f01f598255b68618c962ff616ff9081ab532367d Mon Sep 17 00:00:00 2001 From: Xierumeng <22666760+Xierumeng@users.noreply.github.com> Date: Sun, 13 Oct 2024 02:09:09 +0800 Subject: [PATCH 03/16] Fix landing pad tracking return (#207) --- modules/decision/landing_pad_tracking.py | 28 ++++++------ tests/unit/test_landing_pad_tracking.py | 55 ++++++++++++++++-------- 2 files changed, 52 insertions(+), 31 deletions(-) diff --git a/modules/decision/landing_pad_tracking.py b/modules/decision/landing_pad_tracking.py index c5b0e796..518bb06a 100644 --- a/modules/decision/landing_pad_tracking.py +++ b/modules/decision/landing_pad_tracking.py @@ -54,10 +54,14 @@ 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. + Updates the list of unconfirmed positives and returns the confirmed positives if + they exist, otherwise returns the unconfirmed positives. + + detections: New detections. + + Return: List of confirmed/unconfirmed positives. """ for detection in detections: match_found = False @@ -82,16 +86,16 @@ 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 them 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: - return False, None + # If there are unconfirmed positives, return them + if len(self.__unconfirmed_positives) > 0: + # Sort list by variance in ascending order + self.__unconfirmed_positives.sort(key=lambda x: x.spherical_variance) - # Sort list by variance in ascending order - self.__unconfirmed_positives.sort(key=lambda x: x.spherical_variance) + return True, self.__unconfirmed_positives - # Return detection with lowest variance - return True, self.__unconfirmed_positives[0] + # All landing pads have been visited, none are viable + return False, None diff --git a/tests/unit/test_landing_pad_tracking.py b/tests/unit/test_landing_pad_tracking.py index 51bfc3f7..fd6fce60 100644 --- a/tests/unit/test_landing_pad_tracking.py +++ b/tests/unit/test_landing_pad_tracking.py @@ -384,7 +384,10 @@ def test_run_with_empty_detections_list( """ Test run method with empty detections list. """ + # Run result, actual = tracker.run([]) + + # Check assert not result assert actual is None @@ -396,8 +399,8 @@ def test_run_one_input( """ Test run with only 1 input. """ - expected_output = detections_1[2] - expected_unconfirmed_positives = [ + # Setup + expected = [ detections_1[2], detections_1[1], detections_1[4], @@ -405,11 +408,12 @@ def test_run_one_input( detections_1[3], ] + # Run result, actual = tracker.run(detections_1) + # Check assert result - assert actual == expected_output - assert tracker._LandingPadTracking__unconfirmed_positives == expected_unconfirmed_positives # type: ignore + assert actual == expected def test_run_one_input_similar_detections( self, @@ -419,19 +423,20 @@ def test_run_one_input_similar_detections( """ Test run with only 1 input where 2 landing pads are similar. """ - expected_output = detections_3[2] - expected_unconfirmed_positives = [ + # Setup + expected = [ detections_3[2], detections_3[1], detections_3[4], detections_3[3], ] + # Run result, actual = tracker.run(detections_3) + # Check assert result - assert actual == expected_output - assert tracker._LandingPadTracking__unconfirmed_positives == expected_unconfirmed_positives # type: ignore + assert actual == expected def test_run_multiple_inputs( self, @@ -442,8 +447,8 @@ def test_run_multiple_inputs( """ Test run with 2 inputs where some landing pads are similar. """ - expected_output = detections_2[0] - expected_unconfirmed_positives = [ + # Setup + expected = [ detections_2[0], detections_1[2], detections_2[1], @@ -454,12 +459,15 @@ def test_run_multiple_inputs( detections_1[3], ] - tracker.run(detections_1) + # Run + result, _ = tracker.run(detections_1) + assert result + result, actual = tracker.run(detections_2) + # Check assert result - assert actual == expected_output - assert tracker._LandingPadTracking__unconfirmed_positives == expected_unconfirmed_positives # type: ignore + assert actual == expected def test_run_with_confirmed_positive( self, @@ -469,14 +477,18 @@ def test_run_with_confirmed_positive( """ Test run when there is a confirmed positive. """ - _, confirmed_positive = object_in_world.ObjectInWorld.create(1, 1, 1) + # Setup + result, confirmed_positive = object_in_world.ObjectInWorld.create(1, 1, 1) + assert result assert confirmed_positive is not None tracker._LandingPadTracking__confirmed_positives.append(confirmed_positive) # type: ignore - expected = confirmed_positive + expected = [confirmed_positive] + # Run result, actual = tracker.run(detections_1) + # Check assert result assert actual == expected @@ -488,12 +500,17 @@ def test_run_with_false_positive( """ Test to see if run function doesn't add landing pads that are similar to false positives. """ - _, false_positive = object_in_world.ObjectInWorld.create(1, 1, 1) + # Setup + result, false_positive = object_in_world.ObjectInWorld.create(1, 1, 1) + assert result assert false_positive is not None tracker._LandingPadTracking__false_positives.append(false_positive) # type: ignore - expected_unconfirmed_positives = [detections_2[3], detections_2[2], detections_2[4]] + expected = [detections_2[3], detections_2[2], detections_2[4]] - tracker.run(detections_2) + # Run + result, actual = tracker.run(detections_2) - assert tracker._LandingPadTracking__unconfirmed_positives == expected_unconfirmed_positives # type: ignore + # Check + assert result + assert actual == expected From fb2897285348d796c109afb5ba85028c8d6d4a7e Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Thu, 31 Oct 2024 00:27:14 -0400 Subject: [PATCH 04/16] fixed formatting issue --- main_2024.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main_2024.py b/main_2024.py index c18bd0c0..2f6cacd0 100644 --- a/main_2024.py +++ b/main_2024.py @@ -398,7 +398,7 @@ def main() -> int: cluster_estimation_data = cluster_estimation_to_main_queue.queue.get_nowait() except queue.Empty: cluster_estimation_data = None - + if cluster_estimation_data is not None: for object_in_world in cluster_estimation_data: main_logger.debug("Cluster in world: ", True) From 415829a2b29a374a63c537fec894a6454aaf5cdf Mon Sep 17 00:00:00 2001 From: Aayush Deshpande <88875959+Aayush1104@users.noreply.github.com> Date: Fri, 1 Nov 2024 13:37:02 -0400 Subject: [PATCH 05/16] Added code for logging data in flight interface and data merge (#215) * Added code for logging data in flight interface and data merge * changes made * added __repr__ method * removed empty line --- modules/data_merge/data_merge_worker.py | 26 ++++++++++++++++++-- modules/detections_and_time.py | 9 +++++-- modules/flight_interface/flight_interface.py | 12 ++++++++- modules/odometry_and_time.py | 2 +- 4 files changed, 43 insertions(+), 6 deletions(-) diff --git a/modules/data_merge/data_merge_worker.py b/modules/data_merge/data_merge_worker.py index dc19e5b7..d3357dc5 100644 --- a/modules/data_merge/data_merge_worker.py +++ b/modules/data_merge/data_merge_worker.py @@ -2,6 +2,8 @@ Merges detections and telemetry by time. """ +import os +import pathlib import queue from utilities.workers import queue_proxy_wrapper @@ -9,6 +11,7 @@ from .. import detections_and_time from .. import merged_odometry_detections from .. import odometry_and_time +from ..common.logger.modules import logger def data_merge_worker( @@ -28,7 +31,14 @@ def data_merge_worker( Merge work is done in the worker process as the queues and control mechanisms are naturally available. """ - # TODO: Logging? + 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 + + assert local_logger is not None # Mitigate potential deadlock caused by early program exit try: @@ -39,7 +49,7 @@ def data_merge_worker( timeout=timeout, ) except queue.Empty: - print("ERROR: Queue timed out on startup") + local_logger.error("Queue timed out on startup", True) return while not controller.is_exit_requested(): @@ -72,15 +82,27 @@ def data_merge_worker( previous_odometry.odometry_data, detections.detections, ) + + odometry_timestamp = previous_odometry.timestamp else: result, merged = merged_odometry_detections.MergedOdometryDetections.create( current_odometry.odometry_data, detections.detections, ) + odometry_timestamp = current_odometry.timestamp + + local_logger.info( + f"Odometry timestamp: {odometry_timestamp}, detections timestamp: {detections.timestamp}, detections - odometry: {detections.timestamp - odometry_timestamp}", + True, + ) + if not result: + local_logger.warning("Failed to create merged odometry and detections", True) continue + local_logger.info(str(merged), True) + # Get Pylance to stop complaining assert merged is not None diff --git a/modules/detections_and_time.py b/modules/detections_and_time.py index da04d00f..fcedcd9e 100644 --- a/modules/detections_and_time.py +++ b/modules/detections_and_time.py @@ -108,10 +108,15 @@ def __str__(self) -> str: To string. """ return ( - f"{self.__class__}, time: {int(self.timestamp)}, size: {len(self)}\n" - + f"{self.detections}" + f"{self.__class__}, time: {self.timestamp}, size: {len(self)}\n" + f"{self.detections}" ) + def __repr__(self) -> str: + """ + For collections (e.g. list). + """ + return str(self) + def __len__(self) -> int: """ Gets the number of detected objects. diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index b49c5fa9..ab87a265 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -87,7 +87,17 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": # Get Pylance to stop complaining assert odometry_local is not None - return odometry_and_time.OdometryAndTime.create(odometry_local) + + result, odometry_and_time_object = odometry_and_time.OdometryAndTime.create(odometry_local) + if not result: + return False, None + + # Get Pylance to stop complaining + assert odometry_and_time_object is not None + + self.__logger.info(str(odometry_and_time_object), True) + + return True, odometry_and_time_object def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: """ diff --git a/modules/odometry_and_time.py b/modules/odometry_and_time.py index a6b421a2..0c3ae8d7 100644 --- a/modules/odometry_and_time.py +++ b/modules/odometry_and_time.py @@ -47,4 +47,4 @@ def __str__(self) -> str: """ To string. """ - return f"{self.__class__}, time: {int(self.timestamp)}\n" + f"{self.odometry_data}" + return f"{self.__class__}, time: {self.timestamp}\n" + f"{self.odometry_data}" From 62e5b7a01c2055315f2c95cb7844254b6e7e1dff Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Fri, 1 Nov 2024 15:15:34 -0400 Subject: [PATCH 06/16] implemented reviewed changes --- main_2024.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/main_2024.py b/main_2024.py index 2f6cacd0..4349ea9e 100644 --- a/main_2024.py +++ b/main_2024.py @@ -113,9 +113,9 @@ def main() -> int: GEOLOCATION_CAMERA_ORIENTATION_PITCH = config["geolocation"]["camera_orientation_pitch"] GEOLOCATION_CAMERA_ORIENTATION_ROLL = config["geolocation"]["camera_orientation_roll"] - MIN_ACTIVATION_THRESHOLD = config["cluster_merge"]["min_activation_threshold"] - MIN_NEW_POINTS_TO_RUN = config["cluster_merge"]["min_new_points_to_run"] - RANDOM_STATE = config["cluster_merge"]["random_state"] + MIN_ACTIVATION_THRESHOLD = config["cluster_estimation"]["min_activation_threshold"] + MIN_NEW_POINTS_TO_RUN = config["cluster_estimation"]["min_new_points_to_run"] + RANDOM_STATE = config["cluster_estimation"]["random_state"] # pylint: enable=invalid-name except KeyError as exception: main_logger.error(f"Config key(s) not found: {exception}", True) @@ -401,8 +401,8 @@ def main() -> int: if cluster_estimation_data is not None: for object_in_world in cluster_estimation_data: - main_logger.debug("Cluster in world: ", True) - main_logger.debug(object_in_world.__str__) + main_logger.debug("Cluster in world: ", (object_in_world is not None)) + main_logger.debug(str(object_in_world)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break From 7543e3d61112394260de3f371eddf348fa1c7095 Mon Sep 17 00:00:00 2001 From: Evan <57083990+Evang264@users.noreply.github.com> Date: Fri, 1 Nov 2024 16:13:02 -0400 Subject: [PATCH 07/16] Move `local_global_conversion` (#213) * Move local_global_conversion.py and its dependency to common repo * Refactored imports --- modules/common | 2 +- modules/drone_odometry_local.py | 133 ------------------ modules/flight_interface/flight_interface.py | 6 +- .../local_global_conversion.py | 100 ------------- modules/merged_odometry_detections.py | 2 +- modules/odometry_and_time.py | 2 +- requirements.txt | 1 - tests/integration/test_data_merge_worker.py | 3 +- tests/integration/test_geolocation_worker.py | 3 +- tests/test_search_pattern.py | 2 +- tests/unit/test_decision.py | 2 +- tests/unit/test_geolocation.py | 5 +- 12 files changed, 12 insertions(+), 249 deletions(-) delete mode 100644 modules/drone_odometry_local.py delete mode 100644 modules/flight_interface/local_global_conversion.py diff --git a/modules/common b/modules/common index 5c3894e0..07b515df 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit 5c3894e043c5129a1c0579c6f6acae2cc7a52a18 +Subproject commit 07b515df9b56b9a65142d826f720b88f66c1a5f0 diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py deleted file mode 100644 index 7071b697..00000000 --- a/modules/drone_odometry_local.py +++ /dev/null @@ -1,133 +0,0 @@ -""" -Drone odometry in local space (origin at home location). -""" - -from .common.mavlink.modules import drone_odometry - - -class DronePositionLocal: - """ - Drone position in NED system. - """ - - __create_key = object() - - @classmethod - def create( - cls, north: float, east: float, down: float - ) -> "tuple[bool, DronePositionLocal | None]": - """ - North, east, down in metres. - """ - return True, DronePositionLocal(cls.__create_key, north, east, down) - - def __init__( - self, class_private_create_key: object, north: float, east: float, down: float - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is DronePositionLocal.__create_key, "Use create() method" - - self.north = north - self.east = east - self.down = down - - def __str__(self) -> str: - """ - To string. - """ - return f"DronePositionLocal (NED): {self.north}, {self.east}, {self.down}" - - -class DroneOrientationLocal: - """ - Wrapper for DroneOrientation as it is the same in both local and global space. - """ - - __create_key = object() - - @classmethod - def create_new( - cls, yaw: float, pitch: float, roll: float - ) -> "tuple[bool, DroneOrientationLocal | None]": - """ - Yaw, pitch, roll in radians. - """ - result, orientation = drone_odometry.DroneOrientation.create(yaw, pitch, roll) - if not result: - return False, None - - # Get Pylance to stop complaining - assert orientation is not None - - return True, DroneOrientationLocal(cls.__create_key, orientation) - - @classmethod - def create_wrap( - cls, orientation: drone_odometry.DroneOrientation - ) -> "tuple[bool, DroneOrientationLocal | None]": - """ - Wrap existing orientation. - """ - return True, DroneOrientationLocal(cls.__create_key, orientation) - - def __init__( - self, class_private_create_key: object, orientation: drone_odometry.DroneOrientation - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is DroneOrientationLocal.__create_key, "Use create() method" - - self.orientation = orientation - - def __str__(self) -> str: - """ - To string. - """ - # TODO: Update common - return f"DroneOrientationLocal (YPR rad): {self.orientation.yaw}, {self.orientation.pitch}, {self.orientation.roll}" - - -class DroneOdometryLocal: - """ - Wrapper for DronePositionLocal and DroneOrientationLocal. - """ - - __create_key = object() - - @classmethod - def create( - cls, position: DronePositionLocal, orientation: DroneOrientationLocal - ) -> "tuple[bool, DroneOdometryLocal | None]": - """ - Position and orientation in one class. - """ - if position is None: - return False, None - - if orientation is None: - return False, None - - return True, DroneOdometryLocal(cls.__create_key, position, orientation) - - def __init__( - self, - class_private_create_key: object, - position: DronePositionLocal, - orientation: DroneOrientationLocal, - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is DroneOdometryLocal.__create_key, "Use create() method" - - self.position = position - self.orientation = orientation - - def __str__(self) -> str: - """ - To string. - """ - return f"DroneOdometryLocal: {self.position}, {self.orientation}" diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index ab87a265..9786f413 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -2,13 +2,13 @@ Creates flight controller and combines odometry data and timestamp. """ -from . import local_global_conversion -from .. import drone_odometry_local -from .. import odometry_and_time from .. import decision_command +from .. import odometry_and_time 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 flight_controller +from ..common.mavlink.modules import local_global_conversion class FlightInterface: diff --git a/modules/flight_interface/local_global_conversion.py b/modules/flight_interface/local_global_conversion.py deleted file mode 100644 index 1797fa27..00000000 --- a/modules/flight_interface/local_global_conversion.py +++ /dev/null @@ -1,100 +0,0 @@ -""" -Conversion between local and global space. -""" - -import pymap3d as pm - -from .. import drone_odometry_local -from ..common.mavlink.modules import drone_odometry - - -def drone_position_global_from_local( - home_location: drone_odometry.DronePosition, - drone_position_local: drone_odometry_local.DronePositionLocal, -) -> "tuple[bool, drone_odometry.DronePosition | None]": - """ - Local coordinates to global coordinates. - Return: Drone position in WGS 84. - """ - latitude, longitude, altitude = pm.ned2geodetic( - drone_position_local.north, - drone_position_local.east, - drone_position_local.down, - home_location.latitude, - home_location.longitude, - home_location.altitude, - ) - - result, drone_position = drone_odometry.DronePosition.create( - latitude, - longitude, - altitude, - ) - if not result: - return False, None - - # Get Pylance to stop complaining - assert drone_position is not None - - return True, drone_position - - -def __drone_position_local_from_global( - home_location: drone_odometry.DronePosition, drone_position: drone_odometry.DronePosition -) -> "tuple[bool, drone_odometry_local.DronePositionLocal | None]": - """ - Global coordinates to local coordinates. - Return: Drone position relative to home location (NED system). - """ - north, east, down = pm.geodetic2ned( - drone_position.latitude, - drone_position.longitude, - drone_position.altitude, - home_location.latitude, - home_location.longitude, - home_location.altitude, - ) - - result, drone_position_local = drone_odometry_local.DronePositionLocal.create( - north, - east, - down, - ) - if not result: - return False, None - - # Get Pylance to stop complaining - assert drone_position_local is not None - - return True, drone_position_local - - -def drone_odometry_local_from_global( - odometry: drone_odometry.DroneOdometry, home_location: drone_odometry.DronePosition -) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]": - """ - Converts global odometry to local. - """ - result, drone_position_local = __drone_position_local_from_global( - home_location, - odometry.position, - ) - if not result: - return False, None - - # Get Pylance to stop complaining - assert drone_position_local is not None - - result, drone_orientation_local = drone_odometry_local.DroneOrientationLocal.create_wrap( - odometry.orientation, - ) - if not result: - return False, None - - # Get Pylance to stop complaining - assert drone_orientation_local is not None - - return drone_odometry_local.DroneOdometryLocal.create( - drone_position_local, - drone_orientation_local, - ) diff --git a/modules/merged_odometry_detections.py b/modules/merged_odometry_detections.py index 8df23229..bae95a1d 100644 --- a/modules/merged_odometry_detections.py +++ b/modules/merged_odometry_detections.py @@ -3,7 +3,7 @@ """ from . import detections_and_time -from . import drone_odometry_local +from .common.mavlink.modules import drone_odometry_local class MergedOdometryDetections: diff --git a/modules/odometry_and_time.py b/modules/odometry_and_time.py index 0c3ae8d7..9494b864 100644 --- a/modules/odometry_and_time.py +++ b/modules/odometry_and_time.py @@ -4,7 +4,7 @@ import time -from . import drone_odometry_local +from .common.mavlink.modules import drone_odometry_local class OdometryAndTime: diff --git a/requirements.txt b/requirements.txt index 3f730a3a..b705d13f 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,7 +1,6 @@ # Packages listed in alphabetical order numpy opencv-python -pymap3d pytest pyyaml scikit-learn diff --git a/tests/integration/test_data_merge_worker.py b/tests/integration/test_data_merge_worker.py index 48898826..2f7b705b 100644 --- a/tests/integration/test_data_merge_worker.py +++ b/tests/integration/test_data_merge_worker.py @@ -7,15 +7,14 @@ import numpy as np -from modules import drone_odometry_local from modules import detections_and_time from modules import merged_odometry_detections from modules import odometry_and_time +from modules.common.mavlink.modules import drone_odometry_local from modules.data_merge import data_merge_worker from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller - DATA_MERGE_WORKER_TIMEOUT = 10.0 # seconds diff --git a/tests/integration/test_geolocation_worker.py b/tests/integration/test_geolocation_worker.py index 2a73cee4..e6f59236 100644 --- a/tests/integration/test_geolocation_worker.py +++ b/tests/integration/test_geolocation_worker.py @@ -8,14 +8,13 @@ import numpy as np from modules import detections_and_time -from modules import drone_odometry_local from modules import merged_odometry_detections +from modules.common.mavlink.modules import drone_odometry_local from modules.geolocation import camera_properties from modules.geolocation import geolocation_worker from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller - WORK_COUNT = 3 diff --git a/tests/test_search_pattern.py b/tests/test_search_pattern.py index 173973f0..1550a248 100644 --- a/tests/test_search_pattern.py +++ b/tests/test_search_pattern.py @@ -7,7 +7,7 @@ from modules import decision_command from modules import odometry_and_time -from modules import drone_odometry_local +from modules.common.mavlink.modules import drone_odometry_local from modules.decision import search_pattern DISTANCE_SQUARED_THRESHOLD = 2.0 diff --git a/tests/unit/test_decision.py b/tests/unit/test_decision.py index 9b24211b..3bd5561f 100644 --- a/tests/unit/test_decision.py +++ b/tests/unit/test_decision.py @@ -8,7 +8,7 @@ from modules import decision_command from modules import object_in_world from modules import odometry_and_time -from modules import drone_odometry_local +from modules.common.mavlink.modules import drone_odometry_local LANDING_PAD_LOCATION_TOLERANCE = 2 diff --git a/tests/unit/test_geolocation.py b/tests/unit/test_geolocation.py index 84a85371..d0e9d1e2 100644 --- a/tests/unit/test_geolocation.py +++ b/tests/unit/test_geolocation.py @@ -7,12 +7,11 @@ from modules import detection_in_world from modules import detections_and_time -from modules import drone_odometry_local from modules import merged_odometry_detections +from modules.common.logger.modules import logger +from modules.common.mavlink.modules import drone_odometry_local from modules.geolocation import camera_properties from modules.geolocation import geolocation -from modules.common.logger.modules import logger - FLOAT_PRECISION_TOLERANCE = 4 From d6183f0e59fcc1b2c7e6a9bba931c2d6a49925ef Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Fri, 1 Nov 2024 20:25:00 -0400 Subject: [PATCH 08/16] fixed review changes --- config.yaml | 4 ++-- main_2024.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/config.yaml b/config.yaml index de8fe916..44bdd6df 100644 --- a/config.yaml +++ b/config.yaml @@ -36,6 +36,6 @@ geolocation: camera_orientation_roll: 0.0 cluster_estimation: - min_activation_threshold: 0 - min_new_points_to_run: 0 + min_activation_threshold: 25 + min_new_points_to_run: 5 random_state: 0 diff --git a/main_2024.py b/main_2024.py index 4349ea9e..0cc5e2e1 100644 --- a/main_2024.py +++ b/main_2024.py @@ -401,7 +401,7 @@ def main() -> int: if cluster_estimation_data is not None: for object_in_world in cluster_estimation_data: - main_logger.debug("Cluster in world: ", (object_in_world is not None)) + main_logger.debug("Cluster in world: ", True) main_logger.debug(str(object_in_world)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) From fd3cea9cf04bf692d6fb1b09132e93426ae1342e Mon Sep 17 00:00:00 2001 From: Xierumeng <22666760+Xierumeng@users.noreply.github.com> Date: Sun, 3 Nov 2024 18:09:01 -0800 Subject: [PATCH 09/16] Changes from repository common refactor (#218) --- documentation/main_multiprocess_example.py | 8 ++-- .../add_random/add_random.py | 2 +- .../add_random/add_random_worker.py | 2 +- .../concatenator/concatenator.py | 2 +- .../concatenator/concatenator_worker.py | 2 +- .../multiprocess_example/countup/countup.py | 2 +- .../countup/countup_worker.py | 2 +- main_2024.py | 8 ++-- modules/common | 2 +- modules/data_merge/data_merge_worker.py | 2 +- modules/detect_target/detect_target_worker.py | 2 +- modules/flight_interface/flight_interface.py | 46 +++++++++---------- .../flight_interface_worker.py | 2 +- modules/geolocation/geolocation.py | 8 ++-- modules/geolocation/geolocation_worker.py | 2 +- modules/merged_odometry_detections.py | 2 +- modules/odometry_and_time.py | 2 +- modules/video_input/video_input.py | 2 +- tests/integration/test_data_merge_worker.py | 16 ++++--- .../test_flight_interface_hardware.py | 2 +- tests/integration/test_geolocation_worker.py | 8 ++-- tests/test_search_pattern.py | 12 +++-- tests/unit/test_decision.py | 16 ++++--- tests/unit/test_geolocation.py | 19 ++++---- utilities/workers/worker_manager.py | 2 +- 25 files changed, 95 insertions(+), 78 deletions(-) diff --git a/documentation/main_multiprocess_example.py b/documentation/main_multiprocess_example.py index 6efa155d..8280cb1b 100644 --- a/documentation/main_multiprocess_example.py +++ b/documentation/main_multiprocess_example.py @@ -11,9 +11,9 @@ from documentation.multiprocess_example.add_random import add_random_worker from documentation.multiprocess_example.concatenator import concatenator_worker from documentation.multiprocess_example.countup import countup_worker -from modules.common.logger.modules import logger -from modules.common.logger.modules import logger_setup_main -from modules.common.logger.read_yaml.modules import read_yaml +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 from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from utilities.workers import worker_manager @@ -44,7 +44,7 @@ def main() -> int: assert config is not None # Setup main logger - result, main_logger, _ = logger_setup_main.setup_main_logger(config) + result, main_logger, _ = logger_main_setup.setup_main_logger(config) if not result: print("ERROR: Failed to create main logger") return -1 diff --git a/documentation/multiprocess_example/add_random/add_random.py b/documentation/multiprocess_example/add_random/add_random.py index b6f16718..e3cfee62 100644 --- a/documentation/multiprocess_example/add_random/add_random.py +++ b/documentation/multiprocess_example/add_random/add_random.py @@ -5,7 +5,7 @@ import time import random -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from .. import intermediate_struct diff --git a/documentation/multiprocess_example/add_random/add_random_worker.py b/documentation/multiprocess_example/add_random/add_random_worker.py index 190ff640..8c610d36 100644 --- a/documentation/multiprocess_example/add_random/add_random_worker.py +++ b/documentation/multiprocess_example/add_random/add_random_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import add_random diff --git a/documentation/multiprocess_example/concatenator/concatenator.py b/documentation/multiprocess_example/concatenator/concatenator.py index df9f3a8a..f6460128 100644 --- a/documentation/multiprocess_example/concatenator/concatenator.py +++ b/documentation/multiprocess_example/concatenator/concatenator.py @@ -4,7 +4,7 @@ import time -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from .. import intermediate_struct diff --git a/documentation/multiprocess_example/concatenator/concatenator_worker.py b/documentation/multiprocess_example/concatenator/concatenator_worker.py index aa821a4d..7a436e97 100644 --- a/documentation/multiprocess_example/concatenator/concatenator_worker.py +++ b/documentation/multiprocess_example/concatenator/concatenator_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import concatenator diff --git a/documentation/multiprocess_example/countup/countup.py b/documentation/multiprocess_example/countup/countup.py index 4ddb7ca1..bc978bad 100644 --- a/documentation/multiprocess_example/countup/countup.py +++ b/documentation/multiprocess_example/countup/countup.py @@ -4,7 +4,7 @@ import time -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger class Countup: diff --git a/documentation/multiprocess_example/countup/countup_worker.py b/documentation/multiprocess_example/countup/countup_worker.py index 7960e1d7..5718d311 100644 --- a/documentation/multiprocess_example/countup/countup_worker.py +++ b/documentation/multiprocess_example/countup/countup_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import countup diff --git a/main_2024.py b/main_2024.py index 66ad913d..f6297226 100644 --- a/main_2024.py +++ b/main_2024.py @@ -19,9 +19,9 @@ from modules.data_merge import data_merge_worker from modules.geolocation import geolocation_worker from modules.geolocation import camera_properties -from modules.common.logger.modules import logger -from modules.common.logger.modules import logger_setup_main -from modules.common.logger.read_yaml.modules import read_yaml +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 from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from utilities.workers import worker_manager @@ -64,7 +64,7 @@ def main() -> int: assert config_logger is not None # Setup main logger - result, main_logger, logging_path = logger_setup_main.setup_main_logger(config_logger) + result, main_logger, logging_path = logger_main_setup.setup_main_logger(config_logger) if not result: print("ERROR: Failed to create main logger") return -1 diff --git a/modules/common b/modules/common index 07b515df..a32385ee 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit 07b515df9b56b9a65142d826f720b88f66c1a5f0 +Subproject commit a32385eefab7d6a5468f5b8f2e708860a5f085d6 diff --git a/modules/data_merge/data_merge_worker.py b/modules/data_merge/data_merge_worker.py index d3357dc5..989ed6e7 100644 --- a/modules/data_merge/data_merge_worker.py +++ b/modules/data_merge/data_merge_worker.py @@ -11,7 +11,7 @@ from .. import detections_and_time from .. import merged_odometry_detections from .. import odometry_and_time -from ..common.logger.modules import logger +from ..common.modules.logger import logger def data_merge_worker( diff --git a/modules/detect_target/detect_target_worker.py b/modules/detect_target/detect_target_worker.py index 7c1efe56..7d03b9fa 100644 --- a/modules/detect_target/detect_target_worker.py +++ b/modules/detect_target/detect_target_worker.py @@ -8,7 +8,7 @@ from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import detect_target_factory -from ..common.logger.modules import logger +from ..common.modules.logger import logger def detect_target_worker( diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 9786f413..e1b47d86 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -4,11 +4,11 @@ from .. import decision_command from .. import odometry_and_time -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 flight_controller -from ..common.mavlink.modules import local_global_conversion +from ..common.modules.logger import logger +from ..common.modules import position_global +from ..common.modules import position_local +from ..common.modules.mavlink import flight_controller +from ..common.modules.mavlink import local_global_conversion class FlightInterface: @@ -39,21 +39,23 @@ def create( # Get Pylance to stop complaining assert controller is not None - result, home_location = controller.get_home_location(timeout_home) + result, home_position = controller.get_home_position(timeout_home) if not result: - local_logger.error("home_location could not be created", True) + local_logger.error("home_position could not be created", True) return False, None # Get Pylance to stop complaining - assert home_location is not None + assert home_position is not None - return True, FlightInterface(cls.__create_key, controller, home_location, local_logger) + local_logger.info(str(home_position), True) + + return True, FlightInterface(cls.__create_key, controller, home_position, local_logger) def __init__( self, class_private_create_key: object, controller: flight_controller.FlightController, - home_location: drone_odometry.DronePosition, + home_position: position_global.PositionGlobal, local_logger: logger.Logger, ) -> None: """ @@ -62,11 +64,9 @@ def __init__( assert class_private_create_key is FlightInterface.__create_key, "Use create() method" self.controller = controller - self.__home_location = home_location + self.__home_position = home_position self.__logger = local_logger - self.__logger.info(str(self.__home_location), True) - def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ Returns a possible OdometryAndTime with current timestamp. @@ -79,8 +79,8 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": assert odometry is not None result, odometry_local = local_global_conversion.drone_odometry_local_from_global( + self.__home_position, odometry, - self.__home_location, ) if not result: return False, None @@ -117,7 +117,7 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: # Convert current global position to local NED coordinates. result, current_local_odometry = ( local_global_conversion.drone_odometry_local_from_global( - current_odometry, self.__home_location + self.__home_position, current_odometry ) ) if not result or current_local_odometry is None: @@ -128,15 +128,15 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: target_east = current_local_odometry.position.east + command_position[1] target_down = current_local_odometry.position.down + command_position[2] - result, target_local_position = drone_odometry_local.DronePositionLocal.create( + result, target_local_position = position_local.PositionLocal.create( target_north, target_east, target_down ) if not result or target_local_position is None: return False result, target_global_position = ( - local_global_conversion.drone_position_global_from_local( - self.__home_location, target_local_position + local_global_conversion.position_global_from_position_local( + self.__home_position, target_local_position ) ) if not result or target_global_position is None: @@ -148,7 +148,7 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: if command_type == decision_command.DecisionCommand.CommandType.MOVE_TO_ABSOLUTE_POSITION: # Move to absolute position. # Note that command_position[2] is the absolute altitude not relative altitude. - result, target_position = drone_odometry.DronePosition.create( + result, target_position = position_global.PositionGlobal.create( command_position[0], command_position[1], command_position[2] ) @@ -171,7 +171,7 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: # Convert current global position to local NED coordinates result, current_local_odometry = ( local_global_conversion.drone_odometry_local_from_global( - current_odometry, self.__home_location + self.__home_position, current_odometry ) ) if not result or current_local_odometry is None: @@ -183,15 +183,15 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: target_down = current_local_odometry.position.down + command_position[2] # Create target local position. - result, target_local_position = drone_odometry_local.DronePositionLocal.create( + result, target_local_position = position_local.PositionLocal.create( target_north, target_east, target_down ) if not result or target_local_position is None: return False # Convert target local position to global position. result, target_global_position = ( - local_global_conversion.drone_position_global_from_local( - self.__home_location, target_local_position + local_global_conversion.position_global_from_position_local( + self.__home_position, target_local_position ) ) if not result or target_global_position is None: diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 0c81dab9..2ead7bae 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -9,7 +9,7 @@ from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import flight_interface -from ..common.logger.modules import logger +from ..common.modules.logger import logger def flight_interface_worker( diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py index 4006d447..63641eb5 100644 --- a/modules/geolocation/geolocation.py +++ b/modules/geolocation/geolocation.py @@ -9,7 +9,7 @@ from .. import detection_in_world from .. import detections_and_time from .. import merged_odometry_detections -from ..common.logger.modules import logger +from ..common.modules.logger import logger class Geolocation: @@ -285,9 +285,9 @@ def run( # Generate projective perspective matrix # Camera rotation in world result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( - detections.odometry_local.orientation.orientation.yaw, - detections.odometry_local.orientation.orientation.pitch, - detections.odometry_local.orientation.orientation.roll, + detections.odometry_local.orientation.yaw, + detections.odometry_local.orientation.pitch, + detections.odometry_local.orientation.roll, ) if not result: self.__logger.error("Drone rotation matrix could not be created") diff --git a/modules/geolocation/geolocation_worker.py b/modules/geolocation/geolocation_worker.py index ac040a95..11781688 100644 --- a/modules/geolocation/geolocation_worker.py +++ b/modules/geolocation/geolocation_worker.py @@ -9,7 +9,7 @@ from utilities.workers import worker_controller from . import camera_properties from . import geolocation -from ..common.logger.modules import logger +from ..common.modules.logger import logger def geolocation_worker( diff --git a/modules/merged_odometry_detections.py b/modules/merged_odometry_detections.py index bae95a1d..fb7e4385 100644 --- a/modules/merged_odometry_detections.py +++ b/modules/merged_odometry_detections.py @@ -3,7 +3,7 @@ """ from . import detections_and_time -from .common.mavlink.modules import drone_odometry_local +from .common.modules.mavlink import drone_odometry_local class MergedOdometryDetections: diff --git a/modules/odometry_and_time.py b/modules/odometry_and_time.py index 9494b864..6ea03766 100644 --- a/modules/odometry_and_time.py +++ b/modules/odometry_and_time.py @@ -4,7 +4,7 @@ import time -from .common.mavlink.modules import drone_odometry_local +from .common.modules.mavlink import drone_odometry_local class OdometryAndTime: diff --git a/modules/video_input/video_input.py b/modules/video_input/video_input.py index 28ebe114..faaef9ac 100644 --- a/modules/video_input/video_input.py +++ b/modules/video_input/video_input.py @@ -2,8 +2,8 @@ Combines image and timestamp together. """ -from ..common.camera.modules import camera_device from .. import image_and_time +from ..common.modules.camera import camera_device class VideoInput: diff --git a/tests/integration/test_data_merge_worker.py b/tests/integration/test_data_merge_worker.py index 2f7b705b..bdcf6b7e 100644 --- a/tests/integration/test_data_merge_worker.py +++ b/tests/integration/test_data_merge_worker.py @@ -10,7 +10,9 @@ from modules import detections_and_time from modules import merged_odometry_detections from modules import odometry_and_time -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local from modules.data_merge import data_merge_worker from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller @@ -44,15 +46,17 @@ def simulate_flight_input_worker( Place the odometry into the queue. """ # Timestamp is stored in latitude - result, position = drone_odometry_local.DronePositionLocal.create(timestamp, 0.0, -1.0) + result, drone_position = position_local.PositionLocal.create(timestamp, 0.0, -1.0) assert result - assert position is not None + assert drone_position is not None - result, orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0) + result, drone_orientation = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert drone_orientation is not None - result, odometry = drone_odometry_local.DroneOdometryLocal.create(position, orientation) + result, odometry = drone_odometry_local.DroneOdometryLocal.create( + drone_position, drone_orientation + ) assert result assert odometry is not None diff --git a/tests/integration/test_flight_interface_hardware.py b/tests/integration/test_flight_interface_hardware.py index e38c0cba..e0050441 100644 --- a/tests/integration/test_flight_interface_hardware.py +++ b/tests/integration/test_flight_interface_hardware.py @@ -5,7 +5,7 @@ import pathlib from modules.flight_interface import flight_interface -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger MAVLINK_CONNECTION_ADDRESS = "tcp:localhost:14550" diff --git a/tests/integration/test_geolocation_worker.py b/tests/integration/test_geolocation_worker.py index e6f59236..db984c99 100644 --- a/tests/integration/test_geolocation_worker.py +++ b/tests/integration/test_geolocation_worker.py @@ -9,7 +9,9 @@ from modules import detections_and_time from modules import merged_odometry_detections -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local from modules.geolocation import camera_properties from modules.geolocation import geolocation_worker from utilities.workers import queue_proxy_wrapper @@ -22,7 +24,7 @@ def simulate_previous_worker(in_queue: queue_proxy_wrapper.QueueProxyWrapper) -> """ Place the image into the queue. """ - result_simulate, drone_position = drone_odometry_local.DronePositionLocal.create( + result_simulate, drone_position = position_local.PositionLocal.create( 0.0, 0.0, -100.0, @@ -30,7 +32,7 @@ def simulate_previous_worker(in_queue: queue_proxy_wrapper.QueueProxyWrapper) -> assert result_simulate assert drone_position is not None - result_simulate, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result_simulate, drone_orientation = orientation.Orientation.create( 0.0, -np.pi / 2, 0.0, diff --git a/tests/test_search_pattern.py b/tests/test_search_pattern.py index 1550a248..e78cba83 100644 --- a/tests/test_search_pattern.py +++ b/tests/test_search_pattern.py @@ -7,7 +7,9 @@ from modules import decision_command from modules import odometry_and_time -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local from modules.decision import search_pattern DISTANCE_SQUARED_THRESHOLD = 2.0 @@ -65,9 +67,11 @@ def create_drone_odometry( """ Creates an OdometryAndTime instance representing the drone's current position. """ - position = drone_odometry_local.DronePositionLocal.create(pos_y, pos_x, -depth)[1] - orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0)[1] - odometry_data = drone_odometry_local.DroneOdometryLocal.create(position, orientation)[1] + drone_position = position_local.PositionLocal.create(pos_y, pos_x, -depth)[1] + drone_orientation = orientation.Orientation.create(0.0, 0.0, 0.0)[1] + odometry_data = drone_odometry_local.DroneOdometryLocal.create( + drone_position, drone_orientation + )[1] odometry = odometry_and_time.OdometryAndTime.create(odometry_data)[1] return odometry diff --git a/tests/unit/test_decision.py b/tests/unit/test_decision.py index 3bd5561f..3b3cf68f 100644 --- a/tests/unit/test_decision.py +++ b/tests/unit/test_decision.py @@ -8,7 +8,9 @@ from modules import decision_command from modules import object_in_world from modules import odometry_and_time -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local LANDING_PAD_LOCATION_TOLERANCE = 2 @@ -89,19 +91,21 @@ def drone_odometry_and_time() -> odometry_and_time.OdometryAndTime: # type: ign Create an OdometryAndTime instance with the drone positioned within tolerance of landing pad. """ # Creating the position within tolerance of the specified landing pad. - result, position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( BEST_PAD_LOCATION_X - DRONE_OFFSET_FROM_PAD, BEST_PAD_LOCATION_Y - DRONE_OFFSET_FROM_PAD, -5.0, ) assert result - assert position is not None + assert drone_position is not None - result, orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0) + result, drone_orientation = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert drone_orientation is not None - result, odometry_data = drone_odometry_local.DroneOdometryLocal.create(position, orientation) + result, odometry_data = drone_odometry_local.DroneOdometryLocal.create( + drone_position, drone_orientation + ) assert result assert odometry_data is not None diff --git a/tests/unit/test_geolocation.py b/tests/unit/test_geolocation.py index d0e9d1e2..47a41e4b 100644 --- a/tests/unit/test_geolocation.py +++ b/tests/unit/test_geolocation.py @@ -8,11 +8,14 @@ from modules import detection_in_world from modules import detections_and_time from modules import merged_odometry_detections -from modules.common.logger.modules import logger -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.logger import logger +from modules.common.modules.mavlink import drone_odometry_local from modules.geolocation import camera_properties from modules.geolocation import geolocation + FLOAT_PRECISION_TOLERANCE = 4 @@ -815,7 +818,7 @@ def test_basic( 2 detections. """ # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( 0.0, 0.0, -100.0, @@ -823,7 +826,7 @@ def test_basic( assert result assert drone_position is not None - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result, drone_orientation = orientation.Orientation.create( 0.0, -np.pi / 2, 0.0, @@ -921,7 +924,7 @@ def test_advanced( 2 point detections. """ # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( 10.0 - np.cos(-np.pi / 12), # Camera at 10m north, drone at relative 0.0, -100.0 - np.sin(-np.pi / 12), # Camera at 100m above ground @@ -929,7 +932,7 @@ def test_advanced( assert result assert drone_position is not None - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result, drone_orientation = orientation.Orientation.create( 0.0, np.pi / 12, -np.pi, @@ -1032,7 +1035,7 @@ def test_bad_direction( Bad direction. """ # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( 0.0, 0.0, -100.0, @@ -1040,7 +1043,7 @@ def test_bad_direction( assert result assert drone_position is not None - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result, drone_orientation = orientation.Orientation.create( 0.0, 0.0, 0.0, diff --git a/utilities/workers/worker_manager.py b/utilities/workers/worker_manager.py index 4a5a26a2..be7480a7 100644 --- a/utilities/workers/worker_manager.py +++ b/utilities/workers/worker_manager.py @@ -4,7 +4,7 @@ import multiprocessing as mp -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import worker_controller from utilities.workers import queue_proxy_wrapper From 617ae90cc0ac57f957eb7a53a4414632d52e0bdb Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Wed, 9 Oct 2024 12:21:06 -0400 Subject: [PATCH 10/16] redo of integrating cluster estimation worker into main --- modules/common | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/common b/modules/common index 5c3894e0..a32385ee 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit 5c3894e043c5129a1c0579c6f6acae2cc7a52a18 +Subproject commit a32385eefab7d6a5468f5b8f2e708860a5f085d6 From 05fdf6309b973c2cbbcf46e2dad7b63c24d14588 Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Wed, 9 Oct 2024 12:21:06 -0400 Subject: [PATCH 11/16] redo of integrating cluster estimation worker into main --- config.yaml | 5 +++++ main_2024.py | 62 ++++++++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 60 insertions(+), 7 deletions(-) diff --git a/config.yaml b/config.yaml index b0dd9811..62534d32 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_merge: + min_activation_threshold: 0 + min_new_points_to_run: 0 + random_state: 0 \ No newline at end of file diff --git a/main_2024.py b/main_2024.py index f6297226..4ffc4b9a 100644 --- a/main_2024.py +++ b/main_2024.py @@ -19,6 +19,7 @@ from modules.data_merge import data_merge_worker from modules.geolocation import geolocation_worker from modules.geolocation import camera_properties +from modules.cluster_estimation import cluster_estimation_worker from modules.common.modules.logger import logger from modules.common.modules.logger import logger_main_setup from modules.common.modules.read_yaml import read_yaml @@ -85,8 +86,8 @@ def main() -> int: VIDEO_INPUT_SAVE_PREFIX = str(pathlib.Path(logging_path, VIDEO_INPUT_SAVE_NAME_PREFIX)) DETECT_TARGET_WORKER_COUNT = config["detect_target"]["worker_count"] - detect_target_option_int = config["detect_target"]["option"] - DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(detect_target_option_int) + DETECT_TARGET_OPTION_INT = config["detect_target"]["option"] + DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(DETECT_TARGET_OPTION_INT) DETECT_TARGET_DEVICE = "cpu" if args.cpu else config["detect_target"]["device"] DETECT_TARGET_MODEL_PATH = config["detect_target"]["model_path"] DETECT_TARGET_OVERRIDE_FULL_PRECISION = args.full @@ -111,6 +112,10 @@ 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_merge"]["min_activation_threshold"] + MIN_NEW_POINTS_TO_RUN = config["cluster_merge"]["min_new_points_to_run"] + RANDOM_STATE = config["cluster_merge"]["random_state"] # pylint: enable=invalid-name except KeyError as exception: main_logger.error(f"Config key(s) not found: {exception}", True) @@ -139,7 +144,7 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -147,6 +152,10 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + cluster_estimation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) result, camera_intrinsics = camera_properties.CameraIntrinsics.create( GEOLOCATION_RESOLUTION_X, @@ -266,7 +275,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_main_queue], + output_queues=[geolocation_to_cluster_estimation_queue], controller=controller, local_logger=main_logger, ) @@ -277,6 +286,22 @@ def main() -> int: # Get Pylance to stop complaining assert geolocation_worker_properties is not None + result, cluster_estimation_worker_properties = worker_manager.WorkerProperties.create( + count=1, + target=cluster_estimation_worker.cluster_estimation_worker, + work_arguments=(MIN_ACTIVATION_THRESHOLD, MIN_NEW_POINTS_TO_RUN, RANDOM_STATE), + input_queues=[geolocation_to_cluster_estimation_queue], + output_queues=[cluster_estimation_to_main_queue], + controller=controller, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create arguments for Video Input", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_worker_properties is not None + # Create managers worker_managers = [] @@ -345,6 +370,19 @@ def main() -> int: worker_managers.append(geolocation_manager) + result, cluster_estimation_manager = worker_manager.WorkerManager.create( + worker_properties=cluster_estimation_worker_properties, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create manager for Flight Interface", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_manager is not None + + worker_managers.append(cluster_estimation_manager) + # Run for manager in worker_managers: manager.start_workers() @@ -357,7 +395,7 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_main_queue.queue.get_nowait() + geolocation_data = geolocation_to_cluster_estimation_queue.queue.get_nowait() except queue.Empty: geolocation_data = None @@ -374,7 +412,16 @@ def main() -> int: main_logger.debug( "geolocation confidence: " + str(detection_world.confidence), True ) - + try: + cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() + except queue.Empty: + cluster_estimations = None + if cluster_estimations is not None: + for cluster in cluster_estimations: + main_logger.debug("Cluser in world: True") + main_logger.debug("Cluster location x: " + str(cluster.location_x)) + main_logger.debug("Cluster location y: " + str(cluster.location_y)) + main_logger.debug("Cluster spherical variance: " + str(cluster.spherical_variance)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break @@ -386,8 +433,9 @@ def main() -> int: detect_target_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_data_merge_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() - geolocation_to_main_queue.fill_and_drain_queue() + geolocation_to_cluster_estimation_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() + cluster_estimation_to_main_queue.fill_and_drain_queue() for manager in worker_managers: manager.join_workers() From d86f7cec4ac3e9831352fe718b8e69286ec71e22 Mon Sep 17 00:00:00 2001 From: Cyuber Date: Fri, 11 Oct 2024 19:06:27 -0400 Subject: [PATCH 12/16] implemented changes from review --- config.yaml | 4 ++-- main_2024.py | 37 +++++++++---------------------------- modules/object_in_world.py | 6 ++++++ 3 files changed, 17 insertions(+), 30 deletions(-) diff --git a/config.yaml b/config.yaml index 62534d32..de8fe916 100644 --- a/config.yaml +++ b/config.yaml @@ -35,7 +35,7 @@ geolocation: camera_orientation_pitch: -1.57079632679 camera_orientation_roll: 0.0 -cluster_merge: +cluster_estimation: min_activation_threshold: 0 min_new_points_to_run: 0 - random_state: 0 \ No newline at end of file + random_state: 0 diff --git a/main_2024.py b/main_2024.py index 4ffc4b9a..6768d361 100644 --- a/main_2024.py +++ b/main_2024.py @@ -296,7 +296,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 Cluster Estimation", True) return -1 # Get Pylance to stop complaining @@ -375,7 +375,7 @@ def main() -> int: local_logger=main_logger, ) if not result: - main_logger.error("Failed to create manager for Flight Interface", True) + main_logger.error("Failed to create manager for Cluster Estimation", True) return -1 # Get Pylance to stop complaining @@ -395,33 +395,14 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_cluster_estimation_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 - ) - try: - cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() + cluster_estimation_data = cluster_estimation_to_main_queue.queue.get_nowait() except queue.Empty: - cluster_estimations = None - if cluster_estimations is not None: - for cluster in cluster_estimations: - main_logger.debug("Cluser in world: True") - main_logger.debug("Cluster location x: " + str(cluster.location_x)) - main_logger.debug("Cluster location y: " + str(cluster.location_y)) - main_logger.debug("Cluster spherical variance: " + str(cluster.spherical_variance)) + cluster_estimation_data = None + + if cluster_estimation_data is not None: + for object_in_world in cluster_estimation_data: + main_logger.debug("Cluster in world: ", True) + main_logger.debug(object_in_world.__str__) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break diff --git a/modules/object_in_world.py b/modules/object_in_world.py index 1ce9f65e..31f7ef01 100644 --- a/modules/object_in_world.py +++ b/modules/object_in_world.py @@ -38,3 +38,9 @@ def __init__( self.location_x = location_x self.location_y = location_y self.spherical_variance = spherical_variance + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}, location x: {self.location_x}, location y: {self.location_y}, spherical variance: {self.spherical_variance}, label: {self.label}" From a203f426fa737364acba0e283d55118cd87e164e Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Thu, 31 Oct 2024 00:27:14 -0400 Subject: [PATCH 13/16] fixed formatting issue --- main_2024.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main_2024.py b/main_2024.py index 6768d361..100957ee 100644 --- a/main_2024.py +++ b/main_2024.py @@ -398,7 +398,7 @@ def main() -> int: cluster_estimation_data = cluster_estimation_to_main_queue.queue.get_nowait() except queue.Empty: cluster_estimation_data = None - + if cluster_estimation_data is not None: for object_in_world in cluster_estimation_data: main_logger.debug("Cluster in world: ", True) From 4e0602fc42e08db7f8c069c7cf5334d2df647267 Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Fri, 1 Nov 2024 15:15:34 -0400 Subject: [PATCH 14/16] implemented reviewed changes --- main_2024.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/main_2024.py b/main_2024.py index 100957ee..43dcbcce 100644 --- a/main_2024.py +++ b/main_2024.py @@ -113,9 +113,9 @@ def main() -> int: GEOLOCATION_CAMERA_ORIENTATION_PITCH = config["geolocation"]["camera_orientation_pitch"] GEOLOCATION_CAMERA_ORIENTATION_ROLL = config["geolocation"]["camera_orientation_roll"] - MIN_ACTIVATION_THRESHOLD = config["cluster_merge"]["min_activation_threshold"] - MIN_NEW_POINTS_TO_RUN = config["cluster_merge"]["min_new_points_to_run"] - RANDOM_STATE = config["cluster_merge"]["random_state"] + MIN_ACTIVATION_THRESHOLD = config["cluster_estimation"]["min_activation_threshold"] + MIN_NEW_POINTS_TO_RUN = config["cluster_estimation"]["min_new_points_to_run"] + RANDOM_STATE = config["cluster_estimation"]["random_state"] # pylint: enable=invalid-name except KeyError as exception: main_logger.error(f"Config key(s) not found: {exception}", True) @@ -401,8 +401,8 @@ def main() -> int: if cluster_estimation_data is not None: for object_in_world in cluster_estimation_data: - main_logger.debug("Cluster in world: ", True) - main_logger.debug(object_in_world.__str__) + main_logger.debug("Cluster in world: ", (object_in_world is not None)) + main_logger.debug(str(object_in_world)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break From c84fe868edd5c9b7aa5fde328af555550431d32a Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Fri, 1 Nov 2024 20:25:00 -0400 Subject: [PATCH 15/16] fixed review changes --- config.yaml | 4 ++-- main_2024.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/config.yaml b/config.yaml index de8fe916..44bdd6df 100644 --- a/config.yaml +++ b/config.yaml @@ -36,6 +36,6 @@ geolocation: camera_orientation_roll: 0.0 cluster_estimation: - min_activation_threshold: 0 - min_new_points_to_run: 0 + min_activation_threshold: 25 + min_new_points_to_run: 5 random_state: 0 diff --git a/main_2024.py b/main_2024.py index 43dcbcce..9af33b30 100644 --- a/main_2024.py +++ b/main_2024.py @@ -401,7 +401,7 @@ def main() -> int: if cluster_estimation_data is not None: for object_in_world in cluster_estimation_data: - main_logger.debug("Cluster in world: ", (object_in_world is not None)) + main_logger.debug("Cluster in world: ", True) main_logger.debug(str(object_in_world)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) From f0aaaa96aca086970d5198aa13c21ea9b6d0eafe Mon Sep 17 00:00:00 2001 From: Aleksa-M Date: Thu, 7 Nov 2024 05:32:18 -0500 Subject: [PATCH 16/16] rebased --- modules/common | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/common b/modules/common index a32385ee..5c3894e0 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit a32385eefab7d6a5468f5b8f2e708860a5f085d6 +Subproject commit 5c3894e043c5129a1c0579c6f6acae2cc7a52a18