diff --git a/config.yaml b/config.yaml index 77fa533..7a8a990 100644 --- a/config.yaml +++ b/config.yaml @@ -1,6 +1,7 @@ # Global constants for main. queue_max_size: 10 +obstacle_avoidance_mode: "simple" flight_interface: address: "/dev/ttyUSB0" @@ -21,7 +22,6 @@ clustering: max_cluster_distance: 0.2 # metres data_merge: - merge_data_type: "OBSTACLES" # "OBSTACLES" or "DETECTIONS" delay: 0.1 # seconds decision: diff --git a/main.py b/main.py index 2379a23..a3f22a2 100644 --- a/main.py +++ b/main.py @@ -11,6 +11,7 @@ from modules.clustering import clustering_worker from modules.data_merge import data_merge_worker from modules.decision import decision_worker +from modules.deflection import deflection_worker from modules.detection import detection_worker from modules.flight_interface import flight_interface_worker from worker import queue_wrapper @@ -43,6 +44,7 @@ def main() -> int: # Local constants # pylint: disable=invalid-name QUEUE_MAX_SIZE = config["queue_max_size"] + OBSTACLE_AVOIDANCE_MODE = config["obstacle_avoidance_mode"] # either "simple" or "normal" FLIGHT_INTERFACE_ADDRESS = config["flight_interface"]["address"] FLIGHT_INTERFACE_TIMEOUT = config["flight_interface"]["timeout"] @@ -61,7 +63,6 @@ def main() -> int: MAX_CLUSTER_DISTANCE = config["clustering"]["max_cluster_distance"] - MERGE_DATA_TYPE = config["data_merge"]["merge_data_type"] DELAY = config["data_merge"]["delay"] OBJECT_PROXIMITY_LIMIT = config["decision"]["object_proximity_limit"] @@ -78,12 +79,9 @@ def main() -> int: flight_interface_to_data_merge_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) detection_to_data_merge_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) - detection_to_clustering_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) - clustering_to_cluster_classification_queue = queue_wrapper.QueueWrapper( - mp_manager, QUEUE_MAX_SIZE - ) - obstacle_to_data_merge_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) - merged_to_decision_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) + merged_to_decision_queue = None + merged_to_clustering_queue = None + clustering_to_deflection_queue = None command_to_flight_interface_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) flight_interface_process = mp.Process( @@ -110,54 +108,85 @@ def main() -> int: HIGH_ANGLE, ROTATE_SPEED, detection_to_data_merge_queue, - detection_to_clustering_queue, - controller, - ), - ) - - clustering_process = mp.Process( - target=clustering_worker.clustering_worker, - args=( - MAX_CLUSTER_DISTANCE, - detection_to_clustering_queue, - clustering_to_cluster_classification_queue, controller, ), ) - # cluster_classification_process will go here. - - data_merge_process = mp.Process( - target=data_merge_worker.data_merge_worker, - args=( - MERGE_DATA_TYPE, - DELAY, - detection_to_data_merge_queue, - obstacle_to_data_merge_queue, - flight_interface_to_data_merge_queue, - merged_to_decision_queue, - controller, - ), - ) - - decision_process = mp.Process( - target=decision_worker.decision_worker, - args=( - OBJECT_PROXIMITY_LIMIT, - MAX_HISTORY, - COMMAND_TIMEOUT, - merged_to_decision_queue, - command_to_flight_interface_queue, - controller, - ), - ) + data_merge_process = None + clustering_process = None + deflection_process = None + decision_process = None + + if OBSTACLE_AVOIDANCE_MODE == "simple": + merged_to_decision_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) + + data_merge_process = mp.Process( + target=data_merge_worker.data_merge_worker, + args=( + DELAY, + detection_to_data_merge_queue, + flight_interface_to_data_merge_queue, + merged_to_decision_queue, + controller, + ), + ) + + decision_process = mp.Process( + target=decision_worker.decision_worker, + args=( + OBJECT_PROXIMITY_LIMIT, + MAX_HISTORY, + COMMAND_TIMEOUT, + merged_to_decision_queue, + command_to_flight_interface_queue, + controller, + ), + ) + + elif OBSTACLE_AVOIDANCE_MODE == "normal": + merged_to_clustering_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) + clustering_to_deflection_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE) + + data_merge_process = mp.Process( + target=data_merge_worker.data_merge_worker, + args=( + DELAY, + detection_to_data_merge_queue, + flight_interface_to_data_merge_queue, + merged_to_clustering_queue, + controller, + ), + ) + + clustering_process = mp.Process( + target=clustering_worker.clustering_worker, + args=( + MAX_CLUSTER_DISTANCE, + merged_to_clustering_queue, + clustering_to_deflection_queue, + controller, + ), + ) + + deflection_process = mp.Process( + target=deflection_worker.deflection_worker, + args=( + clustering_to_deflection_queue, + command_to_flight_interface_queue, + controller, + ), + ) # Run flight_interface_process.start() detection_process.start() - clustering_process.start() + if clustering_process is not None: + clustering_process.start() data_merge_process.start() - decision_process.start() + if decision_process is not None: + decision_process.start() + if deflection_process is not None: + deflection_process.start() while True: try: @@ -170,16 +199,20 @@ def main() -> int: # Teardown flight_interface_to_data_merge_queue.fill_and_drain_queue() detection_to_data_merge_queue.fill_and_drain_queue() - detection_to_clustering_queue.fill_and_drain_queue() - clustering_to_cluster_classification_queue.fill_and_drain_queue() merged_to_decision_queue.fill_and_drain_queue() + merged_to_clustering_queue.fill_and_drain_queue() + clustering_to_deflection_queue.fill_and_drain_queue() command_to_flight_interface_queue.fill_and_drain_queue() flight_interface_process.join() detection_process.join() - clustering_process.join() + if clustering_process is not None: + clustering_process.join() data_merge_process.join() - decision_process.join() + if decision_process is not None: + decision_process.join() + if deflection_process is not None: + deflection_process.join() return 0 diff --git a/modules/deflection/__init__.py b/modules/deflection/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/modules/deflection/deflection.py b/modules/deflection/deflection.py new file mode 100644 index 0000000..ef2edc7 --- /dev/null +++ b/modules/deflection/deflection.py @@ -0,0 +1,20 @@ +""" +Deflection. +""" + + +class Deflection: + """ + Deflection. + """ + + def __init__(self) -> None: + """ + Initialization. + """ + + def run(self) -> "tuple[False, None]": + """ + Run. + """ + return False, None diff --git a/modules/deflection/deflection_worker.py b/modules/deflection/deflection_worker.py new file mode 100644 index 0000000..c136517 --- /dev/null +++ b/modules/deflection/deflection_worker.py @@ -0,0 +1,36 @@ +""" +Gets obstacles and odometry and outputs a decision. +""" + +from modules import obstacles_and_odometry +from worker import queue_wrapper +from worker import worker_controller +from . import deflection + + +def deflection_worker( + cluster_in_queue: queue_wrapper.QueueWrapper, + command_out_queue: queue_wrapper.QueueWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + + cluster_in_queue, command_out_queue are data queues. + controller is how the main process communicates to this worker process. + """ + + deflecter = deflection.Deflection() + + while not controller.is_exit_requested(): + controller.check_pause() + + cluster_data: obstacles_and_odometry.ObstaclesAndOdometry = cluster_in_queue.queue.get() + if cluster_data is None: + break + + result, value = deflecter.run(cluster_data) + if not result: + continue + + command_out_queue.queue.put(value)