Skip to content

Commit

Permalink
Cleanup obstacle_avoidance.py #16
Browse files Browse the repository at this point in the history
  • Loading branch information
RexBerry committed Oct 11, 2022
1 parent 80f2efd commit 0067b45
Showing 1 changed file with 17 additions and 20 deletions.
37 changes: 17 additions & 20 deletions flight/avoidance/obstacle_avoidance.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,6 @@
# pylint: disable=fixme

from dataclasses import dataclass
from typing import Optional
from typing import Union

from mavsdk import System
from mavsdk.telemetry import Position as MavsdkPosition
Expand All @@ -16,8 +14,7 @@

# Input points are dicts with time and UTM coordinate data
# May change in the future
# Optional[float] for time, allows type to be used for points without time
InputPoint = dict[str, Union[float, int, str, Optional[float]]]
InputPoint = dict[str, float | int | str]


# TODO: Maybe add a time component
Expand Down Expand Up @@ -45,12 +42,12 @@ class Point:
utm_y: float
utm_zone_number: int
utm_zone_letter: str
altitude: Optional[float]
altitude: float | None

@classmethod
def from_dict(cls, position_data: InputPoint) -> "Point":
"""
Converts a dict with position data to a Point object
Factory method accepting a dict with position data
Parameters
----------
Expand All @@ -63,7 +60,6 @@ def from_dict(cls, position_data: InputPoint) -> "Point":
A new Point object
"""

# TODO: Fix pylint errors
return cls(
float(position_data["utm_x"]),
float(position_data["utm_y"]),
Expand All @@ -75,7 +71,7 @@ def from_dict(cls, position_data: InputPoint) -> "Point":
@classmethod
def from_mavsdk_position(cls, position: MavsdkPosition) -> "Point":
"""
Converts a position data dict to a Point object
Factory method accepting a mavsdk.telemetry.Position object
Parameters
----------
Expand All @@ -87,14 +83,21 @@ def from_mavsdk_position(cls, position: MavsdkPosition) -> "Point":
A new Point object
"""

# TODO: Fix pylint errors
return cls(*utm.from_latlon(position.latitude_deg, position.longitude_deg), None)
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 argument return value because mypy complains
return cls(easting, northing, zone_number, zone_letter, None)


async def calculate_avoidance_path(
drone: System,
obstacle_data: list[InputPoint],
position: Optional[MavsdkPosition],
avoidance_radius: float = 10.0,
) -> list[Point]:
"""
Expand All @@ -106,11 +109,8 @@ async def calculate_avoidance_path(
The drone for which the path will be calculated for
obstacle_data : list[InputPoint]
Previously known positions
position : Optional[Position] = None
The position of the drone
The current position of the drone will be used if not provided
avoidance_radius : float
The radius of the sphere, in meters, from the predicted center of the obstacle that the drone should avoid
The radius around the predicted center of the obstacle the drone should avoid
Returns
-------
Expand All @@ -120,12 +120,9 @@ async def calculate_avoidance_path(

# Get position of drone
raw_drone_position: MavsdkPosition
if position is None:
async for position in drone.telemetry.position():
raw_drone_position = position
break
else:
async for position in drone.telemetry.position():
raw_drone_position = position
break

# Convert drone position to UTM Point
drone_position: Point = Point.from_mavsdk_position(raw_drone_position)
Expand Down

0 comments on commit 0067b45

Please sign in to comment.