diff --git a/modules/common b/modules/common index 09811f6..07b515d 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit 09811f6c5ba5182509fef5b32dc1c346de5676ff +Subproject commit 07b515df9b56b9a65142d826f720b88f66c1a5f0 diff --git a/modules/plot_circular_path.py b/modules/plot_circular_path.py new file mode 100644 index 0000000..080146d --- /dev/null +++ b/modules/plot_circular_path.py @@ -0,0 +1,115 @@ +""" +Module to plot n circular waypoints, given a center and radius. +""" + +import csv +import math + +from modules.common.mavlink.modules.drone_odometry import DronePosition +from modules.common.mavlink.modules.drone_odometry_local import DronePositionLocal +from modules.common.mavlink.modules.local_global_conversion import drone_position_global_from_local + +from .waypoint import Waypoint + + +def move_coordinates_by_offset( + start_point: Waypoint, offset_x: float, offset_y: float, name: str +) -> "tuple[bool, Waypoint | None]": + """Given a starting waypoint and offsets x and y displacements, find the + resulting waypoint. + + Args: + starting_point (Waypoint): The starting waypoint + offset_x (float): The offset in the west-east direction. Positive for + east, negative for west. + offset_y (float): The offset in the north-south direction. Positive for + north, negative for south. + name (str): The name for the resulting waypoint + + Returns: + tuple[bool, Waypoint | None]: Either return (False, None), indicating a + a failure in execution, or (True, waypoint), where waypoint is the + resulting waypoint. + """ + success, offset_local = DronePositionLocal.create(offset_y, offset_x, 0) + if not success: + return False, None + + # Because drone_position_global_from_local requires DronePosition class, + # we need to convert Waypoint to DronePosition first + success, start_point_converted = DronePosition.create( + start_point.location_ground.latitude, + start_point.location_ground.longitude, + start_point.altitude, + ) + if not success: + return False, None + + success, end_point = drone_position_global_from_local(start_point_converted, offset_local) + if not success: + return False, None + + return True, Waypoint(name, end_point.latitude, end_point.longitude, end_point.altitude) + + +def generate_circular_path( + center: Waypoint, radius: float, num_points: int +) -> "tuple[bool, list[Waypoint] | None]": + """Generate a list of `num_points` evenly-separated waypoints given a center + and radius. + + Args: + center (Waypoint): The center of the circular path + radius (float): The length of the radius, in meters + num_points (int): The number of waypoints to generate + + Returns: + tuple[bool, list[Waypoint] | None]: Either return (False, None), + indicating a failure in execution, or (True, waypoints), where + waypoints is the list of waypoints forming a circle `radius` away + from `center`. + """ + # validate input + if radius <= 0 or num_points <= 0: + return False, None + + waypoints = [] + + # any two consecutive points are separated by 2 * pi / n radians. + for i in range(num_points): + # number of radians away from standard position + rad = 2 * math.pi / num_points * i + offset_x = radius * math.cos(rad) + offset_y = radius * math.sin(rad) + + success, waypoint = move_coordinates_by_offset( + center, offset_x, offset_y, f"Waypoint {i + 1}" + ) + if not success: + return False, None + + waypoints.append(waypoint) + + return True, waypoints + + +def save_waypoints_to_csv(waypoints: "list[Waypoint]", filename: str) -> None: + """Save a list of waypoints to a CSV file. + + Args: + waypoints (list[Waypoint]): The list of waypoints to save + filename (str): The name of the CSV file to save the waypoints to + """ + + with open(filename, mode="w", encoding="UTF-8") as file: + writer = csv.writer(file) + writer.writerow(["Name", "Latitude", "Longitude", "Altitude"]) + for waypoint in waypoints: + writer.writerow( + [ + waypoint.location_ground.name, + waypoint.location_ground.latitude, + waypoint.location_ground.longitude, + waypoint.altitude, + ] + ) diff --git a/modules/waypoint.py b/modules/waypoint.py index 4d4e792..2fdc78b 100644 --- a/modules/waypoint.py +++ b/modules/waypoint.py @@ -51,4 +51,4 @@ def __repr__(self) -> str: """ String representation """ - return f"LocationGroundAndAltitude: {repr(self.location_ground)}, altitude: {self.altitude}" + return f"LocationGroundAndAltitude: {str(self.location_ground)}, altitude: {self.altitude}" diff --git a/tests/unit/test_plot_circular_path.py b/tests/unit/test_plot_circular_path.py new file mode 100644 index 0000000..e562eb2 --- /dev/null +++ b/tests/unit/test_plot_circular_path.py @@ -0,0 +1,127 @@ +""" +Test plotting waypoints in a circular fashion around a center +""" + +from modules import plot_circular_path +from modules.waypoint import Waypoint + + +def assert_close_enough(point_1: Waypoint, point_2: Waypoint, epsilon: float = 1e-7) -> bool: + """ + Assert whether two waypoints are within epsilon within each other, in both + the x and y directions. + A default epsilon of 1e-7 was chosen so that any error within that range is + within an error of 1.1 centimeter. + """ + return ( + abs(point_1.location_ground.latitude - point_2.location_ground.latitude) < epsilon + and abs(point_1.location_ground.longitude - point_2.location_ground.longitude) < epsilon + and abs(point_1.altitude - point_2.altitude) < epsilon + ) + + +def test_move_north_east() -> None: + """ + Move the drone north east + """ + starting_point = Waypoint("Start", 10, 12, 1) + offset_x = 100_000 # east + offset_y = 100_000 # north + expected_point = Waypoint("End", 10.899322, 12.913195, 1) + + success, result_point = plot_circular_path.move_coordinates_by_offset( + starting_point, offset_x, offset_y, "End" + ) + + assert success + assert isinstance(result_point, Waypoint) + assert_close_enough(result_point, expected_point) + + +def test_generate_circular_path() -> None: + """ + Generate a circular path + """ + center = Waypoint("Center", 10, 12, 1) + radius = 1_000_000 + num_points = 20 + expected_points = [ + (10.0, 21.131950912937036), + (12.77905659637457, 20.685001422236247), + (15.286079770270131, 19.387903480363878), + (17.275664625968226, 17.367626071283176), + (18.55305673554031, 14.82192802389536), + (18.993216059187304, 12.0), + (18.55305673554031, 9.17807197610464), + (17.275664625968226, 6.632373928716825), + (15.286079770270131, 4.612096519636122), + (12.779056596374572, 3.3149985777637543), + (10.0, 2.868049087062964), + (7.220943403625431, 3.3149985777637543), + (4.71392022972987, 4.612096519636121), + (2.724335374031777, 6.632373928716823), + (1.4469432644596925, 9.178071976104638), + (1.006783940812694, 12.0), + (1.4469432644596907, 14.821928023895358), + (2.724335374031776, 17.367626071283176), + (4.713920229729867, 19.387903480363878), + (7.220943403625428, 20.685001422236247), + ] + + success, waypoints = plot_circular_path.generate_circular_path(center, radius, num_points) + + assert success + assert isinstance(waypoints, list) + assert len(waypoints) == num_points + + for i in range(num_points): + assert isinstance(waypoints[i], Waypoint) + assert_close_enough( + waypoints[i], Waypoint(f"Waypoint {i}", expected_points[i][0], expected_points[i][1], 1) + ) + + +def test_move_north_east_invalid_inputs() -> None: + """ + Test that moving a Waypoint with 0 altitude fails + """ + success, result_point = plot_circular_path.move_coordinates_by_offset( + Waypoint("Start", 12, 36, 0), -0.2, 3.6, "End" + ) + assert not success + assert result_point is None + + +def test_generate_circular_path_invalid_input() -> None: + """ + Test generating circular path with invalid inputs. + """ + inputs = [ + (Waypoint("Center", 22.4, -6.7, -0.2), 2, 4), + (Waypoint("Center", 3.99, 12.6, 3.4), 0, 10), + (Waypoint("Center", 3, 6, 12), 500, 0), + ] + + for center, radius, num_points in inputs: + success, waypoints = plot_circular_path.generate_circular_path(center, radius, num_points) + assert not success + assert waypoints is None + + +def test_save_waypoints_to_csv(tmp_path: str) -> None: + """ + Save waypoints to a CSV file + """ + waypoints = [ + Waypoint("WP1", 10, 12, 0), + Waypoint("WP2", 10.0001, 12.0001, 0), + ] + filename = tmp_path / "waypoints.csv" + plot_circular_path.save_waypoints_to_csv(waypoints, filename) + + with open(filename, mode="r", encoding="UTF-8") as file: + lines = file.readlines() + + assert lines[0].strip() == "Name,Latitude,Longitude,Altitude" + assert lines[1].strip() == "WP1,10,12,0" + assert lines[2].strip() == "WP2,10.0001,12.0001,0"