Write code for offboard movement #16

flight/avoidance/__init__.py

@@ -1,5 +1,6 @@
 """
-Import relevant functions and classes from this module
+This package implements obstacle avoidance functions
+for a MAVSDK drone
 """
 
 from .avoidance_goto import goto_with_avoidance

flight/avoidance/__main__.py

@@ -11,7 +11,7 @@
 def main() -> None:
     """Run tests"""
 
-    with subprocess.Popen(["bash", "avoidance/opensitlmultiple.sh"]) as _:
+    with subprocess.Popen(["bash", "avoidance/opensitlmultiple.sh"]):
         pass
 
     asyncio.run(test.run())

flight/avoidance/avoidance_goto.py

@@ -7,6 +7,7 @@
 from asyncio import Task
 from typing import AsyncIterator
 
+import mavsdk
 import mavsdk.telemetry
 
 from .obstacle_avoidance import calculate_avoidance_velocity
@@ -26,7 +27,8 @@ async def goto_with_avoidance(
 ) -> None:
     """
     A goto function with obstacle avoidance intended to replace
-    goto_location from mavsdk
+    goto_location from MAVSDK. Unlike goto_location, this function
+    uses offboard methods.
 
     Parameters
     ----------
@@ -69,7 +71,7 @@ async def restart_goto() -> Task[None]:
 
             # Calculate avoidance velocity
             avoidance_velocity: Velocity | None = await calculate_avoidance_velocity(
-                drone, obstacle_positions, avoidance_radius, avoidance_speed, False
+                drone, obstacle_positions, avoidance_radius, avoidance_speed
             )
 
             # If no avoidance is needed, restart goto and continue

flight/avoidance/movetoward.py

@@ -0,0 +1,79 @@
+"""
+This module implements a function to tell the drone
+to starting moving toward a location
+"""
+
+import mavsdk
+import mavsdk.offboard
+import mavsdk.telemetry
+import utm
+
+from .vector import Vector3
+from .velocity import Velocity
+
+
+async def move_toward(
+    drone: mavsdk.System,
+    latitude: float,
+    longitude: float,
+    absolute_altitude_m: float,
+    time_seconds: float = 3.0,
+) -> mavsdk.offboard.VelocityNedYaw:
+    """
+    Causes the drone to start moving toward a location;
+    only sets the drone's velocity once, then returns
+
+    Parameters
+    ----------
+    drone : mavsdk.System
+        The drone to move
+    latitude : float
+        The latitude, in degrees
+    longitude : float
+        The longitude, in degrees
+    absolute_altitude_m : float
+        The altitude above mean sea level, in meters
+    time_seconds : float
+        Controls how quickly the drone will attempt to move
+        to the target; the drone may not be able to actually
+        move this quickly
+
+    Returns
+    -------
+    The value the drone's velocity was set to,
+    as a mavsdk.offboard.VelocityNewYaw object
+    """
+
+    # Get drone position
+    drone_position: mavsdk.telemetry.Position = await anext(drone.telemetry.position())
+
+    # Get drone and target utm positions
+    target_utm: tuple[float, float, int, str] = utm.from_latlon(latitude, longitude)
+    drone_utm: tuple[float, float, int, str] = utm.from_latlon(
+        drone_position.latitude_deg,
+        drone_position.longitude_deg,
+        force_zone_number=target_utm[2],
+        force_zone_letter=target_utm[3],
+    )
+
+    # Calculate difference in positions
+    position_diff: Vector3 = Vector3(
+        target_utm[0] - drone_utm[0],
+        target_utm[1] - drone_utm[1],
+        absolute_altitude_m - drone_position.absolute_altitude_m,
+    )
+
+    # Calculate target velocity based on time_seconds
+    target_velocity: Vector3 = position_diff / time_seconds
+
+    # Create Velocity object
+    result: mavsdk.offboard.VelocityNedYaw = Velocity(
+        target_velocity.y,  # north
+        target_velocity.x,  # east
+        -target_velocity.z,  # down
+    ).to_mavsdk_velocitynedyaw()
+
+    # Set drone's velocity
+    await drone.offboard.set_velocity_ned(result)
+
+    return result

flight/avoidance/obstacle_avoidance.py

@@ -19,7 +19,6 @@ async def calculate_avoidance_velocity(
     obstacle_data: list[InputPoint],
     avoidance_radius: float = 10.0,
     avoidance_speed: float = 5.0,
-    change_down_vel: bool = False,
 ) -> Velocity | None:
     """
     Given a drone and a moving obstacle, calculates a velocity at which the
@@ -36,8 +35,6 @@ async def calculate_avoidance_velocity(
         obstacle avoidance will activate
     avoidance_speed : float = 5.0
         The speed, in m/s, at which we should move away from the obstacle
-    change_down_vel : bool = False
-        Whether to change the vertical velocity
 
     Returns
     -------
@@ -141,10 +138,6 @@ async def calculate_avoidance_velocity(
 
     # Get the amount by which we should correct the drone's velocity
     correction_velocity: Velocity = desired_relative_velocity - relative_velocity
-    if not change_down_vel:
-        correction_velocity = Velocity(
-            correction_velocity.north_vel, correction_velocity.east_vel, 0.0
-        )
 
     # Get the velocity at which the drone should move to avoid the obstacle
     avoidance_velocity: Velocity = drone_velocity + correction_velocity

flight/avoidance/point.py

@@ -14,7 +14,7 @@
 InputPoint = dict[str, float | str]
 
 
-@dataclass(frozen=True, slots=True)
+@dataclass(slots=True)
 class Point:
     """
     A point in 3D space

flight/avoidance/test.py

@@ -9,14 +9,37 @@
 
 import mavsdk
 import mavsdk.core
+import mavsdk.offboard
 import mavsdk.telemetry
 
 from .avoidance_goto import goto_with_avoidance
 from .point import InputPoint, Point
 
-TAKEOFF_ALTITUDE: float = 20.0
-DEFAULT_SPREAD_LATLON: float = 5e-5
-DEFAULT_SPREAD_ALTITUDE: float = 8.0
+TAKEOFF_ALTITUDE: float = 100.0
+DEFAULT_SPREAD_LATLON: float = 1e-4
+DEFAULT_SPREAD_ALTITUDE: float = 10.0
+
+
+async def takeoff(drone: mavsdk.System, altitude: float) -> None:
+    """
+    Makes a drone take off and waits until it reaches the desired altitude
+
+    Parameters
+    ----------
+    drone : mavsdk.System
+        The drone that will take off
+    altitude : float
+        The minimum altitude, in meters, above the ground to take off to;
+        the actual altitude might be slightly higher
+    """
+
+    await drone.action.set_takeoff_altitude(altitude + 2.0)
+    await drone.action.takeoff()
+
+    # Temporary solution
+    # Originally attempted to use telemetry to detect when the desired
+    #   altitude was reached, but telemetry is broken in Gazebo
+    await asyncio.sleep(15.0)
 
 
 async def random_position(
@@ -78,7 +101,6 @@ async def drone_positions(drone: mavsdk.System) -> AsyncIterator[list[InputPoint
 
         positions.append(in_point)
 
-        print(positions)
         yield positions[:]
         await asyncio.sleep(1.0)
 
@@ -110,10 +132,16 @@ async def avoiding_drone_test(
             break
 
     await drone.action.arm()
-    await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE)
-    await drone.action.takeoff()
+    await takeoff(drone, TAKEOFF_ALTITUDE)
+    await drone.offboard.set_velocity_ned(mavsdk.offboard.VelocityNedYaw(0.0, 0.0, 0.0, 0.0))
 
-    await goto_with_avoidance(drone, 0.0, 0.0, 100.0, 0.0, position_updates)
+    # Randomly move drone
+    while True:
+        pos: tuple[float, float, float] = await random_position(
+            drone, DEFAULT_SPREAD_LATLON, DEFAULT_SPREAD_ALTITUDE
+        )
+        await goto_with_avoidance(drone, *pos, 0.0, position_updates)
+        await asyncio.sleep(4.0 * random.random() * random.random())
 
 
 async def drone_to_avoid_test(drone: mavsdk.System) -> None:
@@ -139,8 +167,9 @@ async def drone_to_avoid_test(drone: mavsdk.System) -> None:
             break
 
     await drone.action.arm()
-    await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE)
-    await drone.action.takeoff()
+    await takeoff(drone, TAKEOFF_ALTITUDE)
+    await drone.offboard.set_velocity_ned(mavsdk.offboard.VelocityNedYaw(0.0, 0.0, 0.0, 0.0))
+    await drone.offboard.start()
 
     # Randomly move drone
     while True:

flight/avoidance/vector/__init__.py

@@ -0,0 +1,5 @@
+"""
+This package implements various vector classes
+"""
+
+from .vector3 import Vector3

flight/avoidance/vector/vector3.py

@@ -0,0 +1,129 @@
+"""
+This module implements the Vector3 class
+"""
+
+from __future__ import annotations
+from dataclasses import dataclass
+import math
+from typing import Iterator, overload
+
+
+@dataclass(slots=True)
+class Vector3:
+    """
+    A 3D vector
+
+    Attributes
+    ----------
+    x : float
+        The x component of this vector
+    y : float
+        The y component of this vector
+    z : float
+        The z component of this vector
+    length: float
+    """
+
+    x: float
+    y: float
+    z: float
+
+    @overload
+    def __init__(self, x: float, y: float, z: float):
+        ...
+
+    @overload
+    def __init__(self, x: float):
+        ...
+
+    def __init__(self, x: float, y: float | None = None, z: float | None = None):
+        self.x = x
+        self.y = x if y is None else y
+        self.z = x if z is None else z
+
+    def __hash__(self) -> int:
+        big_hash: int = ((hash(self.x) * 3) + hash(self.y)) * 3 + hash(self.z)
+        return big_hash & 0xFFFF_FFFF_FFFF_FFFF
+
+    # Implement unpacking
+    def __iter__(self) -> Iterator[float]:
+        return iter((self.x, self.y, self.z))
+
+    # Implement **kwargs unpacking
+
+    def keys(self) -> list[str]:
+        """
+        Gets a list of keys for **kwargs unpacking
+
+        Returns
+        -------
+        The list ['x', 'y', 'z']
+        """
+        return ["x", "y", "z"]
+
+    def __getitem__(self, key: str) -> float:
+        match key:
+            case "x":
+                return self.x
+            case "y":
+                return self.y
+            case "z":
+                return self.z
+            case _:
+                raise KeyError(f"{key} is not a valid key of {type(self).__name__}")
+
+    @property
+    def length(self) -> float:
+        """
+        Calculates the magnitude of this vector
+
+        Returns
+        -------
+        The magnitude of this vector
+        """
+        return math.hypot(self.x, self.y, self.z)
+
+    def normalized(self) -> Vector3:
+        """
+        Creates a normalized version of this vector
+
+        Returns
+        -------
+        A vector with the same direction as this vector
+        and a magnitude of 1.0 (within floating-point error)
+        """
+        return self / self.length
+
+    def __neg__(self) -> Vector3:
+        return Vector3(-self.x, -self.y, -self.z)
+
+    def __add__(self, rhs: Vector3) -> Vector3:
+        return Vector3(
+            self.x + rhs.x,
+            self.y + rhs.y,
+            self.z + rhs.z,
+        )
+
+    def __sub__(self, rhs: Vector3) -> Vector3:
+        return self + -rhs
+
+    def __mul__(self, rhs: Vector3 | float) -> Vector3:
+        if isinstance(rhs, Vector3):
+            return Vector3(
+                self.x * rhs.x,
+                self.y * rhs.y,
+                self.z * rhs.z,
+            )
+        return Vector3(self.x * rhs, self.y * rhs, self.z * rhs)
+
+    def __rmul__(self, lhs: float) -> Vector3:
+        return self * lhs
+
+    def __truediv__(self, rhs: Vector3 | float) -> Vector3:
+        if isinstance(rhs, Vector3):
+            return Vector3(
+                self.x / rhs.x,
+                self.y / rhs.y,
+                self.z / rhs.z,
+            )
+        return Vector3(self.x / rhs, self.y / rhs, self.z / rhs)