def _convert_pixel_to_real(self, pixel: Coord, distance: float) -> Coord:
pixel_vector = np.array([pixel.x, pixel.y, 1]).transpose()
real_vector = self._camera_matrix_inverse.dot(pixel_vector)
real_vector = np.multiply(real_vector, distance).transpose()
return Coord(int(real_vector[0]), int(real_vector[1]))
def create(self, obstacles: List[Coord]) -> IPathfinder:
table = Table(self._table_template.height, self._table_template.width,
self._table_template[Coord(0, 0)],
self._table_template.exclusion_radius,
self._table_template.orientation,
self._table_template.border)
return GrassfirePathfinder(table, obstacles)
def test_when_get_goal_then_center_of_goal_and_orientation_are_returned_as_real_coordinate(
self) -> None:
self.initialiseService()
expected_coord = Coord(0, 0)
expected_angle = Angle(0)
self.goal_finder.find = Mock(return_value=(Rectangle(0, 0, 10, 8),
expected_angle))
self.camera_calibration.convert_table_pixel_to_real = Mock(
return_value=Coord(0, 0))
position = self.vision_service.get_goal()
actual_coord = position.coordinate
actual_angle = position.orientation
self.camera_calibration.convert_table_pixel_to_real.assert_called_with(
Coord(5, 4))
self.assertEqual(expected_coord, actual_coord)
self.assertEqual(expected_angle, actual_angle)
def __str__(self) -> str:
table_str = ""
for row in range(self.height):
line = ""
for column in range(self.width):
value = str(self[Coord(column, row)])
space = len(value)
line += " " * (4 - space) + value
table_str += line + "\n"
return table_str
def _add_border_to_obstacle_table(self) -> None:
for x in range(self._obstacle_table.width):
for y in range(self._obstacle_table.height):
cell = Coord(x, y)
if cell.x < self._obstacle_table.border or cell.x > self._obstacle_table.width\
- self._obstacle_table.border - 1:
self._obstacle_table[cell] = float("inf")
if cell.y < self._obstacle_table.border or cell.y > self._obstacle_table.height\
- self._obstacle_table.border - 1:
self._obstacle_table[cell] = float("inf")
def qr_code(self) -> Table:
with self._populating:
if not self._got_qr_code.is_set():
self._set_layout()
found_path = False
for position in [
Position(Coord(190, 40), Angle(pi)),
Position(Coord(170, 60), Angle(7 * pi / 8))
]:
try:
self._qr_code = self._pathfinder.pathable_to(position)
found_path = True
except CannotPathThereError:
continue
if not found_path:
raise CannotPathThereError
self._got_qr_code.set()
return self._qr_code
def home(self) -> Table:
with self._populating:
if not self._got_home.is_set():
self._set_layout()
home = Coord(55, 55)
self._home = self._pathfinder.pathable_to(
Position(home, Angle(0)))
self._got_home.set()
return self._home
def charge_station(self) -> Table:
with self._populating:
if not self._got_charge_station.is_set():
self._set_layout()
charge_station = Position(Coord(20, 80), Angle(0))
self._charge_station = self._pathfinder.pathable_to(
charge_station)
self._got_charge_station.set()
return self._charge_station
def _set_octogon_value_in_bounding_box(self, x_min, x_max, y_min, y_max, value) -> None:
first_value = self._exclusion_radius / 2
removed = 1
for row in range(y_min, y_max):
for column in range(x_min, x_max):
if x_min + first_value < column <= x_max - first_value:
cell = Coord(column, row)
try:
self[cell] = value
except KeyError:
pass
if first_value != 0:
removed += 1
if row >= y_max - removed:
first_value += 1
else:
first_value -= 1
def _interpolate_robot_position_from_corners(
self, corners: np.ndarray, corner_to_adjust_index: int,
pred_corner_index: int, next_corner_index: int) -> Coord:
x_to_adjust, y_to_adjust = CvRobotFinder._extract_corner_coordination(
corners, corner_to_adjust_index)
pred_x, pred_y = CvRobotFinder._extract_corner_coordination(
corners, pred_corner_index)
next_x, next_y = CvRobotFinder._extract_corner_coordination(
corners, next_corner_index)
x = x_to_adjust + self._ratio_distance_marker_size * (
x_to_adjust -
pred_x) + self._ratio_distance_marker_size * (x_to_adjust - next_x)
y = y_to_adjust + self._ratio_distance_marker_size * (
y_to_adjust -
pred_y) + self._ratio_distance_marker_size * (y_to_adjust - next_y)
return Coord(int(x), int(y))
def _get_adjacent_cells(self, cell: Coord, table: Table) -> List[Coord]:
possible_cells = [
Coord(cell.x, cell.y - 1),
Coord(cell.x + 1, cell.y - 1),
Coord(cell.x + 1, cell.y),
Coord(cell.x + 1, cell.y + 1),
Coord(cell.x, cell.y + 1),
Coord(cell.x - 1, cell.y + 1),
Coord(cell.x - 1, cell.y),
Coord(cell.x - 1, cell.y - 1)
]
valid_cells = []
for possible_cell in possible_cells:
try:
if table[possible_cell] < 0:
valid_cells.append(possible_cell)
except KeyError:
pass
return valid_cells
def bottom_right_corner(self) -> Coord:
return Coord(self._x + self.width, self._y + self.height)
def top_left_corner(self) -> Coord:
return Coord(self._x, self._y)
def get_center(self) -> Coord:
return Coord(int(self._x + self.width / 2), int(self._y + self.height / 2))
def _calculate_coords_with_orientation(self, position: Position):
new_x = position.coordinate.x + 10 * sin(position.orientation.radians)
new_y = position.coordinate.y - 10 * cos(position.orientation.radians)
return Coord(new_x, new_y)
def _get_coord_relative_to_table(self, image: Image,
coord: Coord) -> Coord:
width_image, height_image = image.size
return Coord(int(coord.x * width_image / TABLE_WIDTH),
int(coord.y * height_image / TABLE_HEIGHT))
class AdaptivePathableCatalog(IPathableCatalog):
charge_position_pos = Position(Coord(21, 80), Angle(0))
qr_codes_pos = [Position(Coord(190, 40), Angle(pi)), Position(Coord(170, 60), Angle(7 * pi / 8))]
home_pos = Position(Coord(55, 55), Angle(0))
def __init__(self, vision_service: VisionService, pathfinder_factory: IPathfinderFactory,
approach_position_finder: IApproachPositionFinder) -> None:
self._vision_service = vision_service
self._pathfinder_factory = pathfinder_factory
self._approach_position_finder = approach_position_finder
self._populating = Lock()
self._filled = Event()
@property
def home(self) -> Table:
with self._populating:
if not self._filled.is_set():
self._fill()
return self._home
@property
def charge_station(self) -> Table:
with self._populating:
if not self._filled.is_set():
self._fill()
return self.charge_station
@property
def qr_code(self) -> Table:
with self._populating:
if not self._filled.is_set():
self._fill()
return self._qr_code
@property
def goal(self) -> Table:
with self._populating:
if not self._filled.is_set():
self._fill()
return self._goal
@property
def source(self) -> Table:
with self._populating:
if not self._filled.is_set():
self._fill()
return self._source
def _fill(self) -> None:
obstacles = self._find_obstacles()
self._find_destinations()
pathfinder = self._pathfinder_factory.create(obstacles)
while not self._filled.is_set():
self._charge_station: Table = pathfinder.pathable_to(self.charge_position_pos)
found_path = False
for qr_code in self.qr_codes_pos:
try:
self._qr_code = pathfinder.pathable_to(qr_code)
found_path = True
except CannotPathThereError:
continue
if not found_path:
raise CannotPathThereError
self._home: Table = pathfinder.pathable_to(self.home_pos)
self._source: Table = pathfinder.pathable_to(self.source_pos)
self._goal: Table = pathfinder.pathable_to(self.goal_pos)
try:
assert self._charge_station[self.home_pos.coordinate] != -1 and self._charge_station[
self.home_pos.coordinate] != float("inf")
assert self._qr_code[self.home_pos.coordinate] != -1 and self._qr_code[
self.home_pos.coordinate] != float("inf")
assert self._source[self.home_pos.coordinate] != -1 and self._source[
self.home_pos.coordinate] != float("inf")
assert self._goal[self.home_pos.coordinate] != -1 and self._goal[
self.home_pos.coordinate] != float("inf")
self._filled.set()
except AssertionError:
self._pathfinder_factory.exclusion_radius -= 1
print(self._pathfinder_factory.exclusion_radius)
pathfinder = self._pathfinder_factory.create(obstacles)
def _find_obstacles(self) -> List[Coord]:
found_obstacles = False
count = 3
obstacles = []
while not found_obstacles and count > 0:
try:
self._vision_service.update()
obstacles = [obstacle.to_centimeters() for obstacle in self._vision_service.get_obstacles()]
found_obstacles = True
except VisionError:
count -= 1
return obstacles
def _find_destinations(self) -> None:
source = self._vision_service.get_source()
source = Position(source.coordinate.to_centimeters(), source.orientation)
self.source_pos = self._approach_position_finder.calculate_from(source)
goal = self._vision_service.get_goal()
goal = Position(goal.coordinate.to_centimeters(), goal.orientation)
self.goal_pos = self._approach_position_finder.calculate_from(goal)
def test_when_pathing_to_some_point_then_gives_the_right_map(self) -> None:
path = self.grassfire_pathfinder.pathable_to(Position(Coord(24, 24), Angle(0)))
self.assertEqual(path.data, EXPECTED_PATH)
def _adjust_real_to_table(self, real: Coord) -> Coord:
return Coord(real.x - self._table_real_origin.x,
real.y - self._table_real_origin.y)
def _get_adjacent_cells(self, cell: Coord, table: Table) -> List[Coord]:
possible_cells = [
Coord(cell.x, cell.y - 1),
Coord(cell.x + 1, cell.y - 1),
Coord(cell.x + 1, cell.y),
Coord(cell.x + 1, cell.y + 1),
Coord(cell.x, cell.y + 1),
Coord(cell.x - 1, cell.y + 1),
Coord(cell.x - 1, cell.y),
Coord(cell.x - 1, cell.y - 1)
]
valid_cells = []
for possible_cell in possible_cells:
try:
if table[possible_cell] < 0:
valid_cells.append(possible_cell)
except KeyError:
pass
return valid_cells
if __name__ == '__main__':
template_table = Table(30, 30, -1, 7, Angle(0), 1)
obstacles = [Coord(14, 14)]
grassfire_pathfinder = GrassfirePathfinder(template_table, obstacles)
dest_map = grassfire_pathfinder.pathable_to(Position(Coord(20, 20), Angle(0)))
print(dest_map)
def setUp(self) -> None:
table = Table(30, 30, -1, 5, Angle(0), 1)
self.grassfire_pathfinder = GrassfirePathfinder(table, [Coord(7, 7), Coord(17, 17)])
def deserialize(cls, data: str):
deserialized = loads(data)
return Movement(Coord.deserialize(deserialized["start"]),
Coord.deserialize(deserialized["stop"]))