Plot circular path, save to CSV file (#72)

* Create plot_circular_path.py with tests

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,
+                ]
+            )

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}"

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"