Convert UTM coordinates to the same zone #16

flight/avoidance/obstacle_avoidance.py

@@ -30,10 +30,10 @@ async def calculate_avoidance_velocity(
         The drone for which the path will be calculated for
     obstacle_data : list[InputPoint]
         Positions at previous times of the obstacle (probably another drone)
-    avoidance_radius : float
+    avoidance_radius : float = 10.0
         The distance between the drone and obstacle, in meters, at which
         obstacle avoidance will activate
-    avoidance_speed : float
+    avoidance_speed : float = 5.0
         The speed, in m/s, at which we should move away from the obstacle
 
     Returns
@@ -49,9 +49,6 @@ async def calculate_avoidance_velocity(
             f"got a length of {len(obstacle_data)}"
         )
 
-    # Convert obstacle data to list of Point objects
-    obstacle_positions: list[Point] = [Point.from_dict(in_point) for in_point in obstacle_data]
-
     # Get position of drone
     drone_position: mavsdk.telemetry.Position
     async for position in drone.telemetry.position():
@@ -61,16 +58,27 @@ async def calculate_avoidance_velocity(
     # Convert drone position to Point object
     drone_position: Point = Point.from_mavsdk_position(drone_position)  # type: ignore
 
-    # TODO: Make the function work if UTM zones differ
+    # Convert obstacle data to list of Point objects
+    obstacle_positions: list[Point] = [
+        Point.from_dict(
+            in_point,
+            force_zone_number=drone_position.utm_zone_number,
+            force_zone_letter=drone_position.utm_zone_letter,
+        )
+        for in_point in obstacle_data
+    ]
+
     # Check if all positions are in the same UTM zone
+    # Should only error if the utm module isn't working properly
     point: Point
     for point in obstacle_positions:
         if (
             point.utm_zone_letter != drone_position.utm_zone_letter
             or point.utm_zone_number != drone_position.utm_zone_number
         ):
             raise ValueError(
-                "Points are in different UTM zones (Note: tell obstacle avoidance team to fix this)"
+                "Points are in different UTM zones\n"
+                "Note: this error should not occur. The utm module should have prevented this."
             )
 
     # Sort obstacle positions with respect to time

flight/avoidance/point.py

@@ -43,7 +43,12 @@ class Point:
     time: float
 
     @classmethod
-    def from_dict(cls, position_data: InputPoint) -> "Point":
+    def from_dict(
+        cls,
+        position_data: InputPoint,
+        force_zone_number: int | None = None,
+        force_zone_letter: str | None = None,
+    ) -> "Point":
         """
         Factory method accepting a dict with position data
 
@@ -52,17 +57,32 @@ def from_dict(cls, position_data: InputPoint) -> "Point":
         position_data : dict[str, Union[float, int, str]]
             A dict containing at least the following keys:
             'utm_x', 'utm_y', 'utm_zone_number', 'utm_zone_letter'
+        force_zone_letter : int | None = None
+            Forces the UTM zone letter of the resulting Point to a certain value
+        force_zone_number : int | None = None
+            Forces the UTM zone number of the resulting Point to a certain value
 
         Returns
         -------
         A new Point object
         """
 
+        utm_x: float = float(position_data["utm_x"])
+        utm_y: float = float(position_data["utm_y"])
+        utm_zone_number: int = int(position_data["utm_zone_number"])
+        utm_zone_letter: str = str(position_data["utm_zone_letter"])
+        if force_zone_number is not None or force_zone_letter is not None:
+            utm_x, utm_y, utm_zone_number, utm_zone_letter = utm.from_latlon(
+                *utm.to_latlon(utm_x, utm_y, utm_zone_number, utm_zone_letter),
+                force_zone_number=force_zone_number,
+                force_zone_letter=force_zone_letter
+            )
+
         return cls(
-            float(position_data["utm_x"]),
-            float(position_data["utm_y"]),
-            int(position_data["utm_zone_number"]),
-            str(position_data["utm_zone_letter"]),
+            utm_x,
+            utm_y,
+            utm_zone_number,
+            utm_zone_letter,
             float(position_data["altitude"]),
             float(position_data["time"]),
         )