From bd88931103be7730c5819fdc680d815bca3c47f8 Mon Sep 17 00:00:00 2001 From: RexBerry <59031902+RexBerry@users.noreply.github.com> Date: Thu, 20 Oct 2022 18:33:08 -0500 Subject: [PATCH] Move Point and Velocity classes to separate files #16 --- flight/avoidance/obstacle_avoidance.py | 126 +------------------------ flight/avoidance/point.py | 93 ++++++++++++++++++ flight/avoidance/velocity.py | 44 +++++++++ 3 files changed, 139 insertions(+), 124 deletions(-) create mode 100644 flight/avoidance/point.py create mode 100644 flight/avoidance/velocity.py diff --git a/flight/avoidance/obstacle_avoidance.py b/flight/avoidance/obstacle_avoidance.py index a1dcec16..6724e74d 100644 --- a/flight/avoidance/obstacle_avoidance.py +++ b/flight/avoidance/obstacle_avoidance.py @@ -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( diff --git a/flight/avoidance/point.py b/flight/avoidance/point.py new file mode 100644 index 00000000..acfe4b33 --- /dev/null +++ b/flight/avoidance/point.py @@ -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) diff --git a/flight/avoidance/velocity.py b/flight/avoidance/velocity.py new file mode 100644 index 00000000..e469e647 --- /dev/null +++ b/flight/avoidance/velocity.py @@ -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)