From 22b54801c8e4cc5b06e86f3dd4e1657558b158cf Mon Sep 17 00:00:00 2001 From: applepie7864 Date: Tue, 28 Nov 2023 11:22:46 -0500 Subject: [PATCH 1/5] fixed --- modules/waypoints_to_spline_commands.py | 56 ++++++++++++++++++++++ tests/test_waypoints_to_spline_commands.py | 55 +++++++++++++++++++++ 2 files changed, 111 insertions(+) create mode 100644 modules/waypoints_to_spline_commands.py create mode 100644 tests/test_waypoints_to_spline_commands.py diff --git a/modules/waypoints_to_spline_commands.py b/modules/waypoints_to_spline_commands.py new file mode 100644 index 0000000..4c9f3c8 --- /dev/null +++ b/modules/waypoints_to_spline_commands.py @@ -0,0 +1,56 @@ +""" +Function to convert list of waypoints to dronekit commands. +""" + +import dronekit + +from . import waypoint + + +MAVLINK_FRAME = dronekit.mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT +MAVLINK_COMMAND = dronekit.mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT +ACCEPT_RADIUS = 10 + + +def waypoints_to_spline_commands(waypoints: "list[waypoint.Waypoint]", + altitude: int) -> "tuple[bool, list[dronekit.Command] | None]": + """ + Convert list of waypoints to dronekit commands. + + Parameters + ---------- + waypoints: list[Waypoint] + list of Waypoint objects containing names and coordinates in decimal degrees. + altitude: int + altitude in meters to command the drone to. + Returns + ------- + 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. + """ + if len(waypoints) == 0: + return False, None + + 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, + ) + dronekit_command_list.append(command) + + return True, dronekit_command_list diff --git a/tests/test_waypoints_to_spline_commands.py b/tests/test_waypoints_to_spline_commands.py new file mode 100644 index 0000000..75e377a --- /dev/null +++ b/tests/test_waypoints_to_spline_commands.py @@ -0,0 +1,55 @@ +""" +Test process. +""" + +import dronekit + +from modules import waypoint +from modules import waypoints_to_spline_commands + + +def test_waypoints_to_spline_commands_empty_input(): + """ + Tests functionality correctness of waypoints_to_spline_commands on empty input. + """ + waypoints = [] + altitude = 100 + + result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, altitude) + + assert not result + assert commands_actual is None + + +def test_waypoints_to_spline_commands(): + """ + Tests functionality correctness of waypoints_to_spline_commands. + """ + waypoints = [ + waypoint.Waypoint("Waypoint 1", 42.123, -73.456), + waypoint.Waypoint("Waypoint 2", 42.789, -73.987), + waypoint.Waypoint("Waypoint 3", 42.555, -73.321), + ] + altitude = 100 + + result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, altitude) + + assert result + + assert isinstance(commands_actual, list) + assert len(commands_actual) == len(waypoints) + + for i, command in enumerate(commands_actual): + lat_expected = waypoints[i].latitude + lng_expected = waypoints[i].longitude + + 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.param1 == 0 + assert command.param2 == waypoints_to_spline_commands.ACCEPT_RADIUS + assert command.param3 == 0 + assert command.param4 == 0 + assert command.x == lat_expected + assert command.y == lng_expected + assert command.z == altitude From a574aeeb80f2e4fc03f2df77e780555a4697655f Mon Sep 17 00:00:00 2001 From: applepie7864 Date: Wed, 29 Nov 2023 13:33:07 -0500 Subject: [PATCH 2/5] edits --- modules/waypoints_to_spline_commands.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/waypoints_to_spline_commands.py b/modules/waypoints_to_spline_commands.py index 4c9f3c8..723b978 100644 --- a/modules/waypoints_to_spline_commands.py +++ b/modules/waypoints_to_spline_commands.py @@ -13,7 +13,7 @@ def waypoints_to_spline_commands(waypoints: "list[waypoint.Waypoint]", - altitude: int) -> "tuple[bool, list[dronekit.Command] | None]": + altitude: int) -> "tuple[bool, list[dronekit.Command] | None]": """ Convert list of waypoints to dronekit commands. @@ -27,7 +27,7 @@ def waypoints_to_spline_commands(waypoints: "list[waypoint.Waypoint]", ------- 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) """ if len(waypoints) == 0: return False, None From 27ee179b3c8aba1edce70a768d735fb49a35075c Mon Sep 17 00:00:00 2001 From: applepie7864 Date: Fri, 1 Dec 2023 14:09:31 -0500 Subject: [PATCH 3/5] fix --- tests/test_waypoints_to_spline_commands.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/test_waypoints_to_spline_commands.py b/tests/test_waypoints_to_spline_commands.py index 75e377a..5e7f499 100644 --- a/tests/test_waypoints_to_spline_commands.py +++ b/tests/test_waypoints_to_spline_commands.py @@ -15,7 +15,8 @@ def test_waypoints_to_spline_commands_empty_input(): waypoints = [] altitude = 100 - result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, altitude) + result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, + altitude) assert not result assert commands_actual is None @@ -32,7 +33,8 @@ def test_waypoints_to_spline_commands(): ] altitude = 100 - result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, altitude) + result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, + altitude) assert result From 8f0f300fc36a424894b4411108e5dbda8d50d6ff Mon Sep 17 00:00:00 2001 From: applepie7864 Date: Mon, 4 Dec 2023 20:02:04 -0500 Subject: [PATCH 4/5] edit --- tests/test_waypoints_to_spline_commands.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/tests/test_waypoints_to_spline_commands.py b/tests/test_waypoints_to_spline_commands.py index 5e7f499..6d52110 100644 --- a/tests/test_waypoints_to_spline_commands.py +++ b/tests/test_waypoints_to_spline_commands.py @@ -15,8 +15,10 @@ def test_waypoints_to_spline_commands_empty_input(): waypoints = [] altitude = 100 - result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, - altitude) + result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands( + waypoints, + altitude + ) assert not result assert commands_actual is None @@ -33,8 +35,10 @@ def test_waypoints_to_spline_commands(): ] altitude = 100 - result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands(waypoints, - altitude) + result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands( + waypoints, + altitude + ) assert result From 88cf8c441318450389fb21ff8f1b7e89e1a86291 Mon Sep 17 00:00:00 2001 From: applepie7864 Date: Tue, 5 Dec 2023 17:30:47 -0500 Subject: [PATCH 5/5] edit --- tests/test_waypoints_to_spline_commands.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tests/test_waypoints_to_spline_commands.py b/tests/test_waypoints_to_spline_commands.py index 6d52110..70cffbc 100644 --- a/tests/test_waypoints_to_spline_commands.py +++ b/tests/test_waypoints_to_spline_commands.py @@ -16,9 +16,9 @@ def test_waypoints_to_spline_commands_empty_input(): altitude = 100 result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands( - waypoints, - altitude - ) + waypoints, + altitude + ) assert not result assert commands_actual is None @@ -36,9 +36,9 @@ def test_waypoints_to_spline_commands(): altitude = 100 result, commands_actual = waypoints_to_spline_commands.waypoints_to_spline_commands( - waypoints, - altitude - ) + waypoints, + altitude + ) assert result