def __init__(self, key: int): # Default w, h = 30, 50 o = [] p = geometry.Pose(5, 5, 90) if key == 1: w, h, = 40, 40 o = [(0, 10, 20, 39)] p = geometry.Pose(5, 5, 0) elif key == 2: w, h = 30, 60 o = [(15, 29, 20, 40)] p = geometry.Pose(20, 10, 0) elif key == 3: w, h = 50, 50 o = [(20, 30, 20, 30)] p = geometry.Pose(40, 40, 180) elif key == 4: w, h = 50, 50 o = [(0, 20, 20, 35), (40, 49, 0, 15)] elif key == 5: w, h = 50, 50 o = [(0, 15, 20, 35), (30, 35, 0, 15)] elif key == 6: w, h = 60, 20 o = [(20, 30, 0, 5)] elif key == 7: w, h = 110, 70 o = [(40, 41, 0, 40), (75, 76, 30, 69)] p = geometry.Pose(20, 20, 0) super().__init__(width=w, height=h, pose=p, obstacles=o)
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_rotate(self): p = geometry.Pose(5, 4, 45) p.rotate(50) self.assertAlmostEqual(p.orientation.in_degrees(), 95) p.rotate(300) self.assertAlmostEqual(p.orientation.in_degrees(), 35) p.rotate(-65) self.assertAlmostEqual(p.orientation.in_degrees(), -30)
def test_getitem(self): p = geometry.Pose(-5.12, 10.598, -50) self.assertEqual(p[0], p.position.x) self.assertEqual(p[1], p.position.y) self.assertEqual(p[2], p.orientation.in_degrees()) with self.assertRaises(TypeError): p["not integer"] for index in [-1, 3]: with self.assertRaises(IndexError): p[index]
def is_path_free(self, start: geometry.Point, end: geometry.Point, radius: int = 5, threshold: float = 0.0): pose = geometry.Pose(*start, 0) pose.turn_towards(end) while pose.position.distance_to(end) > radius: pose.move_forward(1.5 * radius) if not self.is_surrrounding_free( pose.position, radius=radius, threshold=threshold): return False return True
def get_distance_to_wall(self, measuring_angle: int) -> int: """ Very imperfect. """ angle = self.pose.orientation.in_degrees() + measuring_angle p = geometry.Pose(*self.pose.position, angle) distance = 0 x = int(round(p.position.x)) y = int(round(p.position.y)) while self.location_in_range(x, y) and self.map[y][x] == 0: p.move_forward(1) distance += 1 x = int(round(p.position.x)) y = int(round(p.position.y)) return distance
def __init__(self, data_queue: queue.Queue, origin: geometry.Pose = None, robot_size: float = 10.0, scanning_precision: int = 20, view_angle: int = 180, **kwargs): super().__init__(data_queue) self.pose = origin if origin else geometry.Pose(0, 0, 0) self.robot_size = robot_size self.scanning_precision = scanning_precision self.view_angle = view_angle self.observation_queue = queue.Queue() self.observed_world = oworld.ObservedWorld() self.init_planner() self.init_sensor() self.scanner.start() # Send initial position to the queue data = datapoint.Pose(*self.pose, path_id=PathId.ROBOT_HISTORY) self.data_queue.put(data)
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, 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_move_forward_diagonal(self): p = geometry.Pose(-4, 2, 135) p.move_forward(5) self.assertAlmostEqual(p.position.x, -4 + 5 * np.cos(np.radians(135))) self.assertAlmostEqual(p.position.y, 2 + 5 * np.sin(np.radians(135)))
def test_move_forward_left(self): p = geometry.Pose(-4, 2, 180) p.move_forward(5) self.assertAlmostEqual(p.position.x, -9) self.assertAlmostEqual(p.position.y, 2)
def test_move_forward_up(self): p = geometry.Pose(-4, 2, 90) p.move_forward(5) self.assertAlmostEqual(p.position.x, -4) self.assertAlmostEqual(p.position.y, 7)
def test_str(self): p = geometry.Pose(5.5555, 9.9999, 100) self.assertEqual(str(p), "(5.56, 10.00), 100.00°")
def test_creation(self): p = geometry.Pose(5.5, 9.9, 100) self.assertAlmostEqual(p.position.x, 5.5) self.assertAlmostEqual(p.position.y, 9.9) self.assertAlmostEqual(p.orientation.in_degrees(), 100)