From 86fe92db10cf87a10ad25d4a2d6b18131caa8418 Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 12 Jun 2024 12:28:25 -0400 Subject: [PATCH 1/3] reformatted DetectionsAndOdometry to contain a list of detections --- modules/detection_and_odometry.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/modules/detection_and_odometry.py b/modules/detection_and_odometry.py index 29c6af8..ee7a88a 100644 --- a/modules/detection_and_odometry.py +++ b/modules/detection_and_odometry.py @@ -8,7 +8,7 @@ class DetectionAndOdometry: """ - Contains LiDAR reading and current local odometry. + Contains LiDAR readings and current local odometry. """ __create_key = object() @@ -16,25 +16,24 @@ class DetectionAndOdometry: @classmethod def create( cls, - detection: lidar_detection.LidarDetection, + detections: "list[lidar_detection.LidarDetection]", local_odometry: drone_odometry_local.DroneOdometryLocal, ) -> "tuple[bool, DetectionAndOdometry | None]": """ - Combines lidar reading with local odometry + Combines lidar readings with local odometry """ - - if detection is None: + if len(detections) == 0: return False, None if local_odometry is None: return False, None - return True, DetectionAndOdometry(cls.__create_key, detection, local_odometry) + return True, DetectionAndOdometry(cls.__create_key, detections, local_odometry) def __init__( self, create_key: object, - detection: lidar_detection.LidarDetection, + detections: "list[lidar_detection.LidarDetection]", local_odometry: drone_odometry_local.DroneOdometryLocal, ) -> None: """ @@ -42,11 +41,11 @@ def __init__( """ assert create_key is DetectionAndOdometry.__create_key, "Use create() method" - self.detection = detection + self.detections = detections self.odometry = local_odometry def __str__(self) -> str: """ String representation. """ - return f"{self.__class__.__name__}, {self.detection}, str{self.odometry}" + return f"{self.__class__.__name__}, Detections ({len(self.detections)}): {self.detections}, str{self.odometry}" From 3c86c8f4c70955262b0790f5b7798801f74448fe Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 12 Jun 2024 14:27:26 -0400 Subject: [PATCH 2/3] changed file name and string representation --- modules/data_merge/data_merge_worker.py | 44 +++++++++++++++++++ ...odometry.py => detections_and_odometry.py} | 11 ++--- 2 files changed, 50 insertions(+), 5 deletions(-) create mode 100644 modules/data_merge/data_merge_worker.py rename modules/{detection_and_odometry.py => detections_and_odometry.py} (73%) diff --git a/modules/data_merge/data_merge_worker.py b/modules/data_merge/data_merge_worker.py new file mode 100644 index 0000000..60383e5 --- /dev/null +++ b/modules/data_merge/data_merge_worker.py @@ -0,0 +1,44 @@ +""" +Merges local drone odometry with LiDAR detections +""" + +from worker import queue_wrapper +from worker import worker_controller + +from modules import detections_and_odometry +from modules import drone_odometry_local +from modules import lidar_detection + + +def data_merge_worker( + detection_input_queue: queue_wrapper.QueueWrapper, + odometry_input_queue: queue_wrapper.QueueWrapper, + output_queue: queue_wrapper.QueueWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + Expects lidar detections to be more frequent than odometry readings. + + detection_input_queue, odometry_input_queue, output_queue are data queues. + controller is how the main process communicates to this worker process. + """ + detections = [] + while not controller.is_exit_requested(): + controller.check_pause() + + detection: lidar_detection.LidarDetection = detection_input_queue.queue.get_nowait() + detections.append(detection) + + odometry: drone_odometry_local.DroneOdometryLocal = odometry_input_queue.queue.get_nowait() + + if odometry is not None: + result, merged = detections_and_odometry.DetectionAndOdometry.create( + detections, odometry + ) + + if not result: + continue + + detections = [] + output_queue.queue.put(merged) diff --git a/modules/detection_and_odometry.py b/modules/detections_and_odometry.py similarity index 73% rename from modules/detection_and_odometry.py rename to modules/detections_and_odometry.py index ee7a88a..1ef2144 100644 --- a/modules/detection_and_odometry.py +++ b/modules/detections_and_odometry.py @@ -6,7 +6,7 @@ from . import lidar_detection -class DetectionAndOdometry: +class DetectionsAndOdometry: """ Contains LiDAR readings and current local odometry. """ @@ -18,7 +18,7 @@ def create( cls, detections: "list[lidar_detection.LidarDetection]", local_odometry: drone_odometry_local.DroneOdometryLocal, - ) -> "tuple[bool, DetectionAndOdometry | None]": + ) -> "tuple[bool, DetectionsAndOdometry | None]": """ Combines lidar readings with local odometry """ @@ -28,7 +28,7 @@ def create( if local_odometry is None: return False, None - return True, DetectionAndOdometry(cls.__create_key, detections, local_odometry) + return True, DetectionsAndOdometry(cls.__create_key, detections, local_odometry) def __init__( self, @@ -39,7 +39,7 @@ def __init__( """ Private constructor, use create() method. """ - assert create_key is DetectionAndOdometry.__create_key, "Use create() method" + assert create_key is DetectionsAndOdometry.__create_key, "Use create() method" self.detections = detections self.odometry = local_odometry @@ -48,4 +48,5 @@ def __str__(self) -> str: """ String representation. """ - return f"{self.__class__.__name__}, Detections ({len(self.detections)}): {self.detections}, str{self.odometry}" + detections_str = ", ".join(str(detection) for detection in self.detections) + return f"{self.__class__.__name__}, Detections ({len(self.detections)}): {detections_str}, str{self.odometry}" From 5fe08bf843f5b2560c336ac9b9416c1e9a789a21 Mon Sep 17 00:00:00 2001 From: Andrew Shum Date: Wed, 12 Jun 2024 14:30:45 -0400 Subject: [PATCH 3/3] removed accidental file add --- modules/data_merge/data_merge_worker.py | 44 ------------------------- 1 file changed, 44 deletions(-) delete mode 100644 modules/data_merge/data_merge_worker.py diff --git a/modules/data_merge/data_merge_worker.py b/modules/data_merge/data_merge_worker.py deleted file mode 100644 index 60383e5..0000000 --- a/modules/data_merge/data_merge_worker.py +++ /dev/null @@ -1,44 +0,0 @@ -""" -Merges local drone odometry with LiDAR detections -""" - -from worker import queue_wrapper -from worker import worker_controller - -from modules import detections_and_odometry -from modules import drone_odometry_local -from modules import lidar_detection - - -def data_merge_worker( - detection_input_queue: queue_wrapper.QueueWrapper, - odometry_input_queue: queue_wrapper.QueueWrapper, - output_queue: queue_wrapper.QueueWrapper, - controller: worker_controller.WorkerController, -) -> None: - """ - Worker process. - Expects lidar detections to be more frequent than odometry readings. - - detection_input_queue, odometry_input_queue, output_queue are data queues. - controller is how the main process communicates to this worker process. - """ - detections = [] - while not controller.is_exit_requested(): - controller.check_pause() - - detection: lidar_detection.LidarDetection = detection_input_queue.queue.get_nowait() - detections.append(detection) - - odometry: drone_odometry_local.DroneOdometryLocal = odometry_input_queue.queue.get_nowait() - - if odometry is not None: - result, merged = detections_and_odometry.DetectionAndOdometry.create( - detections, odometry - ) - - if not result: - continue - - detections = [] - output_queue.queue.put(merged)