-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
2 changed files
with
59 additions
and
55 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,56 @@ | ||
|
||
from dataclasses import dataclass, field | ||
from typing import List | ||
import numpy as np | ||
from robot_sf.nav.nav_types import Vec2D, Line2D | ||
|
||
@dataclass | ||
class Obstacle: | ||
""" | ||
A class to represent an obstacle. | ||
Attributes | ||
---------- | ||
vertices : List[Vec2D] | ||
The vertices of the obstacle. | ||
lines : List[Line2D] | ||
The lines that make up the obstacle. This is calculated in the post-init method. | ||
vertices_np : np.ndarray | ||
The vertices of the obstacle as a numpy array. This is calculated in the post-init method. | ||
Methods | ||
------- | ||
__post_init__(): | ||
Validates and processes the vertices to create the lines and vertices_np attributes. | ||
""" | ||
|
||
vertices: List[Vec2D] | ||
lines: List[Line2D] = field(init=False) | ||
vertices_np: np.ndarray = field(init=False) | ||
|
||
def __post_init__(self): | ||
""" | ||
Validates and processes the vertices to create the lines and vertices_np | ||
attributes. | ||
Raises a ValueError if no vertices are specified. | ||
""" | ||
|
||
if not self.vertices: | ||
raise ValueError('No vertices specified for obstacle!') | ||
|
||
# Convert vertices to numpy array | ||
self.vertices_np = np.array(self.vertices) | ||
|
||
# Create edges from vertices | ||
edges = list(zip(self.vertices[:-1], self.vertices[1:])) \ | ||
+ [(self.vertices[-1], self.vertices[0])] | ||
|
||
# Remove fake lines that are just points | ||
edges = list(filter(lambda l: l[0] != l[1], edges)) | ||
|
||
# Create lines from edges | ||
lines = [(p1[0], p2[0], p1[1], p2[1]) for p1, p2 in edges] | ||
self.lines = lines | ||
|
||
if not self.vertices: | ||
print('WARNING: obstacle is just a single point that cannot collide!') |