Read Mavlink from CSV file. Modules completed with tests

modules/advanced_csv_to_commands.py

@@ -0,0 +1,159 @@
+"""
+An command generator with advanced parameters and greater flexibility
+"""
+
+import pathlib
+
+import dronekit
+
+
+VALID_FRAMES = {
+    "global": dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL,
+    "global_relative_alt": dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
+}
+
+VALID_COMMANDS = {
+    "land": dronekit.mavutil.mavlink.MAV_CMD_NAV_LAND,
+    "return_to_launch": dronekit.mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
+    "takeoff": dronekit.mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+    "waypoint": dronekit.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
+    "waypoint_spline": dronekit.mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT,
+    "loiter_timed": dronekit.mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
+    "loiter_unlimited": dronekit.mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
+    "do_jump": dronekit.mavutil.mavlink.MAV_CMD_DO_JUMP,
+}
+
+"""
+The input matrix maps the command types to the valid parameters.
+See documentation for more information: https://uwarg-docs.atlassian.net/wiki/spaces/CV/pages/2567274629/Encoded+MAVLink+commands 
+0: Parameter should be set to zero
+1: Parameter should be a float
+2: Parameter should be an integer
+"""
+COMMAND_TO_PARAMETER_MATRIX = {
+    "land": [0, 0, 0, 0, 1, 1, 0],
+    "return_to_launch": [0, 0, 0, 0, 0, 0, 0],
+    "takeoff": [0, 0, 0, 0, 0, 0, 1],
+    "waypoint": [1, 1, 0, 0, 1, 1, 1],
+    "waypoint_spline": [1, 0, 0, 0, 1, 1, 1],
+    "loiter_timed": [1, 0, 0, 0, 1, 1, 1],
+    "loiter_unlimited": [0, 0, 0, 0, 1, 1, 1],
+    "do_jump": [2, 2, 0, 0, 0, 0, 0],
+}
+
+
+def check_validity(params: "list[float]", matrix_rules: "list[float]") -> bool:
+    """
+    Compares a command's parameters to the matrix rules, and returns whether
+    the command is valid or not
+    """
+    if len(params) != len(matrix_rules):
+        return False
+
+    for index, param in enumerate(params):
+        if matrix_rules[index] == 0 and param != 0:
+            return False
+        if matrix_rules[index] == 2 and not param.is_integer():
+            return False
+
+    return True
+
+
+def generate_command_advanced(
+    frame: str,
+    command_type: str,
+    param1: float,  # I'm anxious about this because of do_jump which requires ints
+    param2: float,
+    param3: float,
+    param4: float,
+    param5: float,
+    param6: float,
+    param7: float,
+) -> "tuple[bool,dronekit.Command | None]":
+    """
+    Parameters: See documentation: https://uwarg-docs.atlassian.net/wiki/spaces/CV/pages/2567274629/Encoded+MAVLink+commands
+
+    Returns: Whether the command was successful and the mission command.
+    """
+
+    # Invalid command
+    if command_type not in VALID_COMMANDS:
+        return False, None
+
+    # Invalid frame type
+    if frame not in VALID_FRAMES:
+        return False, None
+
+    params = [param1, param2, param3, param4, param5, param6, param7]
+
+    if not check_validity(params, COMMAND_TO_PARAMETER_MATRIX[command_type]):
+        return False, None
+
+    return True, dronekit.Command(
+        0,
+        0,
+        0,
+        VALID_FRAMES[frame],
+        VALID_COMMANDS[command_type],
+        0,
+        0,
+        param1,
+        param2,
+        param3,
+        param4,
+        param5,
+        param6,
+        param7,
+    )
+
+
+def csv_to_commands_list(
+    mission_file_path: pathlib.Path,
+) -> "tuple[bool, list[dronekit.Command] | None]":
+    """
+    A method that reads a list of advanced commands from a csv file and generates a mission.
+    Parameters:
+        - mission_file_path: The advanced csv file containing the different mission commands
+
+    Returns:
+        - Whether or not the mission was successfully created
+        - A list of dronekit commands that represent the mission
+
+    """
+    if not mission_file_path.exists():
+        return False, None
+
+    mission = []
+    with open(mission_file_path, encoding="utf-8") as file:
+        for line in file:
+            # Skip header and empty lines
+            if line in "frame,command_type,param1,param2,param3,param4,param5,param6,param7\n":
+                continue
+
+            parts = line.split(",")
+
+            # Incorrect number of parameters
+            if len(parts) != 9:
+                return False, None
+
+            frame, command_type, param1, param2, param3, param4, param5, param6, param7 = parts
+            result, command = generate_command_advanced(
+                frame,
+                command_type,
+                float(param1),
+                float(param2),
+                float(param3),
+                float(param4),
+                float(param5),
+                float(param6),
+                float(param7),
+            )
+            if not result:
+                return False, None
+
+            mission.append(command)
+
+    if len(mission) == 0:
+        return False, None
+
+    return True, mission

tests/test_csv/advanced_csv/test_empty_advanced_csv.csv

@@ -0,0 +1 @@
+frame,command_type,param1,param2,param3,param4,param5,param6,param7

tests/test_csv/advanced_csv/test_incorrect_num_params_csv.csv

@@ -0,0 +1,9 @@
+frame,command_type,param1,param2,param3,param4,param5,param6,param7
+global_relative_alt,takeoff,0,0,0,0,0,0,50
+global_relative_alt,loiter_timed,6,0,0,0,43.47323264522664,-80.5401164,50
+global_relative_alt,loiter_unlimited,0,0,0,43.47,-80.55,50
+global_relative_alt,waypoint,0,0.1,0,0,43.47352476,-80.54144668,10
+global_relative_alt,waypoint_spline,0,0,0,0,43.47323264522664,-80.5401164,10
+global,return_to_launch,0,0,0,0,0,0,0
+global,land,0,0,0,0,0,0,0
+global_relative_alt,do_jump,4,2,0,0,0,0,0

tests/test_csv/advanced_csv/test_invalid_command_name.csv

@@ -0,0 +1,9 @@
+frame,command_type,param1,param2,param3,param4,param5,param6,param7
+global_relative_alt,takeoff,0,0,0,0,0,0,50
+global_relative_alt,loiter_timed,6,0,0,0,43.47323264522664,-80.5401164,50
+global_relative_alt,loiter_unlimited,0,0,0,0,43.47,-80.55,50
+global_relative_alt,wayypoint,0,0.1,0,0,43.47352476,-80.54144668,10
+global_relative_alt,waypoint_spline,0,0,0,0,43.47323264522664,-80.5401164,10
+global,return_to_launch,0,0,0,0,0,0,0
+global,land,0,0,0,0,0,0,0
+global_relative_alt,do_jump,4,2,0,0,0,0,0

tests/test_csv/advanced_csv/test_invalid_frame_name.csv

@@ -0,0 +1,9 @@
+frame,command_type,param1,param2,param3,param4,param5,param6,param7
+global_relative_alt,takeoff,0,0,0,0,0,0,50
+global_relative_alt,loiter_timed,6,0,0,0,43.47323264522664,-80.5401164,50
+global_relative_alt,loiter_unlimited,0,0,0,0,43.47,-80.55,50
+global_relative_alt,waypoint,0,0.1,0,0,43.47352476,-80.54144668,10
+global_relative_alt,waypoint_spline,0,0,0,0,43.47323264522664,-80.5401164,10
+globbal,return_to_launch,0,0,0,0,0,0,0
+global,land,0,0,0,0,0,0,0
+global_relative_alt,do_jump,4,2,0,0,0,0,0

tests/test_csv/advanced_csv/test_non_integer_param_csv.csv

@@ -0,0 +1,9 @@
+frame,command_type,param1,param2,param3,param4,param5,param6,param7
+global_relative_alt,takeoff,0,0,0,0,0,0,50
+global_relative_alt,loiter_timed,6,0,0,0,43.47323264522664,-80.5401164,50
+global_relative_alt,loiter_unlimited,0,0,0,0,43.47,-80.55,50
+global_relative_alt,waypoint,0,0.1,0,0,43.47352476,-80.54144668,10
+global_relative_alt,waypoint_spline,0,0,0,0,43.47323264522664,-80.5401164,10
+global,return_to_launch,0,0,0,0,0,0,0
+global,land,0,0,0,0,0,0,0
+global_relative_alt,do_jump,4.3,2,0,0,0,0,0

tests/test_csv/advanced_csv/test_nonzero_param_csv.csv

@@ -0,0 +1,9 @@
+frame,command_type,param1,param2,param3,param4,param5,param6,param7
+global_relative_alt,takeoff,1,0,0,0,0,0,50
+global_relative_alt,loiter_timed,6,0,0,0,43.47323264522664,-80.5401164,50
+global_relative_alt,loiter_unlimited,0,0,0,0,43.47,-80.55,50
+global_relative_alt,waypoint,0,0.1,0,0,43.47352476,-80.54144668,10
+global_relative_alt,waypoint_spline,0,0,0,0,43.47323264522664,-80.5401164,10
+global,return_to_launch,0,0,0,0,0,0,0
+global,land,0,0,0,0,0,0,0
+global_relative_alt,do_jump,4,2,0,0,0,0,0

tests/test_csv/advanced_csv/test_normal_advanced_csv.csv

@@ -0,0 +1,9 @@
+frame,command_type,param1,param2,param3,param4,param5,param6,param7
+global_relative_alt,takeoff,0,0,0,0,0,0,50
+global_relative_alt,loiter_timed,6,0,0,0,43.47323264522664,-80.5401164,50
+global_relative_alt,loiter_unlimited,0,0,0,0,43.47,-80.55,50
+global_relative_alt,waypoint,0,0.1,0,0,43.47352476,-80.54144668,10
+global_relative_alt,waypoint_spline,0,0,0,0,43.47323264522664,-80.5401164,10
+global,return_to_launch,0,0,0,0,0,0,0
+global,land,0,0,0,0,0,0,0
+global_relative_alt,do_jump,4.0,2,0,0,0,0,0

tests/unit/test_advanced_csv_to_commands.py

@@ -0,0 +1,134 @@
+"""
+Testing the advanced_csv_to_commands module
+"""
+
+import pathlib
+
+import dronekit
+
+from modules import advanced_csv_to_commands
+from modules import generate_command
+
+
+def test_normal_file() -> None:
+    """
+    A normal advanced CSV file
+    """
+    normal_advanced_csv_path = pathlib.Path(
+        "tests", "test_csv", "advanced_csv", "test_normal_advanced_csv.csv"
+    )
+
+    expected_mission = [
+        generate_command.takeoff(50),
+        generate_command.loiter_timed(6.0, 43.47323264522664, -80.5401164, 50),
+        generate_command.loiter_unlimited(43.47, -80.55, 50),
+        generate_command.waypoint(0, 0.1, 43.47352476, -80.54144668, 10),
+        generate_command.waypoint_spline(0, 43.47323264522664, -80.5401164, 10),
+        generate_command.return_to_launch(),
+        generate_command.landing(),
+        generate_command.do_jump(4, 2),
+    ]
+
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(normal_advanced_csv_path)
+
+    # Test
+    assert success
+    assert isinstance(mission, list)
+    assert len(mission) == len(expected_mission)
+
+    for idx, command in enumerate(mission):
+        assert isinstance(command, dronekit.Command)
+        assert command.frame == expected_mission[idx].frame
+        assert command.command == expected_mission[idx].command
+        assert command.param1 == expected_mission[idx].param1
+        assert command.param2 == expected_mission[idx].param2
+        assert command.param3 == expected_mission[idx].param3
+        assert command.param4 == expected_mission[idx].param4
+        assert command.x == expected_mission[idx].x
+        assert command.y == expected_mission[idx].y
+        assert command.z == expected_mission[idx].z
+
+
+def test_empty_csv() -> None:
+    """
+    CSV file is empty, expected to fail
+    """
+    empty_advanced_csv_path = pathlib.Path(
+        "tests", "test_csv", "advanced_csv", "test_empty_advanced_csv.csv"
+    )
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(empty_advanced_csv_path)
+    assert not success
+    assert mission is None
+
+
+def test_invalid_command_name() -> None:
+    """
+    waypoint is misspelled as "wayypoint", expected to fail
+    """
+    invalid_command_name_path = pathlib.Path(
+        "tests", "test_csv", "advanced_csv", "test_invalid_command_name.csv"
+    )
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(invalid_command_name_path)
+    assert not success
+    assert mission is None
+
+
+def test_invalid_frame_name() -> None:
+    """
+    global is misspelled as "globbal", expected to fail
+    """
+    invalid_frame_name_path = pathlib.Path(
+        "tests", "test_csv", "advanced_csv", "test_invalid_frame_name.csv"
+    )
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(invalid_frame_name_path)
+    assert not success
+    assert mission is None
+
+
+def test_file_path_does_not_exist() -> None:
+    """
+    The file path does not exist, expected to fail
+    """
+    invalid_path = pathlib.Path("tests", "test_csv", "path_DNE.csv")
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(invalid_path)
+    assert not success
+    assert mission is None
+
+
+def test_bad_line() -> None:
+    """
+    A line does not have the correct number of parameters.
+    Fourth line (third command) is missing a 0 and therefore an invalid command
+    """
+    invalid_path = pathlib.Path(
+        "tests", "test_csv", "advanced_csv", "test_incorrect_num_params_csv.csv"
+    )
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(invalid_path)
+    assert not success
+    assert mission is None
+
+
+def test_nonzero_param() -> None:
+    """
+    A parameter that should be set to zero is not zero (takeoff param1),
+    expected to fail
+    """
+    nonzero_param_path = pathlib.Path(
+        "tests", "test_csv", "advanced_csv", "test_nonzero_param_csv.csv"
+    )
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(nonzero_param_path)
+    assert not success
+    assert mission is None
+
+
+def test_non_int_param() -> None:
+    """
+    Do-jump's parameters are supposed to be integers but they are not,
+    expected to fail
+    """
+    non_integer_param_path = pathlib.Path(
+        "tests", "test_csv", "advanced_csv", "test_non_integer_param_csv.csv"
+    )
+    success, mission = advanced_csv_to_commands.csv_to_commands_list(non_integer_param_path)
+    assert not success
+    assert mission is None