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)
