From 01690a39e2455ab6b0a75fa06f43de9ed1d52376 Mon Sep 17 00:00:00 2001 From: Xierumeng Date: Sat, 23 Mar 2024 22:01:06 -0700 Subject: [PATCH] Python formatting tools (#37) * Python formatting tools * Format new files * Gitignore * Pylint submodule ignore * Github workflow comment * Remove duplicate by adding command generation * Tests * PR fixes --- .github/workflows/run-tests.yml | 9 +- .gitignore | 2 +- connection_check.py | 22 +- modules/add_takeoff_and_landing_command.py | 47 +-- modules/add_takeoff_and_rtl_command.py | 47 +-- modules/check_stop_condition.py | 36 +-- modules/diversion_qr_to_waypoint_list.py | 10 +- modules/generate_command.py | 196 ++++++++++++ .../load_waypoint_name_to_coordinates_map.py | 12 +- modules/qr_input.py | 2 +- modules/qr_to_waypoint_names.py | 3 +- modules/upload_commands.py | 8 +- modules/waypoint_names_to_coordinates.py | 8 +- modules/waypoint_tracking.py | 7 +- modules/waypoints_dict_to_list.py | 5 +- modules/waypoints_to_commands.py | 34 +- modules/waypoints_to_spline_commands.py | 33 +- path_2023.py | 44 +-- path_2024.py | 50 +-- path_2024_task_1.py | 45 +-- pyproject.toml | 111 +++++++ pytest.ini | 2 - requirements.txt | 5 + setup.cfg | 23 ++ tests/integration/__init__.py | 0 .../test_check_stop_condition.py | 50 ++- tests/{ => integration}/test_qr_input.py | 14 +- .../{ => integration}/test_upload_commands.py | 302 ++++++++---------- tests/test_add_takeoff_and_landing_command.py | 115 ------- tests/test_add_takeoff_and_rtl_command.py | 115 ------- tests/test_qr_to_waypoint_names.py | 159 --------- tests/unit/__init__.py | 0 .../test_add_takeoff_and_landing_command.py | 106 ++++++ .../unit/test_add_takeoff_and_rtl_command.py | 103 ++++++ .../test_diversion_qr_to_waypoint_list.py | 231 +++++++------- ...t_load_waypoint_name_to_coordinates_map.py | 37 ++- tests/unit/test_qr_to_waypoint_names.py | 164 ++++++++++ .../{ => unit}/test_waypoints_dict_to_list.py | 16 +- .../test_waypoints_names_to_coordinates.py | 18 +- .../{ => unit}/test_waypoints_to_commands.py | 13 +- .../test_waypoints_to_spline_commands.py | 13 +- 41 files changed, 1243 insertions(+), 974 deletions(-) create mode 100644 modules/generate_command.py create mode 100644 pyproject.toml delete mode 100644 pytest.ini create mode 100644 setup.cfg create mode 100644 tests/integration/__init__.py rename tests/{ => integration}/test_check_stop_condition.py (66%) rename tests/{ => integration}/test_qr_input.py (57%) rename tests/{ => integration}/test_upload_commands.py (50%) delete mode 100644 tests/test_add_takeoff_and_landing_command.py delete mode 100644 tests/test_add_takeoff_and_rtl_command.py delete mode 100644 tests/test_qr_to_waypoint_names.py create mode 100644 tests/unit/__init__.py create mode 100644 tests/unit/test_add_takeoff_and_landing_command.py create mode 100644 tests/unit/test_add_takeoff_and_rtl_command.py rename tests/{ => unit}/test_diversion_qr_to_waypoint_list.py (60%) rename tests/{ => unit}/test_load_waypoint_name_to_coordinates_map.py (57%) create mode 100644 tests/unit/test_qr_to_waypoint_names.py rename tests/{ => unit}/test_waypoints_dict_to_list.py (68%) rename tests/{ => unit}/test_waypoints_names_to_coordinates.py (82%) rename tests/{ => unit}/test_waypoints_to_commands.py (78%) rename tests/{ => unit}/test_waypoints_to_spline_commands.py (76%) diff --git a/.github/workflows/run-tests.yml b/.github/workflows/run-tests.yml index 76cd6e4..f071a01 100644 --- a/.github/workflows/run-tests.yml +++ b/.github/workflows/run-tests.yml @@ -30,7 +30,7 @@ jobs: git submodule update --init --recursive --remote pip install -r ./modules/common/requirements.txt - # Install pathing dependencies + # Install project dependencies - name: Install project dependencies run: | python -m pip install --upgrade pip @@ -40,6 +40,13 @@ jobs: - name: Install zbar library run: sudo apt-get install libzbar0 + # Run linters and formatters + - name: Linters and formatters + run: | + black --check . + flake8 . + pylint . + # Run tests with PyTest - name: Run tests run: pytest -vv diff --git a/.gitignore b/.gitignore index b9b62af..5e12b06 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,4 @@ __pycache__/ venv/ # Logging -*log* +logs/ diff --git a/connection_check.py b/connection_check.py index acbaaaa..b36d44b 100644 --- a/connection_check.py +++ b/connection_check.py @@ -1,6 +1,7 @@ """ For testing MAVLink connection with DroneKit-Python. """ + import time import dronekit @@ -99,19 +100,30 @@ def read_data(drone: dronekit.Vehicle) -> bool: print("Next waypoint index: " + str(command_sequence.next)) -if __name__ == "__main__": +def main() -> int: + """ + Main function. + """ # TODO: Fails when wait_ready=True, debugging why this is # wait_ready=False is dangerous - drone = dronekit.connect(CONNECTION_ADDRESS, wait_ready=False) + dronekit_vehicle = dronekit.connect(CONNECTION_ADDRESS, wait_ready=False) if WRITE_TEST: - write_test_mission(drone) + write_test_mission(dronekit_vehicle) if READ_TEST: for _ in range(0, 40): - read_data(drone) + read_data(dronekit_vehicle) time.sleep(0.5) - drone.close() + dronekit_vehicle.close() + + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") print("Done!") diff --git a/modules/add_takeoff_and_landing_command.py b/modules/add_takeoff_and_landing_command.py index b3f5097..e5b82ca 100644 --- a/modules/add_takeoff_and_landing_command.py +++ b/modules/add_takeoff_and_landing_command.py @@ -4,15 +4,12 @@ import dronekit +from . import generate_command -MAVLINK_TAKEOFF_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT -MAVLINK_LANDING_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL -MAVLINK_TAKEOFF_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_TAKEOFF -MAVLINK_LANDING_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_LAND - -def add_takeoff_and_landing_command(commands: "list[dronekit.Command]", - altitude: float) -> "tuple[bool, list[dronekit.Command] | None]": +def add_takeoff_and_landing_command( + commands: "list[dronekit.Command]", altitude: float +) -> "tuple[bool, list[dronekit.Command] | None]": """ Prepends a takeoff command and appends a landing command to a list of dronekit commands. @@ -25,47 +22,17 @@ def add_takeoff_and_landing_command(commands: "list[dronekit.Command]", Returns ------- - tuple[bool, list[dronekit.Command] | None]: + tuple[bool, list[dronekit.Command] | None]: (False, None) if empty commands list, (True, dronekit commands with takeoff and land commands that can be sent to the drone) otherwise. """ if len(commands) == 0: return False, None - takeoff_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_TAKEOFF_FRAME, - MAVLINK_TAKEOFF_COMMAND, - 0, - 0, - 0, # param1 - 0, - 0, - 0, - 0, - 0, - altitude, - ) + takeoff_command = generate_command.takeoff(altitude) commands.insert(0, takeoff_command) - landing_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_LANDING_FRAME, - MAVLINK_LANDING_COMMAND, - 0, - 0, - 0, # param1 - 0, - 0, - 0, - 0, - 0, - 0, - ) + landing_command = generate_command.landing() commands.append(landing_command) return True, commands diff --git a/modules/add_takeoff_and_rtl_command.py b/modules/add_takeoff_and_rtl_command.py index f761229..1928df8 100644 --- a/modules/add_takeoff_and_rtl_command.py +++ b/modules/add_takeoff_and_rtl_command.py @@ -4,15 +4,12 @@ import dronekit +from . import generate_command -MAVLINK_TAKEOFF_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT -MAVLINK_RTL_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL -MAVLINK_TAKEOFF_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_TAKEOFF -MAVLINK_RTL_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH - -def add_takeoff_and_rtl_command(commands: "list[dronekit.Command]", - altitude: float) -> "tuple[bool, list[dronekit.Command] | None]": +def add_takeoff_and_rtl_command( + commands: "list[dronekit.Command]", altitude: float +) -> "tuple[bool, list[dronekit.Command] | None]": """ Prepends a takeoff command and appends a RTL command to a list of dronekit commands. @@ -25,47 +22,17 @@ def add_takeoff_and_rtl_command(commands: "list[dronekit.Command]", Returns ------- - tuple[bool, list[dronekit.Command] | None]: + tuple[bool, list[dronekit.Command] | None]: (False, None) if empty commands list, (True, dronekit commands with takeoff and land commands that can be sent to the drone) otherwise. """ if len(commands) == 0: return False, None - takeoff_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_TAKEOFF_FRAME, - MAVLINK_TAKEOFF_COMMAND, - 0, - 0, - 0, # param1 - 0, - 0, - 0, - 0, - 0, - altitude, - ) + takeoff_command = generate_command.takeoff(altitude) commands.insert(0, takeoff_command) - rtl_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_RTL_FRAME, - MAVLINK_RTL_COMMAND, - 0, - 0, - 0, # param1 - 0, - 0, - 0, - 0, - 0, - 0, - ) + rtl_command = generate_command.return_to_launch() commands.append(rtl_command) return True, commands diff --git a/modules/check_stop_condition.py b/modules/check_stop_condition.py index db8f910..2c74402 100644 --- a/modules/check_stop_condition.py +++ b/modules/check_stop_condition.py @@ -4,17 +4,16 @@ import dronekit +from . import generate_command from . import upload_commands -MAVLINK_RTL_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL -MAVLINK_RTL_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH +DRONE_TIMEOUT = 30.0 # seconds -def check_stop_condition(start_time: float, - current_time: float, - drone: dronekit.Vehicle, - maximum_flight_time: float) -> bool: +def check_stop_condition( + start_time: float, current_time: float, drone: dronekit.Vehicle, maximum_flight_time: float +) -> bool: """ Check if drone exceeds the maximum flight time limit and replace with new mission of returning to launch. @@ -27,8 +26,8 @@ def check_stop_condition(start_time: float, drone: dronekit.Vehicle The connected drone. maximum_flight_time: float - Max flight time for drone in seconds. - + Max flight time for drone in seconds. + Returns ------- bool: True if max flight time was exceeded, False otherwise. @@ -36,26 +35,11 @@ def check_stop_condition(start_time: float, if current_time - start_time < maximum_flight_time: return False - rtl_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_RTL_FRAME, - MAVLINK_RTL_COMMAND, - 0, - 0, - 0, # param1 - 0, - 0, - 0, - 0, - 0, - 0, - ) + rtl_command = generate_command.return_to_launch() # Invoke upload_commands to clear previous commands and direct drone back to launch location - result = upload_commands.upload_commands(drone, [rtl_command]) + result = upload_commands.upload_commands(drone, [rtl_command], DRONE_TIMEOUT) if not result: - print("Unable to upload rtl command to drone command sequence.") + print("Unable to upload RTL command to drone command sequence.") return True diff --git a/modules/diversion_qr_to_waypoint_list.py b/modules/diversion_qr_to_waypoint_list.py index fd1f767..005d01e 100644 --- a/modules/diversion_qr_to_waypoint_list.py +++ b/modules/diversion_qr_to_waypoint_list.py @@ -1,6 +1,7 @@ """ Function to parse diversion QR string into a list of vertices and a rejoin waypoint. """ + import re @@ -16,7 +17,7 @@ def diversion_qr_to_waypoint_list(qr_text: str) -> "tuple[bool, tuple[list[str], Returns ------- - tuple[bool, tuple[list[Waypoint], Waypoint] | None] + tuple[bool, tuple[list[Waypoint], Waypoint] | None] Returns False, None if the string is invalid or if the list is empty. """ # Check if string is of valid form @@ -25,11 +26,14 @@ def diversion_qr_to_waypoint_list(qr_text: str) -> "tuple[bool, tuple[list[str], return False, None # Separate qr_text into diversion_waypoints and rejoin_waypoint strings - diversion_waypoints_string, sep, rejoin_waypoint_string = qr_text.partition('.') + diversion_waypoints_string, _, rejoin_waypoint_string = qr_text.partition(".") # Extract semicolon-separated list of diversion waypoint names without leading/trailing whitespace diversion_waypoints = [ - text.strip(" ") for text in diversion_waypoints_string.replace("Avoid the area bounded by:", "", 1).split(";") + text.strip(" ") + for text in diversion_waypoints_string.replace("Avoid the area bounded by:", "", 1).split( + ";" + ) ] # Remove remaining cases of empty names diff --git a/modules/generate_command.py b/modules/generate_command.py new file mode 100644 index 0000000..31789c4 --- /dev/null +++ b/modules/generate_command.py @@ -0,0 +1,196 @@ +""" +Command generators. +""" + +import dronekit + + +LANDING_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL +RTL_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL +TAKEOFF_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT +WAYPOINT_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT +WAYPOINT_SPLINE_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT +LOITER_TIMED_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT +LOITER_UNLIMITED_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + + +def landing() -> dronekit.Command: + """ + Returns landing command. + """ + return dronekit.Command( + 0, + 0, + 0, + LANDING_FRAME, + dronekit.mavutil.mavlink.MAV_CMD_NAV_LAND, + 0, + 0, + 0, # param1 + 0, + 0, + 0, + 0, + 0, + 0, + ) + + +def return_to_launch() -> dronekit.Command: + """ + Returns RTL command. + """ + return dronekit.Command( + 0, + 0, + 0, + RTL_FRAME, + dronekit.mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, + 0, + 0, # param1 + 0, + 0, + 0, + 0, + 0, + 0, + ) + + +def takeoff(altitude: float) -> dronekit.Command: + """ + Returns takeoff command. + + altitude: Metres. + """ + return dronekit.Command( + 0, + 0, + 0, + TAKEOFF_FRAME, + dronekit.mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, + 0, + 0, # param1 + 0, + 0, + 0, + 0, + 0, + altitude, + ) + + +def waypoint( + hold_time: float, acceptance_radius: float, latitude: float, longitude: float, altitude: float +) -> dronekit.Command: + """ + Returns waypoint command. + + hold_time: Seconds. + acceptance_radius: Metres. + latitude: Decimal degrees. + longitude: Decimal degrees. + altitude: Metres. + """ + return dronekit.Command( + 0, + 0, + 0, + WAYPOINT_FRAME, + dronekit.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, + 0, + hold_time, # param1 + acceptance_radius, + 0, + 0, + latitude, + longitude, + altitude, + ) + + +def waypoint_spline( + hold_time: float, latitude: float, longitude: float, altitude: float +) -> dronekit.Command: + """ + Returns waypoint spline command. + + hold_time: Seconds. + latitude: Decimal degrees. + longitude: Decimal degrees. + altitude: Metres. + """ + return dronekit.Command( + 0, + 0, + 0, + WAYPOINT_SPLINE_FRAME, + dronekit.mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT, + 0, + 0, + hold_time, # param1 + 0, + 0, + 0, + latitude, + longitude, + altitude, + ) + + +def loiter_timed( + loiter_time: float, latitude: float, longitude: float, altitude: float +) -> dronekit.Command: + """ + Returns loiter timed command. + + loiter_time: Seconds. + latitude: Decimal degrees. + longitude: Decimal degrees. + altitude: Metres. + """ + return dronekit.Command( + 0, + 0, + 0, + LOITER_TIMED_FRAME, + dronekit.mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, + 0, + 0, + loiter_time, # param1 + 0, + 0, + 0, + latitude, + longitude, + altitude, + ) + + +def loiter_unlimited(latitude: float, longitude: float, altitude: float) -> dronekit.Command: + """ + Returns loiter unlimited command. + + latitude: Decimal degrees. + longitude: Decimal degrees. + altitude: Metres. + """ + return dronekit.Command( + 0, + 0, + 0, + LOITER_UNLIMITED_FRAME, + dronekit.mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, + 0, + 0, + 0, # param1 + 0, + 0, + 0, + latitude, + longitude, + altitude, + ) diff --git a/modules/load_waypoint_name_to_coordinates_map.py b/modules/load_waypoint_name_to_coordinates_map.py index 3f93c38..52a0a02 100644 --- a/modules/load_waypoint_name_to_coordinates_map.py +++ b/modules/load_waypoint_name_to_coordinates_map.py @@ -1,13 +1,15 @@ """ Name-coordinate mapping from CSV file. """ + import pathlib from .common.kml.modules import location_ground -def load_waypoint_name_to_coordinates_map(waypoint_file_path: pathlib.Path) \ - -> "tuple[bool, dict[str, location_ground.LocationGround]]": +def load_waypoint_name_to_coordinates_map( + waypoint_file_path: pathlib.Path, +) -> "tuple[bool, dict[str, location_ground.LocationGround]]": """ Creates a name to coordinate dictionary from the CSV file. """ @@ -18,12 +20,14 @@ def load_waypoint_name_to_coordinates_map(waypoint_file_path: pathlib.Path) \ with open(waypoint_file_path, encoding="utf-8") as file: for line in file: # Skip header and empty lines - parts = line.split(',') + parts = line.split(",") if line in "name,latitude,longitude\n" or len(parts) < 3: continue name, latitude, longitude = parts - name_to_coordinates_map[name] = location_ground.LocationGround(name, float(latitude), float(longitude)) + name_to_coordinates_map[name] = location_ground.LocationGround( + name, float(latitude), float(longitude) + ) if len(name_to_coordinates_map) > 0: return True, name_to_coordinates_map diff --git a/modules/qr_input.py b/modules/qr_input.py index 6b5c0a8..851d38d 100644 --- a/modules/qr_input.py +++ b/modules/qr_input.py @@ -45,7 +45,7 @@ def qr_input(device: "int | str") -> "tuple[bool, str | None]": break # Exit early on manual quit - if cv2.waitKey(1) == ord('q'): + if cv2.waitKey(1) == ord("q"): break # Cleanup diff --git a/modules/qr_to_waypoint_names.py b/modules/qr_to_waypoint_names.py index 6282e91..a6a860e 100644 --- a/modules/qr_to_waypoint_names.py +++ b/modules/qr_to_waypoint_names.py @@ -1,11 +1,12 @@ """ Module to parse QR string to waypoints. """ + import re def qr_to_waypoint_names(qr_text: str) -> "tuple[bool, list[str] | None]": - """ + """ Function for parsing QR text into list of waypoints. Example of valid string: "Follow route: Waterloo; Aerial; Robotics; Group 15" Example of invalid string: "Avoid the area bounded by: Zulu; Bravo; Tango; Uniform. Rejoin the route at Lima" diff --git a/modules/upload_commands.py b/modules/upload_commands.py index 44f44af..b226a96 100644 --- a/modules/upload_commands.py +++ b/modules/upload_commands.py @@ -5,9 +5,9 @@ import dronekit -def upload_commands(drone: dronekit.Vehicle, - commands: "list[dronekit.Command]", - timeout: int) -> bool: +def upload_commands( + drone: dronekit.Vehicle, commands: "list[dronekit.Command]", timeout: int +) -> bool: """ Add the list of commands to the drone’s command sequence, and upload them. If the list is empty, does not upload anything. @@ -31,7 +31,7 @@ def upload_commands(drone: dronekit.Vehicle, # This is to avoid duplicate or conflicting commands command_sequence = drone.commands command_sequence.download() - command_sequence.wait_ready(timeout = timeout) + command_sequence.wait_ready(timeout=timeout) command_sequence.clear() # Adds new commands to command sequence diff --git a/modules/waypoint_names_to_coordinates.py b/modules/waypoint_names_to_coordinates.py index 87625d4..719e3e1 100644 --- a/modules/waypoint_names_to_coordinates.py +++ b/modules/waypoint_names_to_coordinates.py @@ -5,15 +5,15 @@ from .common.kml.modules import location_ground -def waypoint_names_to_coordinates(waypoint_names: "list[str]", - waypoint_mapping: "dict[str, location_ground.LocationGround]") \ - -> "tuple[bool, list[location_ground.LocationGround]]": +def waypoint_names_to_coordinates( + waypoint_names: "list[str]", waypoint_mapping: "dict[str, location_ground.LocationGround]" +) -> "tuple[bool, list[location_ground.LocationGround]]": """ Converts a list of waypoint names to their corresponding coordinates based on a waypoint mapping. Args: waypoint_names (list[str]): A list of waypoint names. - waypoint_mapping (dict[str, LocationGround]): A dictionary mapping waypoint names to their + waypoint_mapping (dict[str, LocationGround]): A dictionary mapping waypoint names to their corresponding LocationGround objects with coordinates. Returns: diff --git a/modules/waypoint_tracking.py b/modules/waypoint_tracking.py index bbc7551..5a4d3e9 100644 --- a/modules/waypoint_tracking.py +++ b/modules/waypoint_tracking.py @@ -6,8 +6,9 @@ import dronekit -def get_current_waypoint_info(drone: dronekit.Vehicle) \ - -> "tuple[bool, tuple[int, tuple[float, float] | None] | None]": +def get_current_waypoint_info( + drone: dronekit.Vehicle, +) -> "tuple[bool, tuple[int, tuple[float, float] | None] | None]": """ Function to retrieve information about the current waypoint sequence and destination @@ -19,7 +20,7 @@ def get_current_waypoint_info(drone: dronekit.Vehicle) \ Returns ------- tuple[bool, tuple[int, tuple[float, float] | None] | None]: - (True, destination waypoint information), where information is (index, location). + (True, destination waypoint information), where information is (index, location). location can be None. """ # Download the mission commands from the drone diff --git a/modules/waypoints_dict_to_list.py b/modules/waypoints_dict_to_list.py index f1d2f7a..d4d1ad8 100644 --- a/modules/waypoints_dict_to_list.py +++ b/modules/waypoints_dict_to_list.py @@ -5,8 +5,9 @@ from .common.kml.modules import location_ground -def waypoints_dict_to_list(waypoint_name_to_coordinates: "dict[str, location_ground.LocationGround]") \ - -> "tuple[bool, list[location_ground.LocationGround]]": +def waypoints_dict_to_list( + waypoint_name_to_coordinates: "dict[str, location_ground.LocationGround]", +) -> "tuple[bool, list[location_ground.LocationGround]]": """ Converts dictionary of waypoints into a list. diff --git a/modules/waypoints_to_commands.py b/modules/waypoints_to_commands.py index 6f3911a..291a7ba 100644 --- a/modules/waypoints_to_commands.py +++ b/modules/waypoints_to_commands.py @@ -4,16 +4,16 @@ import dronekit +from . import generate_command from .common.kml.modules import location_ground -MAVLINK_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT -MAVLINK_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT -ACCEPT_RADIUS = 10 +ACCEPT_RADIUS = 10.0 # metres -def waypoints_to_commands(waypoints: "list[location_ground.LocationGround]", - altitude: int) -> "tuple[bool, list[dronekit.Command] | None]": +def waypoints_to_commands( + waypoints: "list[location_ground.LocationGround]", altitude: float +) -> "tuple[bool, list[dronekit.Command] | None]": """ Convert list of waypoints to dronekit commands. @@ -21,14 +21,14 @@ def waypoints_to_commands(waypoints: "list[location_ground.LocationGround]", ---------- waypoints: list[LocationGround] list of locationGround objects containing names and coordinates in decimal degrees. - altitude: int + altitude: float altitude in meters to command the drone to. Returns ------- - tuple[bool, list[dronekit.Command] | None]: + tuple[bool, list[dronekit.Command] | None]: (False, None) if empty waypoints list, - (True, dronekit commands that can be sent to the drone) otherwise dronekit commands that can be sent to the drone. + (True, dronekit commands that can be sent to the drone) otherwise. """ if len(waypoints) == 0: return False, None @@ -36,22 +36,10 @@ def waypoints_to_commands(waypoints: "list[location_ground.LocationGround]", dronekit_command_list = [] for point in waypoints: - command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_FRAME, - MAVLINK_COMMAND, - 0, - 0, - 0, # param1 - ACCEPT_RADIUS, - 0, - 0, - point.latitude, - point.longitude, - altitude, + command = generate_command.waypoint( + 0.0, ACCEPT_RADIUS, point.latitude, point.longitude, altitude ) + dronekit_command_list.append(command) return True, dronekit_command_list diff --git a/modules/waypoints_to_spline_commands.py b/modules/waypoints_to_spline_commands.py index 1c3585b..512d927 100644 --- a/modules/waypoints_to_spline_commands.py +++ b/modules/waypoints_to_spline_commands.py @@ -4,30 +4,28 @@ import dronekit +from . import generate_command from .common.kml.modules import location_ground -MAVLINK_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT -MAVLINK_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT - - -def waypoints_to_spline_commands(waypoints: "list[location_ground.LocationGround]", - altitude: int) -> "tuple[bool, list[dronekit.Command] | None]": +def waypoints_to_spline_commands( + waypoints: "list[location_ground.LocationGround]", altitude: float +) -> "tuple[bool, list[dronekit.Command] | None]": """ Convert list of waypoints to a list of spline waypoint dronekit commands. - Spline waypoint dronekit commands fly to the target waypoint following a spline path + Spline waypoint dronekit commands fly to the target waypoint following a spline path (a path that curves around a waypoint). Parameters ---------- waypoints: list[LocationGround] list of locationGround objects containing names and coordinates in decimal degrees. - altitude: int + altitude: float altitude in meters to command the drone to. Returns ------- - tuple[bool, list[dronekit.Command] | None]: + tuple[bool, list[dronekit.Command] | None]: (False, None) if empty waypoints list, (True, list of spline waypoint commands) if waypoints list is not empty. """ @@ -37,21 +35,8 @@ def waypoints_to_spline_commands(waypoints: "list[location_ground.LocationGround dronekit_spline_command_list = [] for waypoint in waypoints: - command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_FRAME, - MAVLINK_COMMAND, - 0, - 0, - 0, # param1 - 0, - 0, - 0, - waypoint.latitude, - waypoint.longitude, - altitude, + command = generate_command.waypoint_spline( + 0.0, waypoint.latitude, waypoint.longitude, altitude ) dronekit_spline_command_list.append(command) diff --git a/path_2023.py b/path_2023.py index fda7495..369d693 100644 --- a/path_2023.py +++ b/path_2023.py @@ -1,6 +1,7 @@ """ -Task 1 path. +Reads waypoints from QR code and sends drone commands. """ + import pathlib import time @@ -21,25 +22,29 @@ WAYPOINT_FILE_PATH = pathlib.Path("waypoints", "wrestrc_waypoints.csv") CAMERA = 0 ALTITUDE = 40 +DRONE_TIMEOUT = 30.0 # seconds CONNECTION_ADDRESS = "tcp:localhost:14550" -KML_FILE_PARENT_DIRECTORY = pathlib.Path("waypoints") -KML_FILE_PREFIX = "waypoints_log" +LOG_DIRECTORY_PATH = pathlib.Path("logs") +KML_FILE_PREFIX = "waypoints" DELAY = 0.1 # seconds -# Required for checks -# pylint: disable-next=too-many-branches,too-many-return-statements -def run() -> int: +# Code copied into path_2024.py +# pylint: disable=duplicate-code +def main() -> int: """ - Reads waypoints from QR code and sends drone commands. + Main function. """ + pathlib.Path(LOG_DIRECTORY_PATH).mkdir(exist_ok=True) + # Wait ready is false as the drone may be on the ground - drone = dronekit.connect(CONNECTION_ADDRESS, wait_ready = False) + drone = dronekit.connect(CONNECTION_ADDRESS, wait_ready=False) - result, waypoint_name_to_coordinates = \ + result, waypoint_name_to_coordinates = ( load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( WAYPOINT_FILE_PATH, ) + ) if not result: print("ERROR: load_waypoint_name_to_coordinates_map") return -1 @@ -53,7 +58,8 @@ def run() -> int: result, _ = ground_locations_to_kml.ground_locations_to_kml( waypoints_list, - KML_FILE_PREFIX, KML_FILE_PARENT_DIRECTORY, + KML_FILE_PREFIX, + LOG_DIRECTORY_PATH, ) if not result: print("ERROR: Unable to generate KML file") @@ -82,13 +88,14 @@ def run() -> int: print("Error: waypoints_to_commands") return -1 - result, takeoff_landing_commands = \ + result, takeoff_landing_commands = ( add_takeoff_and_landing_command.add_takeoff_and_landing_command(waypoint_commands, ALTITUDE) + ) if not result: print("Error: add_takeoff_and_landing_command") return -1 - result = upload_commands.upload_commands(drone, takeoff_landing_commands) + result = upload_commands.upload_commands(drone, takeoff_landing_commands, DRONE_TIMEOUT) if not result: print("Error: upload_commands") return -1 @@ -111,11 +118,12 @@ def run() -> int: return 0 +# pylint: enable=duplicate-code + + if __name__ == "__main__": - # Not a constant - # pylint: disable-next=invalid-name - result_run = run() - if result_run < 0: - print("ERROR") + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") - print("Done") + print("Done!") diff --git a/path_2024.py b/path_2024.py index ca5e5da..9fe42ce 100644 --- a/path_2024.py +++ b/path_2024.py @@ -1,6 +1,7 @@ """ -Path template. +Reads in hardcoded waypoints from CSV file and sends drone commands. """ + import pathlib import time @@ -18,29 +19,30 @@ WAYPOINT_FILE_PATH = pathlib.Path("2024", "waypoints", "wrestrc.csv") ALTITUDE = 40 +DRONE_TIMEOUT = 30.0 # seconds CONNECTION_ADDRESS = "tcp:localhost:14550" -KML_FILE_PARENT_DIRECTORY = pathlib.Path("waypoints") -KML_FILE_PREFIX = "waypoints_log" +LOG_DIRECTORY_PATH = pathlib.Path("logs") +KML_FILE_PREFIX = "waypoints" DELAY = 0.1 # seconds MAXIMUM_FLIGHT_TIME = 1800 # seconds -TIMEOUT = 30 # seconds -# Required for checks -# pylint: disable-next=too-many-return-statements -def run() -> int: +def main() -> int: """ - Reads in hardcoded waypoints from CSV file and sends drone commands. + Main function. """ + pathlib.Path(LOG_DIRECTORY_PATH).mkdir(exist_ok=True) + # Wait ready is false as the drone may be on the ground - drone = dronekit.connect(CONNECTION_ADDRESS, wait_ready = False) + drone = dronekit.connect(CONNECTION_ADDRESS, wait_ready=False) # Read in hardcoded waypoints from CSV file # Waypoints are stored in order of insertion, starting with the top row - result, waypoint_name_to_coordinates = \ + result, waypoint_name_to_coordinates = ( load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( WAYPOINT_FILE_PATH, ) + ) if not result: print("ERROR: load_waypoint_name_to_coordinates_map") return -1 @@ -54,7 +56,8 @@ def run() -> int: result, _ = ground_locations_to_kml.ground_locations_to_kml( waypoints_list, - KML_FILE_PREFIX, KML_FILE_PARENT_DIRECTORY, + KML_FILE_PREFIX, + LOG_DIRECTORY_PATH, ) if not result: print("ERROR: Unable to generate KML file") @@ -68,16 +71,17 @@ def run() -> int: print("Error: waypoints_to_commands") return -1 - result, takeoff_landing_commands = \ + result, takeoff_landing_commands = ( add_takeoff_and_landing_command.add_takeoff_and_landing_command( waypoint_commands, ALTITUDE, ) + ) if not result: print("Error: add_takeoff_and_landing_command") return -1 - result = upload_commands.upload_commands(drone, takeoff_landing_commands, timeout = TIMEOUT) + result = upload_commands.upload_commands(drone, takeoff_landing_commands, DRONE_TIMEOUT) if not result: print("Error: upload_commands") return -1 @@ -95,13 +99,15 @@ def run() -> int: print("Error: waypoint_tracking (get_current_location)") else: print(f"Current location (Lat, Lon): {location}") - + # Send drone back to launch if exceeds time limit current_time = time.time() - is_returning_to_launch = check_stop_condition.check_stop_condition(start_time, current_time, drone, MAXIMUM_FLIGHT_TIME) - if is_returning_to_launch: + is_returning_to_launch = check_stop_condition.check_stop_condition( + start_time, current_time, drone, MAXIMUM_FLIGHT_TIME + ) + if is_returning_to_launch: break - + print(f"Elapsed time (s): {current_time - start_time}") time.sleep(DELAY) @@ -110,10 +116,8 @@ def run() -> int: if __name__ == "__main__": - # Not a constant - # pylint: disable-next=invalid-name - result_run = run() - if result_run < 0: - print("ERROR") + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") - print("Done") + print("Done!") diff --git a/path_2024_task_1.py b/path_2024_task_1.py index 32733cb..a243225 100644 --- a/path_2024_task_1.py +++ b/path_2024_task_1.py @@ -1,6 +1,7 @@ """ -Task 1 path. +Task 1 path. Uploads mission to run a maximum number of laps and monitors the mission for early landing. """ + import pathlib import dronekit @@ -14,47 +15,51 @@ CONNECTION_ADDRESS = "tcp:localhost:14550" -# Required for checks -# pylint: disable-next=too-many-return-statements -def run() -> int: +def main() -> int: """ - Uploads mission to run a maximum number of laps and monitors the mission for early landing. + Main function. """ # Wait ready is false as the drone may be on the ground + # TODO: In progress + # pylint: disable-next=unused-variable drone = dronekit.connect(CONNECTION_ADDRESS, wait_ready=False) # Create waypoint name to coordinate dictionary for takeoff waypoint - result, takeoff_waypoint_dictionary = \ + result, takeoff_waypoint_dictionary = ( load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( TAKEOFF_WAYPOINT_FILE_PATH, ) + ) if not result: print("ERROR: Load waypoint to coordinates map") return -1 # Convert named waypoint dictionary to list - result, waypoint_takeoff_list = \ - waypoints_dict_to_list.waypoints_dict_to_list( - takeoff_waypoint_dictionary, - ) + # TODO: In progress + # pylint: disable-next=unused-variable + result, waypoint_takeoff_list = waypoints_dict_to_list.waypoints_dict_to_list( + takeoff_waypoint_dictionary, + ) if not result: print("ERROR: Convert waypoint dictionary to list") return -1 # Create waypoint name to coordinate dictionary for lap waypoints - result, lap_waypoint_dictionary = \ + result, lap_waypoint_dictionary = ( load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( LAP_WAYPOINTS_FILE_PATH, ) + ) if not result: print("ERROR: Load waypoint to coordinates map") return -1 # Convert lap waypoint dictionary to list - result, lap_waypoint_list = \ - waypoints_dict_to_list.waypoints_dict_to_list( - lap_waypoint_dictionary, - ) + # TODO: In progress + # pylint: disable-next=unused-variable + result, lap_waypoint_list = waypoints_dict_to_list.waypoints_dict_to_list( + lap_waypoint_dictionary, + ) if not result: print("ERROR: Convert waypoint dictionary to list") return -1 @@ -63,10 +68,8 @@ def run() -> int: if __name__ == "__main__": - # Not a constant - # pylint: disable-next=invalid-name - result_run = run() - if result_run < 0: - print("ERROR") + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") - print("Done") + print("Done!") diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..4927a25 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,111 @@ +[tool.pylint.main] +# Add files or directories matching the regular expressions patterns to the +# ignore-list. The regex matches against paths and can be in Posix or Windows +# format. Because '\\' represents the directory delimiter on Windows systems, it +# can't be used as an escape character. +ignore-paths = [ + # From .gitignore + # IDEs + ".idea/", + ".vscode/", + + # Python + "__pycache__/", + "venv/", + + # Logging + "logs/", + + # Outside of .gitignore + # Submodules + "modules/common/", +] + +# Use multiple processes to speed up Pylint. Specifying 0 will auto-detect the +# number of processors available to use, and will cap the count on Windows to +# avoid hangs. +jobs = 0 + +# Minimum Python version to use for version dependent checks. Will default to the +# version used to run pylint. +py-version = "3.8" + +# Discover python modules and packages in the file system subtree. +recursive = true + +# [tool.pylint.basic] +# Good variable names which should always be accepted, separated by a comma. +good-names = [ + "i", + "j", + "k", + "ex", + "Run", + "_", + + # Return of main() + "result_main", +] + +[tool.pylint."messages control"] +# Disable the message, report, category or checker with the given id(s). You can +# either give multiple identifiers separated by comma (,) or put this option +# multiple times (only on the command line, not in the configuration file where +# it should appear only once). You can also use "--disable=all" to disable +# everything first and then re-enable specific checks. For example, if you want +# to run only the similarities checker, you can use "--disable=all +# --enable=similarities". If you want to run only the classes checker, but have +# no Warning level messages displayed, use "--disable=all --enable=classes +# --disable=W". +disable = [ + "raw-checker-failed", + "bad-inline-option", + "locally-disabled", + "file-ignored", + "suppressed-message", + "useless-suppression", + "deprecated-pragma", + "use-symbolic-message-instead", + "use-implicit-booleaness-not-comparison-to-string", + "use-implicit-booleaness-not-comparison-to-zero", + # WARG + # Ignore TODOs + "fixme", + # Pylint cannot find modules + "import-error", + # Covered by Black formatter + "line-too-long", + # Pylint cannot handle 3rd party imports + "no-member", + # Some classes are simple + "too-few-public-methods", + # Function signatures + "too-many-arguments", + # Don't care + "too-many-branches", + # Line count in file + "too-many-lines", + # Don't care + "too-many-locals", + # Don't care + "too-many-statements", + # Don't care + "too-many-return-statements", +] + +[tool.pylint.similarities] +# Minimum lines number of a similarity. +# Main guard +min-similarity-lines = 10 + +[tool.pytest.ini_options] +minversion = "6.0" +# Submodules +addopts = "--ignore=modules/common/" + +[tool.black] +line-length = 100 +target-version = ["py38"] +# Excludes files or directories in addition to the defaults +# Submodules +extend-exclude = "modules/common/*" diff --git a/pytest.ini b/pytest.ini deleted file mode 100644 index 79fccda..0000000 --- a/pytest.ini +++ /dev/null @@ -1,2 +0,0 @@ -[pytest] -addopts = --ignore=modules/common/ diff --git a/requirements.txt b/requirements.txt index 6534727..20f51f7 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,8 @@ # Packages listed in alphabetical order dronekit pytest + +# Linters and formatters are explicitly versioned +black==24.2.0 +flake8-annotations==3.0.1 +pylint==3.0.3 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..6a4f4bf --- /dev/null +++ b/setup.cfg @@ -0,0 +1,23 @@ +[flake8] +# For annotations only +select=ANN +# Disable annotations for `self` and `cls` +# https://github.com/sco1/flake8-annotations +ignore=ANN101,ANN102 +# File exclusion +extend-exclude= + # From .gitignore + # IDEs + .idea/, + .vscode/, + + # Python + __pycache__/, + venv/, + + # Logging + logs/, + + # Outside of .gitignore + # Submodules + modules/common/, diff --git a/tests/integration/__init__.py b/tests/integration/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/test_check_stop_condition.py b/tests/integration/test_check_stop_condition.py similarity index 66% rename from tests/test_check_stop_condition.py rename to tests/integration/test_check_stop_condition.py index 3be23ce..2fc448b 100644 --- a/tests/test_check_stop_condition.py +++ b/tests/integration/test_check_stop_condition.py @@ -1,7 +1,7 @@ """ Test the drone stop condition. """ -import sys + import time from modules import add_takeoff_and_landing_command @@ -16,46 +16,72 @@ MAXIMUM_FLIGHT_TIME = 5 # seconds (for testing purposes) ALTITUDE = 50 # metres +DRONE_TIMEOUT = 30.0 # seconds + DELAY = 1 # seconds -if __name__ == "__main__": +# No enable + + +def main() -> int: + """ + Main function. + """ result, controller = flight_controller.FlightController.create(MISSION_PLANNER_ADDRESS) if not result: print("Unable to connect to Mission Planner.") - sys.exit() + return -1 # Get Pylance to stop complaining assert controller is not None # Upload mission with a single waypoint test_waypoint = location_ground.Location_ground("Test", 43.4731, -80.5419) - result, test_waypoint_commands = waypoints_to_commands.waypoints_to_commands([test_waypoint], ALTITUDE) + result, test_waypoint_commands = waypoints_to_commands.waypoints_to_commands( + [test_waypoint], ALTITUDE + ) if not result: print("Unable to create waypoint commands.") - sys.exit() - - result, commands_with_takeoff_landing = add_takeoff_and_landing_command.add_takeoff_and_landing_command(test_waypoint_commands, ALTITUDE) + return -1 + + result, commands_with_takeoff_landing = ( + add_takeoff_and_landing_command.add_takeoff_and_landing_command( + test_waypoint_commands, ALTITUDE + ) + ) if not result: print("Unable to add takeoff and landing commands.") - sys.exit() + return -1 - result = upload_commands.upload_commands(controller.drone, commands_with_takeoff_landing) + result = upload_commands.upload_commands( + controller.drone, commands_with_takeoff_landing, DRONE_TIMEOUT + ) if not result: print("Unable to upload commands.") - sys.exit() + return -1 # Loop mimics path_2024.py structure start_time = time.time() while True: # Check whether drone exceeds max flight time current_time = time.time() - is_returning_to_launch = check_stop_condition.check_stop_condition(start_time, current_time, controller.drone, MAXIMUM_FLIGHT_TIME) - if is_returning_to_launch: + is_returning_to_launch = check_stop_condition.check_stop_condition( + start_time, current_time, controller.drone, MAXIMUM_FLIGHT_TIME + ) + if is_returning_to_launch: break print(f"Elapsed time (s): {current_time - start_time}") time.sleep(DELAY) + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + print("Done!") diff --git a/tests/test_qr_input.py b/tests/integration/test_qr_input.py similarity index 57% rename from tests/test_qr_input.py rename to tests/integration/test_qr_input.py index 119dcbe..86ecfd5 100644 --- a/tests/test_qr_input.py +++ b/tests/integration/test_qr_input.py @@ -8,12 +8,22 @@ CAMERA = 0 -if __name__ == '__main__': - +def main() -> int: + """ + Main function. + """ result, qr_string = qr_input.qr_input(CAMERA) assert result assert qr_string is not None print(f"Decoded QR code with string value: {qr_string}") + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + print("Done!") diff --git a/tests/test_upload_commands.py b/tests/integration/test_upload_commands.py similarity index 50% rename from tests/test_upload_commands.py rename to tests/integration/test_upload_commands.py index 6d045f0..66ec428 100644 --- a/tests/test_upload_commands.py +++ b/tests/integration/test_upload_commands.py @@ -1,176 +1,126 @@ -""" -Integration test for upload_commands. -""" -import math - -import dronekit - -from modules import upload_commands - - -ALTITUDE = 40 - -MAVLINK_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT -MAVLINK_WAYPOINT = dronekit.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT -MAVLINK_TAKEOFF = dronekit.mavutil.mavlink.MAV_CMD_NAV_TAKEOFF -MAVLINK_LANDING = dronekit.mavutil.mavlink.MAV_CMD_NAV_LAND -MAVLINK_LOITER = dronekit.mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME -DELAY = 3 - -TOLERANCE = 0.0001 - -CONNECTION_ADDRESS = "tcp:localhost:14550" - - -def retrieve_commands(drone: dronekit.Vehicle) -> dronekit.CommandSequence: - """ - Retrieves latest version of commands. - """ - command_sequence = drone.commands - command_sequence.download() - drone.wait_ready() - - return command_sequence - - -def upload_and_check_command_list(drone: dronekit.Vehicle, - commands: "list[dronekit.Command]"): - """ - Test the case of a list of waypoint commands. - """ - result = upload_commands.upload_commands(drone, commands) - assert result - - # Retrieve current drone commands and see if they match with inputs - command_sequence = retrieve_commands(drone) - - for i, command in enumerate(command_sequence): - assert command.frame == commands[i].frame - assert command.command == commands[i].command - assert command.param1 == commands[i].param1 - # Parameters 2,3,4 not being tested since Mission Planner ignores them - assert math.isclose(command.x, commands[i].x, abs_tol = TOLERANCE) - assert math.isclose(command.y, commands[i].y, abs_tol = TOLERANCE) - assert math.isclose(command.z, commands[i].z, abs_tol = TOLERANCE) - - -def upload_and_check_empty_command_list(drone: dronekit.Vehicle): - """ - Test the case of an empty command list. - """ - # Retrieve current drone commands and add them to a list - command_sequence = retrieve_commands(drone) - commands = [] - for command in command_sequence: - commands.append(command) - - # Upload empty command list - empty_command_list = [] - result = upload_commands.upload_commands(drone, empty_command_list) - assert not result - - # Retrieve new commands and compare them with previous list - command_check = retrieve_commands(drone) - - for i, command in enumerate(command_check): - assert command.frame == commands[i].frame - assert command.command == commands[i].command - assert command.param1 == commands[i].param1 - # Parameters 2,3,4 not being tested since Mission Planner ignores them - assert math.isclose(command.x, commands[i].x, abs_tol = TOLERANCE) - assert math.isclose(command.y, commands[i].y, abs_tol = TOLERANCE) - assert math.isclose(command.z, commands[i].z, abs_tol = TOLERANCE) - - -if __name__ == "__main__": - # Drone setup - # Wait ready is false as the drone may be on the ground - dronekit_vehicle = dronekit.connect(CONNECTION_ADDRESS, wait_ready=False) - - # Example waypoints list, converted to waypoint commands - waypoints_input = [(39.140, 22.23), (25.123, -76.324)] - - # Test a command sequence with a variety of commands - commands_input = [] - - for waypoint in waypoints_input: - lat_input, lon_input = waypoint - dronekit_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_FRAME, - MAVLINK_WAYPOINT, - 0, - 0, - DELAY, - 0, - 0, - 0, - lat_input, - lon_input, - ALTITUDE, - ) - commands_input.append(dronekit_command) - - takeoff_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_FRAME, - MAVLINK_TAKEOFF, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - ALTITUDE, - ) - commands_input.append(takeoff_command) - - landing_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_FRAME, - MAVLINK_LANDING, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - ) - commands_input.append(landing_command) - - loiter_command = dronekit.Command( - 0, - 0, - 0, - MAVLINK_FRAME, - MAVLINK_LOITER, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - ALTITUDE, - ) - commands_input.append(loiter_command) - - # Test with the command sequence - upload_and_check_command_list(dronekit_vehicle, commands_input) - - # Test with empty command sequence - upload_and_check_empty_command_list(dronekit_vehicle) - - print("Done!") +""" +Integration test for upload_commands. +""" + +import math + +import dronekit + +from modules import generate_command +from modules import upload_commands + + +ALTITUDE = 40 +DRONE_TIMEOUT = 30.0 # seconds + +WAYPOINT_HOLD_TIME = 3.0 # seconds +ACCEPT_RADIUS = 10.0 # metres + +TOLERANCE = 0.0001 + +CONNECTION_ADDRESS = "tcp:localhost:14550" + + +def retrieve_commands(drone: dronekit.Vehicle) -> dronekit.CommandSequence: + """ + Retrieves latest version of commands. + """ + command_sequence = drone.commands + command_sequence.download() + drone.wait_ready() + + return command_sequence + + +def upload_command_list_and_assert( + drone: dronekit.Vehicle, commands: "list[dronekit.Command]" +) -> None: + """ + Test the case of a list of waypoint commands. + """ + result = upload_commands.upload_commands(drone, commands, DRONE_TIMEOUT) + assert result + + # Retrieve current drone commands and see if they match with inputs + command_sequence = retrieve_commands(drone) + + for i, command in enumerate(command_sequence): + assert command.frame == commands[i].frame + assert command.command == commands[i].command + assert command.param1 == commands[i].param1 + # Parameters 2,3,4 not being tested since Mission Planner ignores them + assert math.isclose(command.x, commands[i].x, abs_tol=TOLERANCE) + assert math.isclose(command.y, commands[i].y, abs_tol=TOLERANCE) + assert math.isclose(command.z, commands[i].z, abs_tol=TOLERANCE) + + +def upload_empty_command_list_and_assert(drone: dronekit.Vehicle) -> None: + """ + Test the case of an empty command list. + """ + # Retrieve current drone commands and add them to a list + command_sequence = retrieve_commands(drone) + commands = [] + for command in command_sequence: + commands.append(command) + + # Upload empty command list + empty_command_list = [] + result = upload_commands.upload_commands(drone, empty_command_list, DRONE_TIMEOUT) + assert not result + + # Retrieve new commands and compare them with previous list + command_check = retrieve_commands(drone) + + for i, command in enumerate(command_check): + assert command.frame == commands[i].frame + assert command.command == commands[i].command + assert command.param1 == commands[i].param1 + # Parameters 2,3,4 not being tested since Mission Planner ignores them + assert math.isclose(command.x, commands[i].x, abs_tol=TOLERANCE) + assert math.isclose(command.y, commands[i].y, abs_tol=TOLERANCE) + assert math.isclose(command.z, commands[i].z, abs_tol=TOLERANCE) + + +def main() -> int: + """ + Main function. + """ + # Drone setup + # Wait ready is false as the drone may be on the ground + dronekit_vehicle = dronekit.connect(CONNECTION_ADDRESS, wait_ready=False) + + # Example waypoints list, converted to waypoint commands + waypoints_input = [(39.140, 22.23), (25.123, -76.324)] + + # Test a command sequence with a variety of commands + commands_input = [] + + for waypoint in waypoints_input: + lat_input, lon_input = waypoint + dronekit_command = generate_command.waypoint( + WAYPOINT_HOLD_TIME, ACCEPT_RADIUS, lat_input, lon_input, ALTITUDE + ) + commands_input.append(dronekit_command) + + takeoff_command = generate_command.takeoff(ALTITUDE) + commands_input.append(takeoff_command) + + landing_command = generate_command.landing() + commands_input.append(landing_command) + + # Test with the command sequence + upload_command_list_and_assert(dronekit_vehicle, commands_input) + + # Test with empty command sequence + upload_empty_command_list_and_assert(dronekit_vehicle) + + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + + print("Done!") diff --git a/tests/test_add_takeoff_and_landing_command.py b/tests/test_add_takeoff_and_landing_command.py deleted file mode 100644 index fee48c5..0000000 --- a/tests/test_add_takeoff_and_landing_command.py +++ /dev/null @@ -1,115 +0,0 @@ -""" -Test process. -""" -import copy - -import dronekit -import pytest - -from modules import add_takeoff_and_landing_command - - -ALTITUDE = 50 -MAVLINK_TEST_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT -MAVLINK_TEST_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL -ACCEPT_RADIUS = 10 -FIRST_WAYPOINT_LATITUDE = 42.123 -FIRST_WAYPOINT_LONGITUDE = -73.456 -SECOND_WAYPOINT_LATITUDE = 42.789 -SECOND_WAYPOINT_LONGITUDE = -73.987 - - -@pytest.fixture -def non_empty_commands() -> "list[dronekit.Command]": - """ - Fixture for a list of commands. - """ - commands = [ - dronekit.Command( - 0, - 0, - 0, - MAVLINK_TEST_FRAME, - MAVLINK_TEST_COMMAND, - 0, - 0, - 0, # param1 - ACCEPT_RADIUS, - 0, - 0, - FIRST_WAYPOINT_LATITUDE, - FIRST_WAYPOINT_LONGITUDE, - ALTITUDE, - ), - dronekit.Command( - 0, - 0, - 0, - MAVLINK_TEST_FRAME, - MAVLINK_TEST_COMMAND, - 0, - 0, - 0, # param1 - ACCEPT_RADIUS, - 0, - 0, - SECOND_WAYPOINT_LATITUDE, - SECOND_WAYPOINT_LONGITUDE, - ALTITUDE, - ) - ] - yield commands - - -@pytest.fixture -def empty_commands() -> "list[dronekit.Command]": - """ - Fixture for an empty list of commands. - """ - commands = [] - yield commands - - -def assert_expected_takeoff_and_landing_commands(commands_actual: "list[dronekit.Command]", - commands_expected: "list[dronekit.Command]", - altitude: float): - """ - Helper function to assert the correctness of takeoff and landing commands. - """ - assert isinstance(commands_actual, list) - - # Test takeoff command - takeoff_command = commands_actual[0] - assert isinstance(takeoff_command, dronekit.Command) - assert takeoff_command.frame == add_takeoff_and_landing_command.MAVLINK_TAKEOFF_FRAME - assert takeoff_command.command == add_takeoff_and_landing_command.MAVLINK_TAKEOFF_COMMAND - assert takeoff_command.z == altitude - - # Test landing command - landing_command = commands_actual[-1] - assert isinstance(landing_command, dronekit.Command) - assert landing_command.frame == add_takeoff_and_landing_command.MAVLINK_LANDING_FRAME - assert landing_command.command == add_takeoff_and_landing_command.MAVLINK_LANDING_COMMAND - - # Test original commands - assert len(commands_actual) == len(commands_expected) + 2 - assert commands_actual[1:-1] == commands_expected - -def test_add_takeoff_and_landing_on_empty_commands(empty_commands: "list[dronekit.Command]"): - """ - Tests functionality correctness of add_takeoff_and_landing_command on empty list of commands. - """ - result, commands_actual = add_takeoff_and_landing_command.add_takeoff_and_landing_command(empty_commands, ALTITUDE) - - assert not result - assert commands_actual is None - -def test_add_takeoff_and_landing_on_nonempty_commands(non_empty_commands: "list[dronekit.Command]"): - """ - Tests functionality correctness of add_takeoff_and_landing_command on non-empty list of commands. - """ - commands_expected = copy.deepcopy(non_empty_commands) - result, commands_actual = add_takeoff_and_landing_command.add_takeoff_and_landing_command(non_empty_commands, ALTITUDE) - - assert result - assert_expected_takeoff_and_landing_commands(commands_actual, commands_expected, ALTITUDE) diff --git a/tests/test_add_takeoff_and_rtl_command.py b/tests/test_add_takeoff_and_rtl_command.py deleted file mode 100644 index c957c1a..0000000 --- a/tests/test_add_takeoff_and_rtl_command.py +++ /dev/null @@ -1,115 +0,0 @@ -""" -Test process. -""" -import copy - -import dronekit -import pytest - -from modules import add_takeoff_and_rtl_command - - -ALTITUDE = 50 -MAVLINK_TEST_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT -MAVLINK_TEST_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL -ACCEPT_RADIUS = 10 -FIRST_WAYPOINT_LATITUDE = 42.123 -FIRST_WAYPOINT_LONGITUDE = -73.456 -SECOND_WAYPOINT_LATITUDE = 42.789 -SECOND_WAYPOINT_LONGITUDE = -73.987 - - -@pytest.fixture -def non_empty_commands() -> "list[dronekit.Command]": - """ - Fixture for a list of commands. - """ - commands = [ - dronekit.Command( - 0, - 0, - 0, - MAVLINK_TEST_FRAME, - MAVLINK_TEST_COMMAND, - 0, - 0, - 0, # param1 - ACCEPT_RADIUS, - 0, - 0, - FIRST_WAYPOINT_LATITUDE, - FIRST_WAYPOINT_LONGITUDE, - ALTITUDE, - ), - dronekit.Command( - 0, - 0, - 0, - MAVLINK_TEST_FRAME, - MAVLINK_TEST_COMMAND, - 0, - 0, - 0, # param1 - ACCEPT_RADIUS, - 0, - 0, - SECOND_WAYPOINT_LATITUDE, - SECOND_WAYPOINT_LONGITUDE, - ALTITUDE, - ) - ] - yield commands - - -@pytest.fixture -def empty_commands() -> "list[dronekit.Command]": - """ - Fixture for an empty list of commands. - """ - commands = [] - yield commands - - -def assert_expected_takeoff_and_rtl_commands(commands_actual: "list[dronekit.Command]", - commands_expected: "list[dronekit.Command]", - altitude: float): - """ - Helper function to assert the correctness of takeoff and RTL commands. - """ - assert isinstance(commands_actual, list) - - # Test takeoff command - takeoff_command = commands_actual[0] - assert isinstance(takeoff_command, dronekit.Command) - assert takeoff_command.frame == add_takeoff_and_rtl_command.MAVLINK_TAKEOFF_FRAME - assert takeoff_command.command == add_takeoff_and_rtl_command.MAVLINK_TAKEOFF_COMMAND - assert takeoff_command.z == altitude - - # Test RTL command - rtl_command = commands_actual[-1] - assert isinstance(rtl_command, dronekit.Command) - assert rtl_command.frame == add_takeoff_and_rtl_command.MAVLINK_RTL_FRAME - assert rtl_command.command == add_takeoff_and_rtl_command.MAVLINK_RTL_COMMAND - - # Test original commands - assert len(commands_actual) == len(commands_expected) + 2 - assert commands_actual[1:-1] == commands_expected - -def test_add_takeoff_and_rtl_on_empty_commands(empty_commands: "list[dronekit.Command]"): - """ - Tests functionality correctness of add_takeoff_and_rtl_command on empty list of commands. - """ - result, commands_actual = add_takeoff_and_rtl_command.add_takeoff_and_rtl_command(empty_commands, ALTITUDE) - - assert not result - assert commands_actual is None - -def test_add_takeoff_and_rtl_on_nonempty_commands(non_empty_commands: "list[dronekit.Command]"): - """ - Tests functionality correctness of add_takeoff_and_rtl_command on non-empty list of commands. - """ - commands_expected = copy.deepcopy(non_empty_commands) - result, commands_actual = add_takeoff_and_rtl_command.add_takeoff_and_rtl_command(non_empty_commands, ALTITUDE) - - assert result - assert_expected_takeoff_and_rtl_commands(commands_actual, commands_expected, ALTITUDE) diff --git a/tests/test_qr_to_waypoint_names.py b/tests/test_qr_to_waypoint_names.py deleted file mode 100644 index 195a323..0000000 --- a/tests/test_qr_to_waypoint_names.py +++ /dev/null @@ -1,159 +0,0 @@ -""" -Testcases for parsing QR to waypoint. -""" - -from modules import qr_to_waypoint_names - - -def test_valid_waypoint_single_simple(): - """ - Test basic single case. - """ - input = "Follow route: Alpha" - expected = ["Alpha"] - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert result - assert actual == expected - - -def test_valid_waypoing_single_whitepace(): - """ - Test single waypoint with surrounding whitespace. - """ - input = "Follow route: Epsilon " - expected = ["Epsilon"] - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - assert result - assert actual == expected - - -def test_valid_waypoints_multiple_simple(): - """ - Test basic case of multiple waypoints. - """ - input = "Follow route: Waterloo; Aerial; Robotics; Group 15" - expected = ["Waterloo", "Aerial", "Robotics", "Group 15"] - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert result - assert actual == expected - - -def test_valid_waypoints_multiple_varying_whitespace(): - """ - Test multiple waypoints with varying amounts of whitespace in between. - """ - input = "Follow route: A; B; C,; D " - expected = ["A", "B", "C,", "D"] - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert result - assert actual == expected - - -def test_prefix_only(): - """ - Test input of only prefix - """ - input = "Follow route: " - expected = None - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert not result - assert actual == expected - - -def test_multiple_empty_waypoints(): - """ - Test parsing of multiple empty waypoints with valid delimiter. - """ - input = "Follow route: ; ; ; ;" - expected = None - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert not result - assert actual == expected - - -def test_invalid_waypoints_simple(): - """ - Test case where prefix string does not appear whatsoever. - """ - input = "Avoid the area bounded by: Zulu; Bravo; Tango; Uniform. Rejoin the route at Lima" - expected = None - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert not result - assert actual == expected - - -def test_invalid_waypoints_leading_whitespace(): - """ - Test case leading white space in front of otherwise valid qr string. - """ - input = " Follow route: A; B; C; D" - expected = None - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert not result - assert actual == expected - - -def test_invalid_waypoints_typo(): - """ - Test incorrect capitalization/spelling of otherwise valid qr string. - """ - input = "Follow Route: A; B; C" - expected = None - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert not result - assert actual == expected - - -def test_incorrect_delimiter_comma_separated(): - """ - Test valid qr string with comma delimiter. - """ - input = "Follow route: A, B, C, D" - expected = ["A, B, C, D"] - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - - assert result - assert actual == expected - - -def test_incorrect_delimiter_space_separated(): - """ - Test valid qr string with space separated waypoints. - """ - input = "Follow route: A B C D E F G" - expected = ["A B C D E F G"] - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names(input) - assert result - assert actual == expected - - -def test_incorrect_delimiter_colon_separated(): - """ - Test valid qr string with comma separated waypoints. - """ - input = "Follow route: A: B: C: D:" - expected = ["A: B: C: D:"] - - result, actual = qr_to_waypoint_names.qr_to_waypoint_names("Follow route: A: B: C: D:") - - assert result - assert actual == expected diff --git a/tests/unit/__init__.py b/tests/unit/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/unit/test_add_takeoff_and_landing_command.py b/tests/unit/test_add_takeoff_and_landing_command.py new file mode 100644 index 0000000..d33c1ce --- /dev/null +++ b/tests/unit/test_add_takeoff_and_landing_command.py @@ -0,0 +1,106 @@ +""" +Test process. +""" + +import copy + +import dronekit +import pytest + +from modules import add_takeoff_and_landing_command +from modules import generate_command + + +ALTITUDE = 50.0 # metres +ACCEPT_RADIUS = 10.0 # metres +FIRST_WAYPOINT_LATITUDE = 42.123 +FIRST_WAYPOINT_LONGITUDE = -73.456 +SECOND_WAYPOINT_LATITUDE = 42.789 +SECOND_WAYPOINT_LONGITUDE = -73.987 + + +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=duplicate-code,protected-access,redefined-outer-name + + +@pytest.fixture +def non_empty_commands() -> "list[dronekit.Command]": # type: ignore + """ + Fixture for a list of commands. + """ + commands = [ + generate_command.waypoint( + 0.0, ACCEPT_RADIUS, FIRST_WAYPOINT_LATITUDE, FIRST_WAYPOINT_LONGITUDE, ALTITUDE + ), + generate_command.waypoint( + 0.0, ACCEPT_RADIUS, SECOND_WAYPOINT_LATITUDE, SECOND_WAYPOINT_LONGITUDE, ALTITUDE + ), + ] + yield commands # type: ignore + + +@pytest.fixture +def empty_commands() -> "list[dronekit.Command]": # type: ignore + """ + Fixture for an empty list of commands. + """ + commands = [] + yield commands # type: ignore + + +def assert_expected_takeoff_and_landing_commands( + commands_actual: "list[dronekit.Command]", + commands_expected: "list[dronekit.Command]", + altitude: float, +) -> None: + """ + Helper function to assert the correctness of takeoff and landing commands. + """ + assert isinstance(commands_actual, list) + + # Test takeoff command + takeoff_command = commands_actual[0] + assert isinstance(takeoff_command, dronekit.Command) + assert takeoff_command.frame == dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + assert takeoff_command.command == dronekit.mavutil.mavlink.MAV_CMD_NAV_TAKEOFF + assert takeoff_command.z == altitude + + # Test landing command + landing_command = commands_actual[-1] + assert isinstance(landing_command, dronekit.Command) + assert landing_command.frame == dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL + assert landing_command.command == dronekit.mavutil.mavlink.MAV_CMD_NAV_LAND + + # Test original commands + assert len(commands_actual) == len(commands_expected) + 2 + assert commands_actual[1:-1] == commands_expected + + +def test_add_takeoff_and_landing_on_empty_commands( + empty_commands: "list[dronekit.Command]", +) -> None: + """ + Tests functionality correctness of add_takeoff_and_landing_command on empty list of commands. + """ + result, commands_actual = add_takeoff_and_landing_command.add_takeoff_and_landing_command( + empty_commands, ALTITUDE + ) + + assert not result + assert commands_actual is None + + +def test_add_takeoff_and_landing_on_nonempty_commands( + non_empty_commands: "list[dronekit.Command]", +) -> None: + """ + Tests functionality correctness of add_takeoff_and_landing_command on non-empty list of commands. + """ + commands_expected = copy.deepcopy(non_empty_commands) + result, commands_actual = add_takeoff_and_landing_command.add_takeoff_and_landing_command( + non_empty_commands, ALTITUDE + ) + + assert result + assert_expected_takeoff_and_landing_commands(commands_actual, commands_expected, ALTITUDE) diff --git a/tests/unit/test_add_takeoff_and_rtl_command.py b/tests/unit/test_add_takeoff_and_rtl_command.py new file mode 100644 index 0000000..66f3aa6 --- /dev/null +++ b/tests/unit/test_add_takeoff_and_rtl_command.py @@ -0,0 +1,103 @@ +""" +Test process. +""" + +import copy + +import dronekit +import pytest + +from modules import add_takeoff_and_rtl_command +from modules import generate_command + +ALTITUDE = 50.0 # metres +ACCEPT_RADIUS = 10.0 # metres +FIRST_WAYPOINT_LATITUDE = 42.123 +FIRST_WAYPOINT_LONGITUDE = -73.456 +SECOND_WAYPOINT_LATITUDE = 42.789 +SECOND_WAYPOINT_LONGITUDE = -73.987 + + +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=duplicate-code,protected-access,redefined-outer-name + + +@pytest.fixture +def non_empty_commands() -> "list[dronekit.Command]": # type: ignore + """ + Fixture for a list of commands. + """ + commands = [ + generate_command.waypoint( + 0.0, ACCEPT_RADIUS, FIRST_WAYPOINT_LATITUDE, FIRST_WAYPOINT_LONGITUDE, ALTITUDE + ), + generate_command.waypoint( + 0.0, ACCEPT_RADIUS, SECOND_WAYPOINT_LATITUDE, SECOND_WAYPOINT_LONGITUDE, ALTITUDE + ), + ] + yield commands + + +@pytest.fixture +def empty_commands() -> "list[dronekit.Command]": # type: ignore + """ + Fixture for an empty list of commands. + """ + commands = [] + yield commands + + +def assert_expected_takeoff_and_rtl_commands( + commands_actual: "list[dronekit.Command]", + commands_expected: "list[dronekit.Command]", + altitude: float, +) -> None: + """ + Helper function to assert the correctness of takeoff and RTL commands. + """ + assert isinstance(commands_actual, list) + + # Test takeoff command + takeoff_command = commands_actual[0] + assert isinstance(takeoff_command, dronekit.Command) + assert takeoff_command.frame == dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + assert takeoff_command.command == dronekit.mavutil.mavlink.MAV_CMD_NAV_TAKEOFF + assert takeoff_command.z == altitude + + # Test RTL command + rtl_command = commands_actual[-1] + assert isinstance(rtl_command, dronekit.Command) + assert rtl_command.frame == dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL + assert rtl_command.command == dronekit.mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH + + # Test original commands + assert len(commands_actual) == len(commands_expected) + 2 + assert commands_actual[1:-1] == commands_expected + + +def test_add_takeoff_and_rtl_on_empty_commands(empty_commands: "list[dronekit.Command]") -> None: + """ + Tests functionality correctness of add_takeoff_and_rtl_command on empty list of commands. + """ + result, commands_actual = add_takeoff_and_rtl_command.add_takeoff_and_rtl_command( + empty_commands, ALTITUDE + ) + + assert not result + assert commands_actual is None + + +def test_add_takeoff_and_rtl_on_nonempty_commands( + non_empty_commands: "list[dronekit.Command]", +) -> None: + """ + Tests functionality correctness of add_takeoff_and_rtl_command on non-empty list of commands. + """ + commands_expected = copy.deepcopy(non_empty_commands) + result, commands_actual = add_takeoff_and_rtl_command.add_takeoff_and_rtl_command( + non_empty_commands, ALTITUDE + ) + + assert result + assert_expected_takeoff_and_rtl_commands(commands_actual, commands_expected, ALTITUDE) diff --git a/tests/test_diversion_qr_to_waypoint_list.py b/tests/unit/test_diversion_qr_to_waypoint_list.py similarity index 60% rename from tests/test_diversion_qr_to_waypoint_list.py rename to tests/unit/test_diversion_qr_to_waypoint_list.py index 3972c1a..6635673 100644 --- a/tests/test_diversion_qr_to_waypoint_list.py +++ b/tests/unit/test_diversion_qr_to_waypoint_list.py @@ -1,113 +1,118 @@ -""" -Testcases for parsing diversion QR code to waypoint list and a rejoin waypoint. -""" - -from modules import diversion_qr_to_waypoint_list - - -def test_valid_multiple_avoid_waypoints(): - """ - Test valid input. - """ - # Setup - input = "Avoid the area bounded by: Alpha; Beta. Rejoin the route at Gamma" - expected = ["Alpha", "Beta"], "Gamma" - - # Run - result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(input) - - # Test - assert result - assert actual == expected - - -def test_valid_single_avoid_waypoint(): - """ - Test valid input of single waypoint list and a rejoin waypoint. - """ - # Setup - input = "Avoid the area bounded by: Alpha. Rejoin the route at Beta" - expected = ["Alpha"], "Beta" - - # Run - result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(input) - - # Test - assert result - assert actual == expected - - -def test_valid_waypoints_whitespace(): - """ - Test valid input with varying amounts of whitespace in between. - """ - # Setup - input = "Avoid the area bounded by: A; B; C,; D . Rejoin the route at Beta " - expected = ["A", "B", "C,", "D"], "Beta" - - # Run - result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(input) - - # Test - assert result - assert actual == expected - - -def test_empty_waypoint_list_and_rejoin(): - """ - Test input without waypoints list and rejoin waypoint. - """ - # Setup - input = "Avoid the area bounded by: . Rejoin the route at" - - # Run - result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(input) - - # Test - assert not result - assert actual is None - - -def test_empty_waypoint_list(): - """ - Test case without a waypoint list. - """ - # Setup - input = "Avoid the area bounded by: ; ; ; ;. Rejoin the route at Beta" - - # Run - result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(input) - - # Test - assert not result - assert actual is None - - -def test_empty_rejoin_waypoint(): - """ - Test case without a rejoin waypoint. - """ - # Setup - input = "Avoid the area bounded by: Zulu; Bravo; Tango; Uniform. Rejoin the route at" - - # Run - result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(input) - - # Test - assert not result - assert actual is None - - -def test_mismatch_input(): - """ - Test case with no matching text. - """ - # Setup - input = "Lorem ipsum dolor sit amet. Zulu; Tango; Alpha." - - # Run - result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(input) - - # Test - assert not result - assert actual is None +""" +Testcases for parsing diversion QR code to waypoint list and a rejoin waypoint. +""" + +from modules import diversion_qr_to_waypoint_list + + +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +def test_valid_multiple_avoid_waypoints() -> None: + """ + Test valid input. + """ + # Setup + text = "Avoid the area bounded by: Alpha; Beta. Rejoin the route at Gamma" + expected = ["Alpha", "Beta"], "Gamma" + + # Run + result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(text) + + # Test + assert result + assert actual == expected + + +def test_valid_single_avoid_waypoint() -> None: + """ + Test valid input of single waypoint list and a rejoin waypoint. + """ + # Setup + text = "Avoid the area bounded by: Alpha. Rejoin the route at Beta" + expected = ["Alpha"], "Beta" + + # Run + result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(text) + + # Test + assert result + assert actual == expected + + +def test_valid_waypoints_whitespace() -> None: + """ + Test valid input with varying amounts of whitespace in between. + """ + # Setup + text = "Avoid the area bounded by: A; B; C,; D . Rejoin the route at Beta " + expected = ["A", "B", "C,", "D"], "Beta" + + # Run + result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(text) + + # Test + assert result + assert actual == expected + + +def test_empty_waypoint_list_and_rejoin() -> None: + """ + Test input without waypoints list and rejoin waypoint. + """ + # Setup + text = "Avoid the area bounded by: . Rejoin the route at" + + # Run + result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(text) + + # Test + assert not result + assert actual is None + + +def test_empty_waypoint_list() -> None: + """ + Test case without a waypoint list. + """ + # Setup + text = "Avoid the area bounded by: ; ; ; ;. Rejoin the route at Beta" + + # Run + result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(text) + + # Test + assert not result + assert actual is None + + +def test_empty_rejoin_waypoint() -> None: + """ + Test case without a rejoin waypoint. + """ + # Setup + text = "Avoid the area bounded by: Zulu; Bravo; Tango; Uniform. Rejoin the route at" + + # Run + result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(text) + + # Test + assert not result + assert actual is None + + +def test_mismatch_input() -> None: + """ + Test case with no matching text. + """ + # Setup + text = "Lorem ipsum dolor sit amet. Zulu; Tango; Alpha." + + # Run + result, actual = diversion_qr_to_waypoint_list.diversion_qr_to_waypoint_list(text) + + # Test + assert not result + assert actual is None diff --git a/tests/test_load_waypoint_name_to_coordinates_map.py b/tests/unit/test_load_waypoint_name_to_coordinates_map.py similarity index 57% rename from tests/test_load_waypoint_name_to_coordinates_map.py rename to tests/unit/test_load_waypoint_name_to_coordinates_map.py index bb1c2e9..998988f 100644 --- a/tests/test_load_waypoint_name_to_coordinates_map.py +++ b/tests/unit/test_load_waypoint_name_to_coordinates_map.py @@ -1,39 +1,44 @@ """ Testing with real files. """ + import pathlib from modules import load_waypoint_name_to_coordinates_map from modules.common.kml.modules import location_ground -def test_normal_file(): +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +def test_normal_file() -> None: """ Normal CSV file. """ # Setup normal_csv_file_path = pathlib.Path("tests", "test_csv", "test_normal_csv.csv") expected = { - "WARG": location_ground.LocationGround("WARG", 43.47323264522664, -80.54011639872981), + "WARG": location_ground.LocationGround("WARG", 43.47323264522664, -80.54011639872981), "University of Waterloo Station for 301 ION": location_ground.LocationGround( - "University of Waterloo Station for 301 ION", + "University of Waterloo Station for 301 ION", 43.4735247614021, -80.54144667502672, ), } # Run - result, actual = \ - load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( - normal_csv_file_path, - ) + result, actual = load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( + normal_csv_file_path, + ) # Test assert result assert actual == expected -def test_empty_file(): +def test_empty_file() -> None: """ Empty CSV file. """ @@ -41,17 +46,16 @@ def test_empty_file(): empty_csv_file_path = pathlib.Path("test_csv", "test_empty_csv.csv") # Run - result, actual = \ - load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( - empty_csv_file_path, - ) + result, actual = load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( + empty_csv_file_path, + ) # Test assert not result assert actual is None -def test_nonexistent_file(): +def test_nonexistent_file() -> None: """ CSV file doesn't exist. """ @@ -59,10 +63,9 @@ def test_nonexistent_file(): nonexistent_file_path = pathlib.Path("test_csv", "file_does_not_exist.abc") # Run - result, actual = \ - load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( - nonexistent_file_path, - ) + result, actual = load_waypoint_name_to_coordinates_map.load_waypoint_name_to_coordinates_map( + nonexistent_file_path, + ) # Test assert not result diff --git a/tests/unit/test_qr_to_waypoint_names.py b/tests/unit/test_qr_to_waypoint_names.py new file mode 100644 index 0000000..685739c --- /dev/null +++ b/tests/unit/test_qr_to_waypoint_names.py @@ -0,0 +1,164 @@ +""" +Testcases for parsing QR to waypoint. +""" + +from modules import qr_to_waypoint_names + + +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +def test_valid_waypoint_single_simple() -> None: + """ + Test basic single case. + """ + text = "Follow route: Alpha" + expected = ["Alpha"] + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert result + assert actual == expected + + +def test_valid_waypoing_single_whitepace() -> None: + """ + Test single waypoint with surrounding whitespace. + """ + text = "Follow route: Epsilon " + expected = ["Epsilon"] + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + assert result + assert actual == expected + + +def test_valid_waypoints_multiple_simple() -> None: + """ + Test basic case of multiple waypoints. + """ + text = "Follow route: Waterloo; Aerial; Robotics; Group 15" + expected = ["Waterloo", "Aerial", "Robotics", "Group 15"] + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert result + assert actual == expected + + +def test_valid_waypoints_multiple_varying_whitespace() -> None: + """ + Test multiple waypoints with varying amounts of whitespace in between. + """ + text = "Follow route: A; B; C,; D " + expected = ["A", "B", "C,", "D"] + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert result + assert actual == expected + + +def test_prefix_only() -> None: + """ + Test text of only prefix + """ + text = "Follow route: " + expected = None + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert not result + assert actual == expected + + +def test_multiple_empty_waypoints() -> None: + """ + Test parsing of multiple empty waypoints with valid delimiter. + """ + text = "Follow route: ; ; ; ;" + expected = None + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert not result + assert actual == expected + + +def test_invalid_waypoints_simple() -> None: + """ + Test case where prefix string does not appear whatsoever. + """ + text = "Avoid the area bounded by: Zulu; Bravo; Tango; Uniform. Rejoin the route at Lima" + expected = None + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert not result + assert actual == expected + + +def test_invalid_waypoints_leading_whitespace() -> None: + """ + Test case leading white space in front of otherwise valid qr string. + """ + text = " Follow route: A; B; C; D" + expected = None + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert not result + assert actual == expected + + +def test_invalid_waypoints_typo() -> None: + """ + Test incorrect capitalization/spelling of otherwise valid qr string. + """ + text = "Follow Route: A; B; C" + expected = None + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert not result + assert actual == expected + + +def test_incorrect_delimiter_comma_separated() -> None: + """ + Test valid qr string with comma delimiter. + """ + text = "Follow route: A, B, C, D" + expected = ["A, B, C, D"] + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert result + assert actual == expected + + +def test_incorrect_delimiter_space_separated() -> None: + """ + Test valid qr string with space separated waypoints. + """ + text = "Follow route: A B C D E F G" + expected = ["A B C D E F G"] + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + assert result + assert actual == expected + + +def test_incorrect_delimiter_colon_separated() -> None: + """ + Test valid qr string with comma separated waypoints. + """ + text = "Follow route: A: B: C: D:" + expected = ["A: B: C: D:"] + + result, actual = qr_to_waypoint_names.qr_to_waypoint_names(text) + + assert result + assert actual == expected diff --git a/tests/test_waypoints_dict_to_list.py b/tests/unit/test_waypoints_dict_to_list.py similarity index 68% rename from tests/test_waypoints_dict_to_list.py rename to tests/unit/test_waypoints_dict_to_list.py index 95a43dd..4c5f42b 100644 --- a/tests/test_waypoints_dict_to_list.py +++ b/tests/unit/test_waypoints_dict_to_list.py @@ -6,13 +6,18 @@ from modules.common.kml.modules import location_ground -def test_valid_waypoint_dict(): +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +def test_valid_waypoint_dict() -> None: """ Test conversion to list for a valid dict. """ - alpha = location_ground.LocationGround("Alpha", 43.4340501,-80.5789803) - bravo = location_ground.LocationGround("Bravo", 43.4335758,-80.5775237) - charlie = location_ground.LocationGround("Charlie", 43.4336672,-80.57839) + alpha = location_ground.LocationGround("Alpha", 43.4340501, -80.5789803) + bravo = location_ground.LocationGround("Bravo", 43.4335758, -80.5775237) + charlie = location_ground.LocationGround("Charlie", 43.4336672, -80.57839) waypoint_mapping = {"Alpha": alpha, "Bravo": bravo, "Charlie": charlie} expected = [alpha, bravo, charlie] @@ -23,7 +28,8 @@ def test_valid_waypoint_dict(): assert result assert actual == expected -def test_empty_waypoint_dict(): + +def test_empty_waypoint_dict() -> None: """ Test conversion to list for an empty dict. """ diff --git a/tests/test_waypoints_names_to_coordinates.py b/tests/unit/test_waypoints_names_to_coordinates.py similarity index 82% rename from tests/test_waypoints_names_to_coordinates.py rename to tests/unit/test_waypoints_names_to_coordinates.py index 99b5510..1fd9035 100644 --- a/tests/test_waypoints_names_to_coordinates.py +++ b/tests/unit/test_waypoints_names_to_coordinates.py @@ -10,14 +10,20 @@ "Aerial": location_ground.LocationGround("Aerial", 9, 7), "Group 15": location_ground.LocationGround("Group 15", 3, 4), "Robotics": location_ground.LocationGround("Robotics", -1, 0), - "University of Waterloo Station for 301 ION": \ - location_ground.LocationGround("University of Waterloo Station for 301 ION", 6, 6), + "University of Waterloo Station for 301 ION": location_ground.LocationGround( + "University of Waterloo Station for 301 ION", 6, 6 + ), "WARG": location_ground.LocationGround("WARG", 8, 2), "Waterloo": location_ground.LocationGround("Waterloo", 2, -5), } -def test_valid_names(): +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +def test_valid_names() -> None: """ Valid names as input. """ @@ -41,7 +47,7 @@ def test_valid_names(): assert actual == expected -def test_empty_names(): +def test_empty_names() -> None: """ Empty list as input. """ @@ -59,7 +65,7 @@ def test_empty_names(): assert actual is None -def test_invalid_names(): +def test_invalid_names() -> None: """ Names that don't exist in the map. """ @@ -77,7 +83,7 @@ def test_invalid_names(): assert actual is None -def test_valid_and_invalid(): +def test_valid_and_invalid() -> None: """ A mix of existent and non-existent names. """ diff --git a/tests/test_waypoints_to_commands.py b/tests/unit/test_waypoints_to_commands.py similarity index 78% rename from tests/test_waypoints_to_commands.py rename to tests/unit/test_waypoints_to_commands.py index c7cc5f1..34db53a 100644 --- a/tests/test_waypoints_to_commands.py +++ b/tests/unit/test_waypoints_to_commands.py @@ -8,7 +8,12 @@ from modules.common.kml.modules import location_ground -def test_waypoints_to_commands_empty_input(): +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +def test_waypoints_to_commands_empty_input() -> None: """ Tests functionality correctness of waypoints_to_commands on empty input. """ @@ -21,7 +26,7 @@ def test_waypoints_to_commands_empty_input(): assert commands_actual is None -def test_waypoints_to_commands(): +def test_waypoints_to_commands() -> None: """ Tests functionality correctness of waypoints_to_commands. """ @@ -44,8 +49,8 @@ def test_waypoints_to_commands(): lng_expected = waypoints[i].longitude assert isinstance(command, dronekit.Command) - assert command.frame == waypoints_to_commands.MAVLINK_FRAME - assert command.command == waypoints_to_commands.MAVLINK_COMMAND + assert command.frame == dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + assert command.command == dronekit.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT assert command.param1 == 0 assert command.param2 == waypoints_to_commands.ACCEPT_RADIUS assert command.param3 == 0 diff --git a/tests/test_waypoints_to_spline_commands.py b/tests/unit/test_waypoints_to_spline_commands.py similarity index 76% rename from tests/test_waypoints_to_spline_commands.py rename to tests/unit/test_waypoints_to_spline_commands.py index ff66091..65d73da 100644 --- a/tests/test_waypoints_to_spline_commands.py +++ b/tests/unit/test_waypoints_to_spline_commands.py @@ -8,7 +8,12 @@ from modules.common.kml.modules import location_ground -def test_waypoints_to_spline_commands_empty_input(): +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +def test_waypoints_to_spline_commands_empty_input() -> None: """ Tests functionality of waypoints_to_spline_commands on empty input. """ @@ -24,7 +29,7 @@ def test_waypoints_to_spline_commands_empty_input(): assert commands_actual is None -def test_waypoints_to_spline_commands(): +def test_waypoints_to_spline_commands() -> None: """ Tests functionality of waypoints_to_spline_commands. """ @@ -47,8 +52,8 @@ def test_waypoints_to_spline_commands(): for i, command in enumerate(commands_actual): assert isinstance(command, dronekit.Command) - assert command.frame == waypoints_to_spline_commands.MAVLINK_FRAME - assert command.command == waypoints_to_spline_commands.MAVLINK_COMMAND + assert command.frame == dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + assert command.command == dronekit.mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT assert command.param1 == 0 assert command.param2 == 0 assert command.param3 == 0