Add functions to evaluate polynomials #16

flight/avoidance/obstacle_avoidance.py

@@ -4,6 +4,8 @@
 
 # pylint: disable=fixme
 
+from typing import Callable
+
 import mavsdk
 import mavsdk.telemetry
 import numpy as np
@@ -39,13 +41,13 @@ async def calculate_avoidance_path(
     obstacle_positions: list[Point] = [Point.from_dict(in_point) for in_point in obstacle_data]
 
     # Get position of drone
-    raw_drone_position: mavsdk.telemetry.Position
+    drone_position: mavsdk.telemetry.Position
     async for position in drone.telemetry.position():
-        raw_drone_position = position
+        drone_position = position
         break
 
     # Convert drone position to UTM Point
-    drone_position: Point = Point.from_mavsdk_position(raw_drone_position)
+    drone_position: Point = Point.from_mavsdk_position(drone_position)  # type: ignore
 
     # TODO: Make the function work if UTM zones differ
     # Check if all positions are in the same UTM zone
@@ -60,14 +62,14 @@ async def calculate_avoidance_path(
             )
 
     # Get velocity of drone
-    raw_drone_velocity: mavsdk.telemetry.VelocityNed
+    drone_velocity: mavsdk.telemetry.VelocityNed
     async for velocity in drone.telemetry.velocity_ned():
-        raw_drone_velocity = velocity
+        drone_velocity = velocity
         break
 
     # Convert drone position to Velocity object
     # Units don't change, only the type of the object
-    drone_velocity: Velocity = Velocity.from_mavsdk_velocityned(raw_drone_velocity)
+    drone_velocity: Velocity = Velocity.from_mavsdk_velocityned(drone_velocity)  # type: ignore
 
     obstacle_positions.sort(key=lambda p: p.time)
 
@@ -76,12 +78,6 @@ async def calculate_avoidance_path(
     # Create list of times
     obstacle_times: list[float] = [point.time for point in obstacle_positions]
 
-    # Get weights for polynomial regression
-    # The most recent point should have the highest weight
-    weights: range = range(
-        1, len(obstacle_times) + 1
-    )  # For some reason range objects can be reused
-
     # TODO: Research better models for predicting the obstacle's path
     # Use polynomial regression to model the obstacle's path
     # The polynomial is arr[0] * t**n + arr[1] * t**(n - 1) + ... + arr[n - 1] * t + arr[n]
@@ -90,28 +86,48 @@ async def calculate_avoidance_path(
             [point.utm_x for point in obstacle_positions],
             obstacle_times,
             polynomial_degree,
-            w=weights,
+            w=range(1, len(obstacle_times) + 1),
         )
     )
     y_polynomial: list[float] = list(
         np.polyfit(
             [point.utm_y for point in obstacle_positions],
             obstacle_times,
             polynomial_degree,
-            w=weights,
+            w=range(1, len(obstacle_times) + 1),
         )
     )
     altitude_polynomial: list[float] = list(
         np.polyfit(
             [point.altitude for point in obstacle_positions],
             obstacle_times,
             polynomial_degree,
-            w=weights,
+            w=range(1, len(obstacle_times) + 1),
         )
     )
 
+    def polynomial_evaluator(coefficients: list[float]) -> Callable[[float], float]:
+        def evaluate(val: float) -> float:
+            result: float = 0.0
+            coefficient: float
+            for coefficient in coefficients:
+                result *= val
+                result += coefficient
+            return result
+
+        return evaluate
+
+    predict_x: Callable[[float], float] = polynomial_evaluator(x_polynomial)
+    predict_y: Callable[[float], float] = polynomial_evaluator(y_polynomial)
+    predict_altitude: Callable[[float], float] = polynomial_evaluator(altitude_polynomial)
+
     # TODO: Do something useful with these variables
     print(x_polynomial, y_polynomial, altitude_polynomial, drone_velocity, avoidance_radius)
+    print(
+        predict_x(drone_position.time),
+        predict_y(drone_position.time),
+        predict_altitude(drone_position.time),
+    )
 
     raise NotImplementedError