def update(self) -> None:
position = self._vision_service.get_robot()
robot_position = Position(position.coordinate.to_centimeters(),
position.orientation)
data = robot_position.serialize()
self._communication_service.send_message(
Message("position", position=data))
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 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 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 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 get_robot(self) -> Position:
if not self._initialized.is_set():
raise VisionServiceNotInitialized
self._robot, robot_orientation = self._robot_finder.find(self._image)
robot_coord = self._camera_calibration.convert_robot_pixel_to_real(
self._robot)
logger.debug('[get_robot] x: {}, y: {}, orientation: {}'.format(
robot_coord.x, robot_coord.y, robot_orientation))
return Position(robot_coord, robot_orientation)
def get_goal(self) -> Position:
if not self._initialized.is_set():
raise VisionServiceNotInitialized
self._goal, goal_orientation = self._goal_finder.find(self._image)
goal_coord = self._camera_calibration.convert_table_pixel_to_real(
self._goal.get_center())
logger.info('[get_goal] x: {}, y: {}, orientation: {}'.format(
goal_coord.x, goal_coord.y, goal_orientation))
return Position(goal_coord, goal_orientation)
def update(self) -> None:
new_image = self._vision_service.get_image()
new_path = self._path_service.get_path()
image_with_path = self._draw_on(new_image, new_path)
robot_position = self._vision_service.get_robot()
robot_position_in_centimeters = Position(
robot_position.coordinate.to_centimeters(),
robot_position.orientation)
self._current_path_image = self._draw_robot_on(
image_with_path, robot_position_in_centimeters)
self._notify()
def source(self) -> Table:
with self._populating:
if not self._got_source.is_set():
self._set_layout()
for i in range(3):
try:
position = self._vision_service.get_source()
break
except SourceCouldNotBeFound:
logger.info("SourceCouldNotBeFound")
source = Position(position.coordinate.to_centimeters(),
position.orientation)
approach_position = self._approach_position_finder.calculate_from(
source)
self._source = self._pathfinder.pathable_to(approach_position)
self._got_source.set()
return self._source
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)
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)