def test_helper(x, y, other_x, other_y, expected_distance):
p1 = geometry.Point(x, y)
p2 = geometry.Point(other_x, other_y)
d1 = p1.distance_to(p2)
d2 = p2.distance_to(p1)
self.assertAlmostEqual(d1, d2)
self.assertAlmostEqual(d1, expected_distance)
def get_world_borders(self) -> Tuple[geometry.Point, geometry.Point]:
if len(self.map) == 0:
return None, None
x_min, y_min = np.Inf, np.Inf
x_max, y_max = np.NINF, np.NINF
for key, entry in self.map.items():
x_coords = [o.location.x for o in entry] + [key.x]
x_min = min(x_min, min(x_coords))
x_max = max(x_max, max(x_coords))
y_coords = [o.location.y for o in entry] + [key.y]
y_min = min(y_min, min(y_coords))
y_max = max(y_max, max(y_coords))
return (geometry.Point(x_min, y_min), geometry.Point(x_max, y_max))
def get_unknown_locations(self) \
-> datapoint.Frontier:
"""
Finds locations where the search can be continued (locations free of
obstacles that are near to locations with unknown occupancy).
TODO: Make selection more precize.
"""
grid = self.observed_world.last_prediction_blurred
y_shape, x_shape = grid.shape
frontier = []
min_border, _ = self.observed_world.get_world_borders()
for yi in range(y_shape):
for xi in range(x_shape):
x = min_border.x + xi
y = min_border.y + yi
p = geometry.Point(x, y)
if grid[yi][xi] >= 0:
continue
if grid[yi][xi] < -10:
continue # Enough evidence that here is a free spot
if not self.observed_world.is_surrrounding_free(
p, radius=int(self.robot_size / 2), threshold=1.0):
continue
u = self.observed_world.perc_unknown_surround(
p, radius=int(self.robot_size / 2))
if u < 0.3:
continue
frontier.append(p)
return datapoint.Frontier(min_border.x, min_border.y, frontier)
def apply_function_on_path(grid: np.ndarray, x_start: int, y_start: int,
x_end: int, y_end: int,
f: Callable[[float], float]) -> np.ndarray:
"""
Apply function f on the path from (x_start, y_start) to (x_end, y_end).
"""
p = geometry.Pose(x_start, y_start)
p_end = geometry.Point(x_end, y_end)
p.turn_towards(p_end)
x_old = x_start
y_old = y_start
while p.position.distance_to(p_end) > 0.5:
x = int(round(p.position.x))
y = int(round(p.position.y))
p.move_forward(1)
if x == x_old and y == y_old:
continue
grid[y][x] = f(grid[y][x])
x_old = x
y_old = y
return grid
def test_getitem(self):
p = geometry.Point(3, 5)
self.assertEqual(p[0], p.x)
self.assertEqual(p[1], p.y)
with self.assertRaises(TypeError):
p["not integer"]
for index in [-1, 2]:
with self.assertRaises(IndexError):
p[index]
def select_from_frontier(self,
frontier: datapoint.Frontier,
current_pose: geometry.Pose,
select_randomly: bool = False) -> geometry.Point:
"""
Selects a point to be visited next.
TODO: Take orientation in consideration.
"""
if len(frontier) == 0:
return None
num_points = len(frontier)
distances = np.zeros([num_points, num_points])
for i in range(num_points):
p = frontier[i].location
for j in range(i + 1, num_points):
d = p.distance_to(frontier[j].location)
distances[i][j] = d
distances[j][i] = d
# Choose between points that have some neighbouring points
count = [
sum(1 for d in distances[i] if d < self.distance_tollerance)
for i in range(num_points)
]
candidates = [p for (i, p) in enumerate(frontier) if count[i] >= 3]
if len(candidates) == 0:
return None
if select_randomly:
index = random.randint(0, len(candidates) - 1)
chosen = candidates[index]
logging.info(f"Selected goal: {chosen.location} "
f"(selected randomly)")
else:
chosen = min(candidates,
key=lambda p: geometry.Point(*p.location).distance_to(
current_pose.position))
logging.info(f"Selected goal: {chosen.location}")
return geometry.Point(*chosen.location)
def __init__(self,
x,
y,
color=None,
path_id: int = None,
path_style: str = "-",
existence: Existence = Existence.PERMANENT):
self.location = geometry.Point(x, y)
self.color = color if color is not None else (0., 0., 0., 0.3)
self.graph_type = enums.GraphType.SCATTER
self.path_id = path_id
self.path_style = path_style
self.existence = existence
def test_plus_polar(self):
cart = geometry.Point(3, 5)
def test_helper(angle, radius, expected_x, expected_y):
polar = geometry.Polar(angle, radius)
result = cart.plus_polar(polar)
self.assertAlmostEqual(result.x, expected_x)
self.assertAlmostEqual(result.y, expected_y)
test_helper(0, 1, 4, 5)
test_helper(90, 1, 3, 6)
test_helper(180, 4, -1, 5)
test_helper(-90, 2, 3, 3)
test_helper(45, np.sqrt(2), 4, 6)
test_helper(120, 3, 3 + 3 * np.cos(np.radians(120)),
5 + 3 * np.sin(np.radians(120)))
test_helper(-120, 3, 1.5, 5 + 3 * np.sin(np.radians(-120)))
def get_random_point(self, min_value: float = np.NINF,
max_value: float = np.Inf, blurred=True) \
-> geometry.Point:
"""
Returns a random point with value between min_value and max_value.
Returns None if there is no such a value.
"""
if blurred:
grid = self.last_prediction_blurred
else:
grid = self.last_prediction
candidates = np.argwhere(
np.logical_and(grid >= min_value, grid <= max_value))
if len(candidates) == 0:
return None
min_border, max_border = self.get_world_borders()
index = random.randint(0, len(candidates) - 1)
y, x = candidates[index]
new_x = min(min_border.x + x, max_border.x)
new_y = min(min_border.y + y, max_border.y)
return geometry.Point(new_x, new_y)
def test_str(self):
p = geometry.Point(3, 5)
self.assertEqual(str(p), "(3.00, 5.00)")
def test_helper(x, y, angle, remote_x, remote_y, expected_angle):
p = geometry.Pose(x, y, angle)
point = geometry.Point(remote_x, remote_y)
p.turn_towards(point)
self.assertAlmostEqual(p.orientation.in_degrees(), expected_angle)
def test_add(self):
p1 = geometry.Point(3, 5)
p2 = geometry.Point(2, 1)
p3 = p1 + p2
self.assertEqual(p3.x, 5)
self.assertEqual(p3.y, 6)
def test_eq(self):
p1 = geometry.Point(3, 5)
p2 = geometry.Point(3, 5)
self.assertEqual(p1, p2)
def test_helper(x, y, angle, remote_x, remote_y, expected_angle):
p = geometry.Pose(x, y, angle)
point = geometry.Point(remote_x, remote_y)
a = p.angle_to_point(point)
self.assertAlmostEqual(a.in_degrees(), expected_angle)
def test_helper(x, y, other_x, other_y, expected_angle):
p1 = geometry.Point(x, y)
p2 = geometry.Point(other_x, other_y)
a = p1.angle_to(p2).in_degrees()
self.assertAlmostEqual(a, expected_angle)
def test_creation(self):
p = geometry.Point(3, 5)
self.assertEqual(p.x, 3)
self.assertEqual(p.y, 5)
def test_eq_float(self):
p1 = geometry.Point(3.00000005, 5.000000001)
p2 = geometry.Point(3, 5)
self.assertEqual(p1, p2)
def test_change(self):
p = geometry.Point(3, 5)
p.change(2.5, 1.1)
self.assertAlmostEqual(p.x, 5.5)
self.assertAlmostEqual(p.y, 6.1)
def test_change_negative(self):
p = geometry.Point(3, 5)
p.change(-5.0, -0.5)
self.assertAlmostEqual(p.x, -2)
self.assertAlmostEqual(p.y, 4.5)
