def _grab_puck(self, puck: Puck, is_puck_in_a_corner: bool):
puck_position = puck.get_position()
if self._puck_is_close_to_center_middle(puck):
self._move_robot(
[
Movement(
Direction.BACKWARDS,
Distance(0.2, unit_of_measure=UnitOfMeasure.METER),
)
]
)
robot_pose = self._find_robot_pose()
orientation_to_puck = self._find_orientation_to_puck(puck_position, robot_pose)
self._rotation_service.rotate(orientation_to_puck)
distance_to_puck = (
Distance(0.2, unit_of_measure=UnitOfMeasure.METER)
if is_puck_in_a_corner
else Distance(0.1, unit_of_measure=UnitOfMeasure.METER)
)
movements_to_puck = [Movement(Direction.FORWARD, distance_to_puck)]
print(
movements_to_puck[0].get_direction(),
movements_to_puck[0].get_distance().get_distance(),
)
self._move_robot(movements_to_puck)
self._send_command_to_robot(
Topic.GRAB_PUCK, puck.get_color().get_resistance_digit()
)
self._wait_for_robot_confirmation(Topic.GRAB_PUCK)
def _detect_pucks(self, table_image) -> List[Puck]:
pucks = list()
for color in Color:
if color == Color.NONE:
continue
puck_position = self._puck_detector.detect(table_image, color)
pucks.append(Puck(color, puck_position))
return pucks
def _add_puck_as_obstacle(self, starting_zone_corner_index: int, puck: Puck):
maze = GameState.get_instance().get_game_table().get_maze()
starting_zone = GameState.get_instance().get_game_table().get_starting_zone()
current_corner = GameState.get_instance().get_starting_zone_corners()[
starting_zone_corner_index
]
corner_position = starting_zone.find_corner_position_from_letter(current_corner)
maze.add_puck_as_obstacle(corner_position)
maze.remove_puck_as_obstacle(puck.get_position())
class TestGameTable(TestCase):
A_STARTING_ZONE = MagicMock()
A_MAZE = MagicMock()
A_PUCK_ZONE_CENTER = Position(100, 100)
A_PUCK_LIST = [
Puck(color, Position(200, 200)) for color in Color
if color != Color.NONE
]
def setUp(self) -> None:
self.game_table = GameTable(self.A_STARTING_ZONE, self.A_MAZE,
self.A_PUCK_ZONE_CENTER, self.A_PUCK_LIST)
def test_givenRedColor_whenGetPuck_thenReturnRedPuck(self):
red_color = Color.RED
actual_puck = self.game_table.get_puck(red_color)
self.assertEqual(actual_puck.get_color(), red_color)
def test_givenGreenColor_whenGetPuck_thenReturnGreenPuck(self):
green_color = Color.GREEN
actual_puck = self.game_table.get_puck(green_color)
self.assertEqual(actual_puck.get_color(), green_color)
def _is_puck_in_a_corner(self, puck: Puck) -> bool:
pos_x, pos_y = puck.get_position().to_tuple()
if pos_x > 1100:
return pos_y < 250 or pos_y > 575
return False
def _puck_is_close_to_center_middle(self, puck: Puck):
return puck.get_position().get_x_coordinate() < 940
from domain.pathfinding.MazeFactory import MazeFactory
from infra.vision.TemplateMatchingPuckDetector import TemplateMatchingPuckDetector
if __name__ == "__main__":
context = StationContext(True)
vision_service = context._vision_service
table_image, _ = vision_service.get_vision_state()
second_table_image, _ = vision_service.get_vision_state()
detector = TemplateMatchingPuckDetector()
image_width, image_height, _ = table_image.shape
first_obstacle_position = Position(280, 125)
second_obstacle_position = Position(402, 184)
pucks = [
Puck(color, detector.detect(table_image, color)) for color in Color
if color is not Color.NONE
]
maze = MazeFactory(
ROBOT_RADIUS, OBSTACLE_RADIUS
).create_from_shape_and_obstacles_and_pucks_as_obstacles(
table_image.shape, [first_obstacle_position, second_obstacle_position],
pucks)
maze_array = maze._array
img_to_show = np.zeros((image_width, image_height, 3))
for i, column in enumerate(maze_array):
for j, row in enumerate(column):
if maze_array[i][j] == 1: