diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 00000000..69226dc0 Binary files /dev/null and b/.DS_Store differ diff --git a/bootcamp_main.py b/bootcamp_main.py index aa64faa6..42118ecf 100644 --- a/bootcamp_main.py +++ b/bootcamp_main.py @@ -5,7 +5,6 @@ """ import multiprocessing as mp -import queue import time from pymavlink import mavutil @@ -30,10 +29,24 @@ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= # Set queue max sizes (<= 0 for infinity) +TELEM_TO_COMMAND_QUEUE_MAX = 32 +HEARTBEAT_RECV_TO_MAIN_QUEUE_MAX = 64 +COMMAND_TO_MAIN_QUEUE_MAX = 64 # Set worker counts +HEARTBEAT_SENDER_COUNT = 1 +HEARTBEAT_RECEIVER_COUNT = 1 +TELEMETRY_COUNT = 1 +COMMAND_COUNT = 1 # Any other constants +HEARTBEAT_PERIOD_S = 1.0 +DISCONNECT_THRESHOLD = 5 +TELEMETRY_PERIOD_S = 1.0 +Z_SPEED_M_S = 1.0 +ANGLE_TOLERANCE_DEG = 5.0 +HEIGHT_TOLERANCE_M = 0.5 +TARGET_POSITION = command.Position(10, 20, 30) # ================================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -74,43 +87,147 @@ def main() -> int: # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============================================================================================= # Create a worker controller + controller = worker_controller.WorkerController() # Create a multiprocess manager for synchronized queues + mp_manager = mp.Manager() # Create queues + telem_to_command_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, TELEM_TO_COMMAND_QUEUE_MAX + ) + hb_recv_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, HEARTBEAT_RECV_TO_MAIN_QUEUE_MAX + ) + command_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, COMMAND_TO_MAIN_QUEUE_MAX + ) # Create worker properties for each worker type (what inputs it takes, how many workers) # Heartbeat sender + hb_sender_result, hb_sender_props = worker_manager.WorkerProperties.create( + count=HEARTBEAT_SENDER_COUNT, + target=heartbeat_sender_worker.heartbeat_sender_worker, + work_arguments=(connection, HEARTBEAT_PERIOD_S), + input_queues=[], + output_queues=[], + controller=controller, + local_logger=main_logger, + ) + if not hb_sender_result: + return -1 # Heartbeat receiver + hb_recv_result, hb_recv_props = worker_manager.WorkerProperties.create( + count=HEARTBEAT_RECEIVER_COUNT, + target=heartbeat_receiver_worker.heartbeat_receiver_worker, + work_arguments=( + connection, + HEARTBEAT_PERIOD_S, + DISCONNECT_THRESHOLD, + ), + input_queues=[], + output_queues=[hb_recv_to_main_queue], + controller=controller, + local_logger=main_logger, + ) + if not hb_recv_result: + return -1 # Telemetry + telemetry_result, telemetry_props = worker_manager.WorkerProperties.create( + count=TELEMETRY_COUNT, + target=telemetry_worker.telemetry_worker, + work_arguments=(connection, TELEMETRY_PERIOD_S), + input_queues=[], + output_queues=[telem_to_command_queue], + controller=controller, + local_logger=main_logger, + ) + if not telemetry_result: + return -1 # Command + command_result, command_props = worker_manager.WorkerProperties.create( + count=COMMAND_COUNT, + target=command_worker.command_worker, + work_arguments=( + connection, + TARGET_POSITION, + TELEMETRY_PERIOD_S, + Z_SPEED_M_S, + ANGLE_TOLERANCE_DEG, + HEIGHT_TOLERANCE_M, + ), + input_queues=[telem_to_command_queue], + output_queues=[command_to_main_queue], + controller=controller, + local_logger=main_logger, + ) + if not command_result: + return -1 # Create the workers (processes) and obtain their managers + worker_managers: list[worker_manager.WorkerManager] = [] + for props in (hb_sender_props, hb_recv_props, telemetry_props, command_props): + # Get Pylance to stop complaining + assert props is not None + ok, mgr = worker_manager.WorkerManager.create( + worker_properties=props, local_logger=main_logger + ) + if not ok: + return -1 + assert mgr is not None + worker_managers.append(mgr) # Start worker processes + for mgr in worker_managers: + mgr.start_workers() main_logger.info("Started") # Main's work: read from all queues that output to main, and log any commands that we make # Continue running for 100 seconds or until the drone disconnects + end_time = time.time() + 100 + current_state = "Unknown" + while time.time() < end_time: + try: + # Heartbeat state + state = hb_recv_to_main_queue.queue.get(timeout=HEARTBEAT_PERIOD_S * 2) + current_state = str(state) + main_logger.info(f"Heartbeat state: {current_state}") + if current_state == "Disconnected": + break + # Drain any command outputs without blocking + while True: + cmd_out = command_to_main_queue.queue.get_nowait() + if cmd_out is None: + continue + main_logger.info(f"Command: {cmd_out}") + except: # pylint: disable=bare-except + pass # Stop the processes + controller.request_exit() main_logger.info("Requested exit") # Fill and drain queues from END TO START + telem_to_command_queue.fill_and_drain_queue() + hb_recv_to_main_queue.fill_and_drain_queue() + command_to_main_queue.fill_and_drain_queue() main_logger.info("Queues cleared") # Clean up worker processes + for mgr in worker_managers: + mgr.join_workers() main_logger.info("Stopped") # We can reset controller in case we want to reuse it # Alternatively, create a new WorkerController instance + controller.clear_exit() # ============================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/logs/command/command_drone_48796.log b/logs/command/command_drone_48796.log new file mode 100755 index 00000000..8a3e8218 --- /dev/null +++ b/logs/command/command_drone_48796.log @@ -0,0 +1,28 @@ +23:51:57: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 45] Logger initialized +23:51:57: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:51:58: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:01: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:01: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:02: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:02: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:03: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:03: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:04: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:04: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:05: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:05: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:06: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:06: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:07: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:07: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:08: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:08: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:09: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:09: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:10: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:10: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:11: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:11: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:12: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:12: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 76] Received a valid command +23:52:16: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/command_drone.py | main | 83] Passed! diff --git a/logs/command/command_worker_48793.log b/logs/command/command_worker_48793.log new file mode 100755 index 00000000..6e624561 --- /dev/null +++ b/logs/command/command_worker_48793.log @@ -0,0 +1,58 @@ +23:51:57: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command_worker.py | command_worker | 52] Logger initialized +23:51:57: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 4.000) +23:51:57: [INFO] CHANGE ALTITUDE: 1.0 +23:51:58: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 1.000) +23:51:58: [INFO] CHANGE ALTITUDE: -1.0 +23:51:58: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 0.667) +23:51:59: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 0.500) +23:51:59: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 0.400) +23:52:00: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 0.333) +23:52:00: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 0.286) +23:52:01: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 2.500, 0.250) +23:52:01: [INFO] CHANGE YAW: 63.43494882292201 +23:52:01: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 4.444, 0.222) +23:52:01: [INFO] CHANGE YAW: 90.0 +23:52:02: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 6.000, 0.200) +23:52:02: [INFO] CHANGE YAW: 116.56505117707799 +23:52:02: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (1.818, 5.455, 0.182) +23:52:02: [INFO] CHANGE YAW: 45.0 +23:52:03: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (3.333, 5.000, 0.167) +23:52:03: [INFO] CHANGE YAW: 63.43494882292201 +23:52:03: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (4.615, 4.615, 0.154) +23:52:03: [INFO] CHANGE YAW: 90.0 +23:52:04: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (4.286, 2.857, 0.143) +23:52:04: [INFO] CHANGE YAW: 26.565051177077994 +23:52:04: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (4.000, 1.333, 0.133) +23:52:04: [INFO] CHANGE YAW: 45.0 +23:52:05: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (3.750, 0.000, 0.125) +23:52:05: [INFO] CHANGE YAW: 90.0 +23:52:05: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (2.353, 0.000, 0.118) +23:52:05: [INFO] CHANGE YAW: 45.0 +23:52:06: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (1.111, 0.000, 0.111) +23:52:06: [INFO] CHANGE YAW: 90.0 +23:52:06: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 0.105) +23:52:06: [INFO] CHANGE YAW: 135.0 +23:52:07: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 1.000, 0.100) +23:52:07: [INFO] CHANGE YAW: -26.565051177077994 +23:52:07: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 1.905, 0.095) +23:52:07: [INFO] CHANGE YAW: -45.0 +23:52:08: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 2.727, 0.091) +23:52:08: [INFO] CHANGE YAW: -90.0 +23:52:08: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.870, 2.609, 0.087) +23:52:08: [INFO] CHANGE YAW: -45.0 +23:52:09: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (1.667, 2.500, 0.083) +23:52:09: [INFO] CHANGE YAW: -90.0 +23:52:09: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (2.400, 2.400, 0.080) +23:52:09: [INFO] CHANGE YAW: -135.0 +23:52:10: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (2.308, 1.538, 0.077) +23:52:10: [INFO] CHANGE YAW: -63.43494882292201 +23:52:10: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (2.222, 0.741, 0.074) +23:52:10: [INFO] CHANGE YAW: -90.0 +23:52:11: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (2.143, 0.000, 0.071) +23:52:11: [INFO] CHANGE YAW: -116.56505117707799 +23:52:11: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (1.379, 0.000, 0.069) +23:52:11: [INFO] CHANGE YAW: -45.0 +23:52:12: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.667, 0.000, 0.067) +23:52:12: [INFO] CHANGE YAW: -63.43494882292201 +23:52:12: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/command/command.py | run | 100] Average velocity: (0.000, 0.000, 0.065) +23:52:12: [INFO] CHANGE YAW: -90.0 diff --git a/logs/command/main.log b/logs/command/main.log new file mode 100755 index 00000000..9181466e --- /dev/null +++ b/logs/command/main.log @@ -0,0 +1,4 @@ +23:51:56: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/common/modules/logger/logger_main_setup.py | setup_main_logger | 62] main logger initialized +23:51:57: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_command.py | main | 144] Connected! +23:51:57: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_command.py | read_queue | 81] CHANGE ALTITUDE: 1.0 +23:51:58: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_command.py | read_queue | 81] CHANGE ALTITUDE: -1.0 diff --git a/logs/heartbeat_receiver/heartbeat_receiver_drone_48745.log b/logs/heartbeat_receiver/heartbeat_receiver_drone_48745.log new file mode 100755 index 00000000..72607434 --- /dev/null +++ b/logs/heartbeat_receiver/heartbeat_receiver_drone_48745.log @@ -0,0 +1,13 @@ +23:51:18: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | main | 42] Logger initialized +23:51:18: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:19: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:20: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:21: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:22: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:31: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:32: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:33: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:34: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:35: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:37: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat +23:51:38: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_receiver_drone.py | main | 79] Passesd! diff --git a/logs/heartbeat_receiver/heartbeat_receiver_worker_48742.log b/logs/heartbeat_receiver/heartbeat_receiver_worker_48742.log new file mode 100755 index 00000000..e48e849d --- /dev/null +++ b/logs/heartbeat_receiver/heartbeat_receiver_worker_48742.log @@ -0,0 +1,46 @@ +23:51:18: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver_worker.py | heartbeat_receiver_worker | 46] Logger initialized +23:51:18: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:19: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:20: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:20: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:20: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:21: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:21: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:21: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:22: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:22: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:22: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:23: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:23: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:24: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:24: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:25: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:25: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:26: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:26: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:27: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:27: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Disconnected +23:51:28: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:28: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Disconnected +23:51:29: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:29: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Disconnected +23:51:30: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:30: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Disconnected +23:51:31: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:32: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:32: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:32: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:33: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:33: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:33: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:34: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:35: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:35: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:35: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:36: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:36: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:37: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:38: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:38: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected +23:51:39: [WARNING] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 73] Missed heartbeat +23:51:39: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_receiver.py | run | 78] State: Connected diff --git a/logs/heartbeat_receiver/main.log b/logs/heartbeat_receiver/main.log new file mode 100755 index 00000000..410dd952 --- /dev/null +++ b/logs/heartbeat_receiver/main.log @@ -0,0 +1,30 @@ +23:51:17: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/common/modules/logger/logger_main_setup.py | setup_main_logger | 62] main logger initialized +23:51:18: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | main | 127] Connected! +23:51:18: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:19: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:20: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:20: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:21: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:21: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:22: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:22: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:23: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:24: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:25: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:26: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:27: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Disconnected +23:51:28: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Disconnected +23:51:29: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Disconnected +23:51:30: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Disconnected +23:51:31: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:32: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:32: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:33: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:33: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:34: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:35: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:35: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:36: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:37: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:38: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected +23:51:39: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_receiver.py | read_queue | 77] Connected diff --git a/logs/heartbeat_sender/heartbeat_sender_drone_48729.log b/logs/heartbeat_sender/heartbeat_sender_drone_48729.log new file mode 100755 index 00000000..10f8dc02 --- /dev/null +++ b/logs/heartbeat_sender/heartbeat_sender_drone_48729.log @@ -0,0 +1,12 @@ +23:51:05: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 40] Logger initialized +23:51:06: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:07: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:08: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:09: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:10: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:11: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:12: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:13: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:14: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:15: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat! +23:51:16: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/heartbeat_sender_drone.py | main | 80] Passed! diff --git a/logs/heartbeat_sender/heartbeat_sender_worker_48726.log b/logs/heartbeat_sender/heartbeat_sender_worker_48726.log new file mode 100755 index 00000000..120cb82a --- /dev/null +++ b/logs/heartbeat_sender/heartbeat_sender_worker_48726.log @@ -0,0 +1,12 @@ +23:51:05: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/heartbeat/heartbeat_sender_worker.py | heartbeat_sender_worker | 44] Logger initialized +23:51:05: [INFO] Sent heartbeat +23:51:06: [INFO] Sent heartbeat +23:51:07: [INFO] Sent heartbeat +23:51:08: [INFO] Sent heartbeat +23:51:09: [INFO] Sent heartbeat +23:51:10: [INFO] Sent heartbeat +23:51:11: [INFO] Sent heartbeat +23:51:12: [INFO] Sent heartbeat +23:51:13: [INFO] Sent heartbeat +23:51:14: [INFO] Sent heartbeat +23:51:15: [INFO] Sent heartbeat diff --git a/logs/heartbeat_sender/main.log b/logs/heartbeat_sender/main.log new file mode 100755 index 00000000..a3502725 --- /dev/null +++ b/logs/heartbeat_sender/main.log @@ -0,0 +1,2 @@ +23:51:04: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/common/modules/logger/logger_main_setup.py | setup_main_logger | 62] main logger initialized +23:51:05: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_heartbeat_sender.py | main | 100] Connected! diff --git a/logs/telemetry/main.log b/logs/telemetry/main.log new file mode 100755 index 00000000..946b96d2 --- /dev/null +++ b/logs/telemetry/main.log @@ -0,0 +1,152 @@ +23:51:41: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/common/modules/logger/logger_main_setup.py | setup_main_logger | 62] main logger initialized +23:51:42: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | main | 125] Connected! +23:51:42: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 0, + x: 0.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:43: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 500, + x: 0.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.0471975803375244, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:43: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 1000, + x: 1.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 3.1415927410125732, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:44: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 1500, + x: 1.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: -2.094395160675049, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:44: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 2000, + x: 2.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:45: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 2500, + x: 2.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.0471975803375244, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:45: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 3000, + x: 3.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 3.1415927410125732, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:46: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 3500, + x: 3.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: -2.094395160675049, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:46: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 4000, + x: 4.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:47: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/test_telemetry.py | read_queue | 75] { + time_since_boot: 4500, + x: 4.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.0471975803375244, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } diff --git a/logs/telemetry/telemetry_drone_48776.log b/logs/telemetry/telemetry_drone_48776.log new file mode 100755 index 00000000..d4aa33a0 --- /dev/null +++ b/logs/telemetry/telemetry_drone_48776.log @@ -0,0 +1,52 @@ +23:51:42: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | main | 45] Logger initialized +23:51:42: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 0 +23:51:42: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 0 +23:51:42: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 1 +23:51:43: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 1 +23:51:43: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 2 +23:51:43: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 3 +23:51:43: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 2 +23:51:43: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 4 +23:51:44: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 3 +23:51:44: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 5 +23:51:44: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 6 +23:51:44: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 4 +23:51:44: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 7 +23:51:45: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 5 +23:51:45: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 8 +23:51:45: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 9 +23:51:45: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 6 +23:51:45: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 10 +23:51:46: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 7 +23:51:46: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 11 +23:51:46: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 12 +23:51:46: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 8 +23:51:46: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 13 +23:51:47: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 9 +23:51:47: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 14 +23:51:50: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 0 +23:51:50: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 0 +23:51:50: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 1 +23:51:51: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 1 +23:51:51: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 2 +23:51:51: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 2 +23:51:51: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 3 +23:51:51: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 4 +23:51:52: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 3 +23:51:52: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 5 +23:51:52: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 4 +23:51:52: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 6 +23:51:52: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 7 +23:51:53: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 5 +23:51:53: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 8 +23:51:53: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 6 +23:51:53: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 9 +23:51:53: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 10 +23:51:54: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 7 +23:51:54: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 11 +23:51:54: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 8 +23:51:54: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 12 +23:51:54: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 13 +23:51:55: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 9 +23:51:55: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | send_telemetry | 95] Drone: Sent position 14 +23:51:55: [INFO] [/Users/tomalmog/Clubs/WARG/p2/tests/integration/mock_drones/telemetry_drone.py | main | 134] Passed! diff --git a/logs/telemetry/telemetry_worker_48773.log b/logs/telemetry/telemetry_worker_48773.log new file mode 100755 index 00000000..45db0f3e --- /dev/null +++ b/logs/telemetry/telemetry_worker_48773.log @@ -0,0 +1,301 @@ +23:51:42: [INFO] [/Users/tomalmog/Clubs/WARG/p2/modules/telemetry/telemetry_worker.py | telemetry_worker | 45] Logger initialized +23:51:42: [INFO] { + time_since_boot: 0, + x: 0.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:43: [INFO] { + time_since_boot: 500, + x: 0.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.0471975803375244, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:43: [INFO] { + time_since_boot: 1000, + x: 1.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 3.1415927410125732, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:44: [INFO] { + time_since_boot: 1500, + x: 1.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: -2.094395160675049, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:44: [INFO] { + time_since_boot: 2000, + x: 2.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:45: [INFO] { + time_since_boot: 2500, + x: 2.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.0471975803375244, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:45: [INFO] { + time_since_boot: 3000, + x: 3.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 3.1415927410125732, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:46: [INFO] { + time_since_boot: 3500, + x: 3.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: -2.094395160675049, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:46: [INFO] { + time_since_boot: 4000, + x: 4.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:47: [INFO] { + time_since_boot: 4500, + x: 4.5, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.0471975803375244, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:50: [INFO] { + time_since_boot: 0, + x: 0.0, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:51: [INFO] { + time_since_boot: 500, + x: 0.3333333432674408, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.5707963705062866, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:51: [INFO] { + time_since_boot: 1000, + x: 0.6666666865348816, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 3.1415927410125732, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:52: [INFO] { + time_since_boot: 1500, + x: 1.3333333730697632, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: -1.5707963705062866, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:52: [INFO] { + time_since_boot: 2000, + x: 1.6666666269302368, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:53: [INFO] { + time_since_boot: 2500, + x: 2.3333332538604736, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.5707963705062866, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:53: [INFO] { + time_since_boot: 3000, + x: 2.6666667461395264, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 3.1415927410125732, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:54: [INFO] { + time_since_boot: 3500, + x: 3.3333332538604736, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: -1.5707963705062866, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:54: [INFO] { + time_since_boot: 4000, + x: 3.6666667461395264, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } +23:51:55: [INFO] { + time_since_boot: 4500, + x: 4.333333492279053, + y: 0.0, + z: 0.0, + x_velocity: 1.0, + y_velocity: 0.0, + z_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: 1.5707963705062866, + roll_speed: 0.0, + pitch_speed: 0.0, + yaw_speed: 3.1415927410125732 + } diff --git a/modules/command/command.py b/modules/command/command.py index d85a459a..581c6bda 100644 --- a/modules/command/command.py +++ b/modules/command/command.py @@ -37,44 +37,135 @@ def create( cls, connection: mavutil.mavfile, target: Position, - args, # Put your own arguments here + turning_speed_deg_s: float, local_logger: logger.Logger, - ): + ) -> "tuple[bool, Command | None]": """ Falliable create (instantiation) method to create a Command object. """ - pass # Create a Command object + try: + return True, Command( + Command.__private_key, + connection, + target, + turning_speed_deg_s, + local_logger, + ) + except: # pylint: disable=bare-except + local_logger.error("Failed to create Command", True) + return False, None def __init__( self, key: object, connection: mavutil.mavfile, target: Position, - args, # Put your own arguments here + turning_speed_deg_s: float, local_logger: logger.Logger, ) -> None: assert key is Command.__private_key, "Use create() method" # Do any intializiation here + self.__connection = connection + self.__target = target + self.__turning_speed_deg_s = turning_speed_deg_s + self.__logger = local_logger + self.__total_time_s = 0.0 + self.__disp_x = 0.0 + self.__disp_y = 0.0 + self.__disp_z = 0.0 def run( self, - args, # Put your own arguments here - ): + telemetry_data: telemetry.TelemetryData, + telemetry_period_s: float, + z_speed_m_s: float, + angle_tolerance_deg: float, + height_tolerance_m: float, + ) -> "tuple[bool, str | None]": """ Make a decision based on received telemetry data. """ # Log average velocity for this trip so far + try: + # Integrate displacement using average velocities over the time step + self.__disp_x += float(telemetry_data.x_velocity or 0.0) * telemetry_period_s + self.__disp_y += float(telemetry_data.y_velocity or 0.0) * telemetry_period_s + self.__disp_z += float(telemetry_data.z_velocity or 0.0) * telemetry_period_s + self.__total_time_s += telemetry_period_s + if self.__total_time_s > 0: + avg_vx = self.__disp_x / self.__total_time_s + avg_vy = self.__disp_y / self.__total_time_s + avg_vz = self.__disp_z / self.__total_time_s + self.__logger.info(f"Average velocity: ({avg_vx:.3f}, {avg_vy:.3f}, {avg_vz:.3f})") + except Exception as e: # pylint: disable=broad-except + self.__logger.error(f"Error computing average velocity: {e}") # Use COMMAND_LONG (76) message, assume the target_system=1 and target_componenet=0 # The appropriate commands to use are instructed below # Adjust height using the comand MAV_CMD_CONDITION_CHANGE_ALT (113) # String to return to main: "CHANGE_ALTITUDE: {amount you changed it by, delta height in meters}" + if telemetry_data.z is not None: + delta_z = float(self.__target.z - telemetry_data.z) + if abs(delta_z) > height_tolerance_m: + try: + self.__connection.mav.command_long_send( + 1, # target_system + 0, # target_component + mavutil.mavlink.MAV_CMD_CONDITION_CHANGE_ALT, + 0, # confirmation + float(z_speed_m_s), # param1 ascent/descent m/s + 0, + 0, + 0, + 0, + 0, + float(self.__target.z), # param7 target altitude + ) + except Exception as e: # pylint: disable=broad-except + self.__logger.error(f"Failed to send altitude command: {e}", True) + return True, f"CHANGE ALTITUDE: {delta_z}" # Adjust direction (yaw) using MAV_CMD_CONDITION_YAW (115). Must use relative angle to current state # String to return to main: "CHANGING_YAW: {degree you changed it by in range [-180, 180]}" # Positive angle is counter-clockwise as in a right handed system + if ( + telemetry_data.x is not None + and telemetry_data.y is not None + and telemetry_data.yaw is not None + ): + dx = float(self.__target.x - telemetry_data.x) + dy = float(self.__target.y - telemetry_data.y) + desired_yaw = math.atan2(dy, dx) + delta_rad = desired_yaw - float(telemetry_data.yaw) + # Normalize to [-pi, pi] + while delta_rad > math.pi: + delta_rad -= 2 * math.pi + while delta_rad < -math.pi: + delta_rad += 2 * math.pi + delta_deg = math.degrees(delta_rad) + if abs(delta_deg) > angle_tolerance_deg: + direction = -1 if delta_deg >= 0 else 1 + try: + self.__connection.mav.command_long_send( + 1, + 0, + mavutil.mavlink.MAV_CMD_CONDITION_YAW, + 0, + abs(float(delta_deg)), # angle (deg) + float(self.__turning_speed_deg_s), # turning speed (deg/s) + float(direction), # direction + 1, # relative angle + 0, + 0, + 0, + ) + except Exception as e: # pylint: disable=broad-except + self.__logger.error(f"Failed to send yaw command: {e}", True) + return True, f"CHANGE YAW: {delta_deg}" + + return False, None # ================================================================================================= diff --git a/modules/command/command_worker.py b/modules/command/command_worker.py index 8a3b0ff3..d8902365 100644 --- a/modules/command/command_worker.py +++ b/modules/command/command_worker.py @@ -2,6 +2,8 @@ Command worker to make decisions based on Telemetry Data. """ +# pylint: disable=too-many-arguments, too-many-locals + import os import pathlib @@ -19,8 +21,13 @@ def command_worker( connection: mavutil.mavfile, target: command.Position, - args, # Place your own arguments here - # Add other necessary worker arguments here + telemetry_period_s: float, + z_speed_m_s: float, + angle_tolerance_deg: float, + height_tolerance_m: float, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, ) -> None: """ Worker process. @@ -48,8 +55,39 @@ def command_worker( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============================================================================================= # Instantiate class object (command.Command) + ok, instance = command.Command.create( + connection, + target, + 5.0, # default turning speed (deg/s) + local_logger, + ) + if not ok: + local_logger.error("Failed to create Command instance", True) + return + # Get Pylance to stop complaining + assert instance is not None # Main loop: do work. + while not controller.is_exit_requested(): + controller.check_pause() + data = input_queue.queue.get() + if data is None: + continue + try: + success, output = instance.run( + data, + telemetry_period_s, + z_speed_m_s, + angle_tolerance_deg, + height_tolerance_m, + ) + except Exception as e: # pylint: disable=broad-except + local_logger.error(f"Command run failed: {e}", True) + success, output = False, None + if success: + # Log and forward the output + local_logger.info(str(output), None) + output_queue.queue.put(output) # ================================================================================================= diff --git a/modules/heartbeat/heartbeat_receiver.py b/modules/heartbeat/heartbeat_receiver.py index 3c107f77..d9f5a8df 100644 --- a/modules/heartbeat/heartbeat_receiver.py +++ b/modules/heartbeat/heartbeat_receiver.py @@ -21,34 +21,67 @@ class HeartbeatReceiver: def create( cls, connection: mavutil.mavfile, - args, # Put your own arguments here + heartbeat_period_s: float, + disconnect_threshold: int, local_logger: logger.Logger, - ): + ) -> "tuple[bool, HeartbeatReceiver | None]": """ Falliable create (instantiation) method to create a HeartbeatReceiver object. """ - pass # Create a HeartbeatReceiver object + try: + return True, HeartbeatReceiver( + HeartbeatReceiver.__private_key, + connection, + heartbeat_period_s, + disconnect_threshold, + ) + except: # pylint: disable=bare-except + local_logger.error("Failed to create HeartbeatReceiver", True) + return False, None def __init__( self, key: object, connection: mavutil.mavfile, - args, # Put your own arguments here + heartbeat_period_s: float, + disconnect_threshold: int, ) -> None: assert key is HeartbeatReceiver.__private_key, "Use create() method" # Do any intializiation here + self.__connection = connection + self.__period_s = heartbeat_period_s + self.__missed_in_row = 0 + self.__disconnect_threshold = disconnect_threshold def run( self, - args, # Put your own arguments here - ): + local_logger: logger.Logger, + ) -> "tuple[bool, str]": """ Attempt to recieve a heartbeat message. If disconnected for over a threshold number of periods, the connection is considered disconnected. """ - pass + try: + msg = self.__connection.recv_match( + type="HEARTBEAT", blocking=True, timeout=self.__period_s + ) + except Exception as e: # pylint: disable=broad-except + local_logger.error(f"Exception while receiving heartbeat: {e}", True) + msg = None + + if not msg or msg.get_type() != "HEARTBEAT": + self.__missed_in_row += 1 + local_logger.warning("Missed heartbeat", True) + else: + self.__missed_in_row = 0 + + state = ( + "Connected" if self.__missed_in_row < self.__disconnect_threshold else "Disconnected" + ) + local_logger.info(f"State: {state}", True) + return True, state # ================================================================================================= diff --git a/modules/heartbeat/heartbeat_receiver_worker.py b/modules/heartbeat/heartbeat_receiver_worker.py index 0e90e3d9..9641485f 100644 --- a/modules/heartbeat/heartbeat_receiver_worker.py +++ b/modules/heartbeat/heartbeat_receiver_worker.py @@ -18,8 +18,10 @@ # ================================================================================================= def heartbeat_receiver_worker( connection: mavutil.mavfile, - args, # Place your own arguments here - # Add other necessary worker arguments here + heartbeat_period_s: float, + disconnect_threshold: int, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, ) -> None: """ Worker process. @@ -47,8 +49,26 @@ def heartbeat_receiver_worker( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============================================================================================= # Instantiate class object (heartbeat_receiver.HeartbeatReceiver) + ok, instance = heartbeat_receiver.HeartbeatReceiver.create( + connection, + heartbeat_period_s, + disconnect_threshold, + local_logger, + ) + if not ok: + local_logger.error("Failed to create HeartbeatReceiver instance", True) + return + assert instance is not None # Main loop: do work. + while not controller.is_exit_requested(): + controller.check_pause() + try: + _ok, state = instance.run(local_logger) + except Exception as e: # pylint: disable=broad-except + local_logger.error(f"Heartbeat receive failed: {e}", True) + state = "Disconnected" + output_queue.queue.put(state) # ================================================================================================= diff --git a/modules/heartbeat/heartbeat_sender.py b/modules/heartbeat/heartbeat_sender.py index 627cdde1..c39aba73 100644 --- a/modules/heartbeat/heartbeat_sender.py +++ b/modules/heartbeat/heartbeat_sender.py @@ -19,31 +19,47 @@ class HeartbeatSender: def create( cls, connection: mavutil.mavfile, - args, # Put your own arguments here - ) -> "tuple[True, HeartbeatSender] | tuple[False, None]": + period_s: float, + ) -> "tuple[bool, HeartbeatSender | None]": """ Falliable create (instantiation) method to create a HeartbeatSender object. """ - pass # Create a HeartbeatSender object + try: + return True, HeartbeatSender( + HeartbeatSender.__private_key, + connection, + period_s, + ) + except: # pylint: disable=bare-except + return False, None def __init__( self, key: object, connection: mavutil.mavfile, - args, # Put your own arguments here - ): + period_s: float, + ) -> None: assert key is HeartbeatSender.__private_key, "Use create() method" - # Do any intializiation here + self.__connection = connection + self.__period_s = period_s def run( self, - args, # Put your own arguments here - ): + _unused: None | object = None, + ) -> "tuple[bool, float]": """ Attempt to send a heartbeat message. """ - pass # Send a heartbeat message + # Send heartbeat from GCS once + self.__connection.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, + 0, + 0, + ) + return True, self.__period_s # ================================================================================================= diff --git a/modules/heartbeat/heartbeat_sender_worker.py b/modules/heartbeat/heartbeat_sender_worker.py index ba959083..35ef8db0 100644 --- a/modules/heartbeat/heartbeat_sender_worker.py +++ b/modules/heartbeat/heartbeat_sender_worker.py @@ -18,8 +18,8 @@ # ================================================================================================= def heartbeat_sender_worker( connection: mavutil.mavfile, - args, # Place your own arguments here - # Add other necessary worker arguments here + heartbeat_period_s: float, + controller: worker_controller.WorkerController, ) -> None: """ Worker process. @@ -47,8 +47,21 @@ def heartbeat_sender_worker( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============================================================================================= # Instantiate class object (heartbeat_sender.HeartbeatSender) + ok, instance = heartbeat_sender.HeartbeatSender.create(connection, heartbeat_period_s) + if not ok: + local_logger.error("Failed to create HeartbeatSender instance", True) + return + assert instance is not None # Main loop: do work. + while not controller.is_exit_requested(): + controller.check_pause() + try: + instance.run() + local_logger.info("Sent heartbeat", None) + except Exception as e: # pylint: disable=broad-except + local_logger.error(f"Heartbeat send failed: {e}", True) + time.sleep(heartbeat_period_s) # ================================================================================================= diff --git a/modules/telemetry/telemetry.py b/modules/telemetry/telemetry.py index 0174eeb7..f5ffc0bf 100644 --- a/modules/telemetry/telemetry.py +++ b/modules/telemetry/telemetry.py @@ -76,29 +76,41 @@ class Telemetry: def create( cls, connection: mavutil.mavfile, - args, # Put your own arguments here + timeout_s: float, local_logger: logger.Logger, - ): + ) -> "tuple[bool, Telemetry | None]": """ Falliable create (instantiation) method to create a Telemetry object. """ - pass # Create a Telemetry object + try: + return True, Telemetry( + Telemetry.__private_key, + connection, + timeout_s, + local_logger, + ) + except: # pylint: disable=bare-except + local_logger.error("Failed to create Telemetry", True) + return False, None def __init__( self, key: object, connection: mavutil.mavfile, - args, # Put your own arguments here + timeout_s: float, local_logger: logger.Logger, ) -> None: assert key is Telemetry.__private_key, "Use create() method" # Do any intializiation here + self.__connection = connection + self.__timeout_s = timeout_s + self.__logger = local_logger def run( self, - args, # Put your own arguments here - ): + _unused: None | object = None, + ) -> "tuple[bool, TelemetryData | None]": """ Receive LOCAL_POSITION_NED and ATTITUDE messages from the drone, combining them together to form a single TelemetryData object. @@ -106,7 +118,48 @@ def run( # Read MAVLink message LOCAL_POSITION_NED (32) # Read MAVLink message ATTITUDE (30) # Return the most recent of both, and use the most recent message's timestamp - pass + deadline = time.time() + self.__timeout_s + latest_att = None + latest_pos = None + while time.time() < deadline and (latest_att is None or latest_pos is None): + timeout = max(0.0, deadline - time.time()) + try: + msg = self.__connection.recv_match(blocking=True, timeout=timeout) + except Exception as e: # pylint: disable=broad-except + self.__logger.error(f"Exception while receiving telemetry: {e}", True) + msg = None + if not msg: + break + mtype = msg.get_type() + if mtype == "ATTITUDE": + latest_att = msg + elif mtype == "LOCAL_POSITION_NED": + latest_pos = msg + + if latest_att is None or latest_pos is None: + # Timeout without both messages + return False, None + + # Most recent timestamp across both + time_ms = max(int(latest_att.time_boot_ms), int(latest_pos.time_boot_ms)) + + data = TelemetryData( + time_since_boot=time_ms, + x=float(latest_pos.x), + y=float(latest_pos.y), + z=float(latest_pos.z), + x_velocity=float(latest_pos.vx), + y_velocity=float(latest_pos.vy), + z_velocity=float(latest_pos.vz), + roll=float(latest_att.roll), + pitch=float(latest_att.pitch), + yaw=float(latest_att.yaw), + roll_speed=float(latest_att.rollspeed), + pitch_speed=float(latest_att.pitchspeed), + yaw_speed=float(latest_att.yawspeed), + ) + + return True, data # ================================================================================================= diff --git a/modules/telemetry/telemetry_worker.py b/modules/telemetry/telemetry_worker.py index ed353145..996513ff 100644 --- a/modules/telemetry/telemetry_worker.py +++ b/modules/telemetry/telemetry_worker.py @@ -18,8 +18,9 @@ # ================================================================================================= def telemetry_worker( connection: mavutil.mavfile, - args, # Place your own arguments here - # Add other necessary worker arguments here + timeout_s: float, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, ) -> None: """ Worker process. @@ -47,8 +48,24 @@ def telemetry_worker( # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ============================================================================================= # Instantiate class object (telemetry.Telemetry) + ok, instance = telemetry.Telemetry.create(connection, timeout_s, local_logger) + if not ok: + local_logger.error("Failed to create Telemetry instance", True) + return + assert instance is not None # Main loop: do work. + while not controller.is_exit_requested(): + controller.check_pause() + try: + success, data = instance.run() + except Exception as e: # pylint: disable=broad-except + local_logger.error(f"Telemetry gather failed: {e}", True) + success, data = False, None + if success: + # Log and forward data + local_logger.info(str(data), None) + output_queue.queue.put(data) # ================================================================================================= diff --git a/tests/integration/test_command.py b/tests/integration/test_command.py index 9c1ca3c1..3f99a86b 100644 --- a/tests/integration/test_command.py +++ b/tests/integration/test_command.py @@ -7,6 +7,7 @@ import subprocess import threading import time +import sys from pymavlink import mavutil @@ -35,6 +36,7 @@ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= # Add your own constants here +TELEMETRY_PERIOD_S = TELEMETRY_PERIOD # ================================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -47,38 +49,49 @@ def start_drone() -> None: """ Start the mocked drone. """ - subprocess.run(["python", "-m", MOCK_DRONE_MODULE], shell=True, check=False) + subprocess.run([sys.executable, "-m", MOCK_DRONE_MODULE], check=False) # ================================================================================================= # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= def stop( - args, # Add any necessary arguments + controller: worker_controller.WorkerController, ) -> None: """ Stop the workers. """ - pass # Add logic to stop your worker + controller.request_exit() def read_queue( - args, # Add any necessary arguments + output_queue: queue_proxy_wrapper.QueueProxyWrapper, main_logger: logger.Logger, ) -> None: """ Read and print the output queue. """ - pass # Add logic to read from your worker's output queue and print it using the logger + while True: + try: + item = output_queue.queue.get(timeout=TELEMETRY_PERIOD_S * 2) + except Exception: # pylint: disable=broad-except + break + if item is None: + continue + main_logger.info(str(item)) def put_queue( - args, # Add any necessary arguments + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + items: list[telemetry.TelemetryData], ) -> None: """ Place mocked inputs into the input queue periodically with period TELEMETRY_PERIOD. """ - pass # Add logic to place the mocked inputs into your worker's input queue periodically + for item in items: + input_queue.queue.put(item) + time.sleep(TELEMETRY_PERIOD_S) + input_queue.queue.put(None) # ================================================================================================= @@ -111,7 +124,16 @@ def main() -> int: # Mocked GCS, connect to mocked drone which is listening at CONNECTION_STRING # source_system = 255 (groundside) # source_component = 0 (ground control station) - connection = mavutil.mavlink_connection(CONNECTION_STRING) + # Retry a few times to allow the mock drone to start listening + for _ in range(10): + try: + connection = mavutil.mavlink_connection(CONNECTION_STRING) + break + except Exception: # pylint: disable=broad-except + time.sleep(0.2) + else: + main_logger.error("Failed to create connection to mock drone") + return -1 connection.mav.heartbeat_send( mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, @@ -127,10 +149,14 @@ def main() -> int: # ============================================================================================= # Mock starting a worker, since cannot actually start a new process # Create a worker controller for your worker + controller = worker_controller.WorkerController() # Create a multiprocess manager for synchronized queues + mp_manager = mp.Manager() # Create your queues + input_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, maxsize=128) + main_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, maxsize=128) # Test cases, DO NOT EDIT! path = [ @@ -217,16 +243,24 @@ def main() -> int: ] # Just set a timer to stop the worker after a while, since the worker infinite loops - threading.Timer(TELEMETRY_PERIOD * len(path), stop, (args,)).start() + threading.Timer(TELEMETRY_PERIOD * len(path), stop, (controller,)).start() # Put items into input queue - threading.Thread(target=put_queue, args=(args,)).start() + threading.Thread(target=put_queue, args=(input_queue, path)).start() # Read the main queue (worker outputs) - threading.Thread(target=read_queue, args=(args, main_logger)).start() + threading.Thread(target=read_queue, args=(main_queue, main_logger)).start() command_worker.command_worker( - # Place your own arguments here + connection, + TARGET, + TELEMETRY_PERIOD_S, + Z_SPEED, + ANGLE_TOLERANCE, + HEIGHT_TOLERANCE, + input_queue, + main_queue, + controller, ) # ============================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/tests/integration/test_heartbeat_receiver.py b/tests/integration/test_heartbeat_receiver.py index 23ff4cc5..6c423419 100644 --- a/tests/integration/test_heartbeat_receiver.py +++ b/tests/integration/test_heartbeat_receiver.py @@ -5,6 +5,8 @@ import multiprocessing as mp import subprocess import threading +import time +import sys from pymavlink import mavutil @@ -30,6 +32,7 @@ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= # Add your own constants here +HEARTBEAT_PERIOD_S = HEARTBEAT_PERIOD # ================================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -42,29 +45,36 @@ def start_drone() -> None: """ Start the mocked drone. """ - subprocess.run(["python", "-m", MOCK_DRONE_MODULE], shell=True, check=False) + subprocess.run([sys.executable, "-m", MOCK_DRONE_MODULE], check=False) # ================================================================================================= # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= def stop( - args, # Add any necessary arguments + controller: worker_controller.WorkerController, ) -> None: """ Stop the workers. """ - pass # Add logic to stop your worker + controller.request_exit() def read_queue( - args, # Add any necessary arguments + output_queue: queue_proxy_wrapper.QueueProxyWrapper, main_logger: logger.Logger, ) -> None: """ Read and print the output queue. """ - pass # Add logic to read from your worker's output queue and print it using the logger + while True: + try: + item = output_queue.queue.get(timeout=HEARTBEAT_PERIOD_S * 2) + except Exception: # pylint: disable=broad-except + break + if item is None: + continue + main_logger.info(str(item)) # ================================================================================================= @@ -97,7 +107,16 @@ def main() -> int: # Mocked GCS, connect to mocked drone which is listening at CONNECTION_STRING # source_system = 255 (groundside) # source_component = 0 (ground control station) - connection = mavutil.mavlink_connection(CONNECTION_STRING) + # Retry a few times to allow the mock drone to start listening + for _ in range(10): + try: + connection = mavutil.mavlink_connection(CONNECTION_STRING) + break + except Exception: # pylint: disable=broad-except + time.sleep(0.2) + else: + main_logger.error("Failed to create connection to mock drone") + return -1 connection.mav.heartbeat_send( mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, @@ -113,23 +132,30 @@ def main() -> int: # ============================================================================================= # Mock starting a worker, since cannot actually start a new process # Create a worker controller for your worker + controller = worker_controller.WorkerController() # Create a multiprocess manager for synchronized queues + mp_manager = mp.Manager() # Create your queues + main_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, maxsize=64) # Just set a timer to stop the worker after a while, since the worker infinite loops threading.Timer( HEARTBEAT_PERIOD * (NUM_TRIALS * 2 + DISCONNECT_THRESHOLD + NUM_DISCONNECTS + 2), stop, - (args,), + (controller,), ).start() # Read the main queue (worker outputs) - threading.Thread(target=read_queue, args=(args, main_logger)).start() + threading.Thread(target=read_queue, args=(main_queue, main_logger)).start() heartbeat_receiver_worker.heartbeat_receiver_worker( - # Place your own arguments here + connection, + HEARTBEAT_PERIOD_S, + DISCONNECT_THRESHOLD, + main_queue, + controller, ) # ============================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/tests/integration/test_heartbeat_sender.py b/tests/integration/test_heartbeat_sender.py index d81402c0..1f21cb33 100644 --- a/tests/integration/test_heartbeat_sender.py +++ b/tests/integration/test_heartbeat_sender.py @@ -5,6 +5,8 @@ import multiprocessing as mp import subprocess import threading +import time +import sys from pymavlink import mavutil @@ -26,6 +28,7 @@ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= # Add your own constants here +HEARTBEAT_PERIOD_S = HEARTBEAT_PERIOD # ================================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -38,19 +41,19 @@ def start_drone() -> None: """ Start the mocked drone. """ - subprocess.run(["python", "-m", MOCK_DRONE_MODULE], shell=True, check=False) + subprocess.run([sys.executable, "-m", MOCK_DRONE_MODULE], check=False) # ================================================================================================= # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= def stop( - args, # Add any necessary arguments + controller: worker_controller.WorkerController, ) -> None: """ Stop the workers. """ - pass # Add logic to stop your worker + controller.request_exit() # ================================================================================================= @@ -83,7 +86,16 @@ def main() -> int: # Mocked GCS, connect to mocked drone which is listening at CONNECTION_STRING # source_system = 255 (groundside) # source_component = 0 (ground control station) - connection = mavutil.mavlink_connection(CONNECTION_STRING) + # Retry a few times to allow the mock drone to start listening + for _ in range(10): + try: + connection = mavutil.mavlink_connection(CONNECTION_STRING) + break + except Exception: # pylint: disable=broad-except + time.sleep(0.2) + else: + main_logger.error("Failed to create connection to mock drone") + return -1 # Don't send another heartbeat since the worker will do so main_logger.info("Connected!") # pylint: enable=duplicate-code @@ -93,12 +105,15 @@ def main() -> int: # ============================================================================================= # Mock starting a worker, since cannot actually start a new process # Create a worker controller for your worker + controller = worker_controller.WorkerController() # Just set a timer to stop the worker after a while, since the worker infinite loops - threading.Timer(HEARTBEAT_PERIOD * NUM_TRIALS, stop, (args,)).start() + threading.Timer(HEARTBEAT_PERIOD * NUM_TRIALS, stop, (controller,)).start() heartbeat_sender_worker.heartbeat_sender_worker( - # Place your own arguments here + connection, + HEARTBEAT_PERIOD_S, + controller, ) # ============================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ diff --git a/tests/integration/test_telemetry.py b/tests/integration/test_telemetry.py index 79f9e497..e03553cc 100644 --- a/tests/integration/test_telemetry.py +++ b/tests/integration/test_telemetry.py @@ -5,6 +5,8 @@ import multiprocessing as mp import subprocess import threading +import time +import sys from pymavlink import mavutil @@ -28,6 +30,7 @@ # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= # Add your own constants here +TELEMETRY_TIMEOUT_S = TELEMETRY_PERIOD # ================================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑ @@ -40,29 +43,36 @@ def start_drone() -> None: """ Start the mocked drone. """ - subprocess.run(["python", "-m", MOCK_DRONE_MODULE], shell=True, check=False) + subprocess.run([sys.executable, "-m", MOCK_DRONE_MODULE], check=False) # ================================================================================================= # ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓ # ================================================================================================= def stop( - args, # Add any necessary arguments + controller: worker_controller.WorkerController, ) -> None: """ Stop the workers. """ - pass # Add logic to stop your worker + controller.request_exit() def read_queue( - args, # Add any necessary arguments + output_queue: queue_proxy_wrapper.QueueProxyWrapper, main_logger: logger.Logger, ) -> None: """ Read and print the output queue. """ - pass # Add logic to read from your worker's output queue and print it using the logger + while True: + try: + item = output_queue.queue.get(timeout=TELEMETRY_PERIOD * 2) + except Exception: # pylint: disable=broad-except + break + if item is None: + continue + main_logger.info(str(item)) # ================================================================================================= @@ -95,7 +105,16 @@ def main() -> int: # Mocked GCS, connect to mocked drone which is listening at CONNECTION_STRING # source_system = 255 (groundside) # source_component = 0 (ground control station) - connection = mavutil.mavlink_connection(CONNECTION_STRING) + # Retry a few times to allow the mock drone to start listening + for _ in range(10): + try: + connection = mavutil.mavlink_connection(CONNECTION_STRING) + break + except Exception: # pylint: disable=broad-except + time.sleep(0.2) + else: + main_logger.error("Failed to create connection to mock drone") + return -1 connection.mav.heartbeat_send( mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, @@ -111,19 +130,25 @@ def main() -> int: # ============================================================================================= # Mock starting a worker, since cannot actually start a new process # Create a worker controller for your worker + controller = worker_controller.WorkerController() # Create a multiprocess manager for synchronized queues + mp_manager = mp.Manager() # Create your queues + main_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, maxsize=128) # Just set a timer to stop the worker after a while, since the worker infinite loops - threading.Timer(TELEMETRY_PERIOD * NUM_TRIALS * 2 + NUM_FAILS, stop, (args,)).start() + threading.Timer(TELEMETRY_PERIOD * NUM_TRIALS * 2 + NUM_FAILS, stop, (controller,)).start() # Read the main queue (worker outputs) - threading.Thread(target=read_queue, args=(args, main_logger)).start() + threading.Thread(target=read_queue, args=(main_queue, main_logger)).start() telemetry_worker.telemetry_worker( - # Put your own arguments here + connection, + TELEMETRY_TIMEOUT_S, + main_queue, + controller, ) # ============================================================================================= # ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑