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)