Skip to content

Commit

Permalink
Move Point and Velocity classes to separate files #16
Browse files Browse the repository at this point in the history
  • Loading branch information
RexBerry committed Oct 20, 2022
1 parent fcc18c2 commit bd88931
Show file tree
Hide file tree
Showing 3 changed files with 139 additions and 124 deletions.
126 changes: 2 additions & 124 deletions flight/avoidance/obstacle_avoidance.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,134 +4,12 @@

# pylint: disable=fixme

from dataclasses import dataclass

import mavsdk
import mavsdk.telemetry
import numpy as np
import utm

# Input points are dicts with time and UTM coordinate data
# May change in the future
InputPoint = dict[str, float | int | str]


# TODO: Maybe move to a different file in this directory
@dataclass
class Point:
"""
A point in 3D space
Attributes
----------
utm_x : float
The x-coordinate of this point, in meters, in UTM coordinates
utm_y : float
The y-coordinate of this point, in meters, in UTM coordinates
utm_zone_number : int
The UTM zone this point is in
utm_zone_letter : str
The letter of the UTM latitude band
altitude : float
The altitude of the point above sea level, in meters
time : float | None
The time at which an object was at this point, in Unix time
"""

utm_x: float
utm_y: float
utm_zone_number: int
utm_zone_letter: str
altitude: float
time: float | None

@classmethod
def from_dict(cls, position_data: InputPoint) -> "Point":
"""
Factory method accepting a dict with position data
Parameters
----------
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'
Returns
-------
A new Point object
"""

return cls(
float(position_data["utm_x"]),
float(position_data["utm_y"]),
int(position_data["utm_zone_number"]),
str(position_data["utm_zone_letter"]),
float(position_data["altitude"]), # TODO: Change key to actual key in data input
float(position_data["time"]),
)

@classmethod
def from_mavsdk_position(cls, position: mavsdk.telemetry.Position) -> "Point":
"""
Factory method accepting a mavsdk.telemetry.Position object
Parameters
----------
position : mavsdk.telemetry.Position
A position from MAVSDK
Returns
-------
A new Point object
"""

easting: float
northing: float
zone_number: int
zone_letter: str
easting, northing, zone_number, zone_letter = utm.from_latlon(
position.latitude_deg, position.longitude_deg
)

# Can't directly unpack function return value because mypy complains
return cls(easting, northing, zone_number, zone_letter, position.absolute_altitude_m, None)


@dataclass
class Velocity:
"""
A velocity in 3D space
Attributes
----------
north_vel : float
Eastward velocity in meters per second
east_vel : float
Northward velocity in meters per second
down_vel : float
Downward velocity in meters per second
"""

north_vel: float
east_vel: float
down_vel: float

@classmethod
def from_mavsdk_velocityned(cls, velocity: mavsdk.telemetry.VelocityNed) -> "Velocity":
"""
Factory method accepting a mavsdk.telemetry.VelocityNed object
Parameters
----------
velocity : mavsdk.telemetry.VelocityNed
A velocity (NED) from MAVSDK
Returns
-------
A new Velocity object
"""

return cls(velocity.north_m_s, velocity.east_m_s, velocity.down_m_s)
from point import Point, InputPoint
from velocity import Velocity


async def calculate_avoidance_path(
Expand Down
93 changes: 93 additions & 0 deletions flight/avoidance/point.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
"""
Defines and implements the Point class used in obstacle_avoidance.py
"""

from dataclasses import dataclass

import mavsdk.telemetry

import utm

# Input points are dicts with time and UTM coordinate data
# May change in the future
InputPoint = dict[str, float | int | str]


@dataclass
class Point:
"""
A point in 3D space
Attributes
----------
utm_x : float
The x-coordinate of this point, in meters, in UTM coordinates
utm_y : float
The y-coordinate of this point, in meters, in UTM coordinates
utm_zone_number : int
The UTM zone this point is in
utm_zone_letter : str
The letter of the UTM latitude band
altitude : float
The altitude of the point above sea level, in meters
time : float | None
The time at which an object was at this point, in Unix time
"""

utm_x: float
utm_y: float
utm_zone_number: int
utm_zone_letter: str
altitude: float
time: float | None

@classmethod
def from_dict(cls, position_data: InputPoint) -> "Point":
"""
Factory method accepting a dict with position data
Parameters
----------
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'
Returns
-------
A new Point object
"""

return cls(
float(position_data["utm_x"]),
float(position_data["utm_y"]),
int(position_data["utm_zone_number"]),
str(position_data["utm_zone_letter"]),
float(position_data["altitude"]),
float(position_data["time"]),
)

@classmethod
def from_mavsdk_position(cls, position: mavsdk.telemetry.Position) -> "Point":
"""
Factory method accepting a mavsdk.telemetry.Position object
Parameters
----------
position : mavsdk.telemetry.Position
A position from MAVSDK
Returns
-------
A new Point object
"""

easting: float
northing: float
zone_number: int
zone_letter: str
easting, northing, zone_number, zone_letter = utm.from_latlon(
position.latitude_deg, position.longitude_deg
)

# Can't directly unpack function return value because mypy complains
return cls(easting, northing, zone_number, zone_letter, position.absolute_altitude_m, None)
44 changes: 44 additions & 0 deletions flight/avoidance/velocity.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
"""
Defines and implements the Velocity class used in obstacle_avoidance.py
"""

from dataclasses import dataclass

import mavsdk.telemetry


@dataclass
class Velocity:
"""
A velocity in 3D space
Attributes
----------
north_vel : float
Eastward velocity in meters per second
east_vel : float
Northward velocity in meters per second
down_vel : float
Downward velocity in meters per second
"""

north_vel: float
east_vel: float
down_vel: float

@classmethod
def from_mavsdk_velocityned(cls, velocity: mavsdk.telemetry.VelocityNed) -> "Velocity":
"""
Factory method accepting a mavsdk.telemetry.VelocityNed object
Parameters
----------
velocity : mavsdk.telemetry.VelocityNed
A velocity (NED) from MAVSDK
Returns
-------
A new Velocity object
"""

return cls(velocity.north_m_s, velocity.east_m_s, velocity.down_m_s)

0 comments on commit bd88931

Please sign in to comment.