def _compute_orientation(self, image_width, image_height) -> None:
goal_center = self._goal.get_center()
if self._goal.width_height_ratio > 1.0: # target is horizontal
if goal_center.y > image_height / 2: # target is on bottom
self._orientation = Angle(pi)
else:
self._orientation = Angle(0)
else: # target is vertical
if goal_center.x > image_width / 2: # target is on the right
self._orientation = Angle(pi / 2)
else:
self._orientation = Angle(3 * pi / 2)
def __init__(self, port: int, address: str, timeout: float) -> None:
self._play_area_finder = CvPlayAreaFinder()
self._vision_service = VisionService(
CvCameraFactory(),
CvCameraCalibrationFactory(self._play_area_finder),
CvImageDrawer(), self._play_area_finder, CvGoalFinder(),
CvSourceFinder(), CvObstacleFinder(), CvRobotFinder())
self._time_service = TimeService(PythonChronometer())
self._robot_camera_service = RobotCameraService()
self._robot_connector = SocketRobotConnector(port, address, timeout)
self._communication_service = CommunicationService(
self._robot_connector)
self._position_service = PositionService(self._vision_service,
self._communication_service)
self._light_service = LightService()
self._objective_service = ObjectiveService()
self._remote_control = RemoteControl(self._communication_service)
self._remote_service = RemoteService(self._remote_control)
self._subroutine_runner = RemoteSubroutineRunner(self._remote_service)
pathfinder_factory = GrassfirePathfinderFactory(
Table(111, 231, -1, 27, Angle(0), 15))
self._pathable_catalog = PathableCatalog(self._vision_service,
pathfinder_factory,
ApproachPositionFinder())
self._pathfinding_service = PathfindingService(self._pathable_catalog)
self._path_service = PathService()
self._path_drawing_service = PathDrawingService(
OpenCvDrawer(), self._vision_service, self._path_service)
self._prehensor_service = PrehensorService()
def _compute_orientation_between_corners(corners: np.ndarray,
first_index: int,
second_index: int) -> Angle:
fourth_x, fourth_y = CvRobotFinder._extract_corner_coordination(
corners, first_index)
first_x, first_y = CvRobotFinder._extract_corner_coordination(
corners, second_index)
return Angle(atan2(fourth_y - first_y, fourth_x - first_x) - (pi / 2))
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 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)
removed += 1
if row >= y_max - removed:
first_value += 1
else:
first_value -= 1
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 serialize(self) -> str:
return dumps({
"height": self.height,
"width": self.width,
"data": self.data,
"orientation": self.orientation.radians
})
if __name__ == "__main__":
t = Table(50, 50, -1, 5, Angle(0), 10)
print(t.serialize())
print(t)
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)
def setUp(self) -> None:
table = Table(30, 30, -1, 5, Angle(0), 1)
self.grassfire_pathfinder = GrassfirePathfinder(table, [Coord(7, 7), Coord(17, 17)])