def test_givenPathWithPosition_whenAddVerticallyNonAdjacentPosition_thenThrowInvalidPositionException(
self, ):
path = Path([Position(1, 3)])
a_position = Position(1, 1)
with self.assertRaises(PositionNotAdjacentException):
path.add(a_position)
def _find_starting_zone_corners(self, image) -> List[Position]:
mask = self._prepare_mask(image)
contours, _ = cv2.findContours(mask, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
corners_list = []
for contour in contours:
contours_poly = cv2.approxPolyDP(contour, 3, True)
bounding_rect = cv2.boundingRect(contours_poly)
rect_X = bounding_rect[0]
rect_y = bounding_rect[1]
rect_width = bounding_rect[2]
rect_height = bounding_rect[3]
rectangle_area = rect_width * rect_height
aspect_ratio = rect_width / rect_height
delta = abs(1.0 - aspect_ratio)
epsilon = 0.2
if rectangle_area > AREA_MIN and delta < epsilon:
top_left_corner = Position(rect_X, rect_y)
corners_list.append(top_left_corner)
top_right_corner = Position(rect_X + rect_width, rect_y)
corners_list.append(top_right_corner)
bottom_left_corner = Position(rect_X, rect_y + rect_height)
corners_list.append(bottom_left_corner)
bottom_right_corner = Position(
rect_X + rect_width, rect_y + rect_height
)
corners_list.append(bottom_right_corner)
if len(corners_list) != 4:
raise StartingZoneCornersNotFound
return corners_list
def _find_shortest_path(self, start_position: Position,
goal_position: Position):
if self._maze is None:
raise InvalidMazeException
start = start_position.to_tuple()
goal = goal_position.to_tuple()
start_x, start_y = start
goal_x, goal_y = goal
size_x, size_y = self.get_dimensions()
if start_x not in range(0, size_x + 1) or start_y not in range(
0, size_y + 1):
raise InvalidStartPointException
if goal_x not in range(0, size_x + 1) or goal_y not in range(
0, size_y + 1):
raise InvalidDestinationException
if self._maze[start_x][start_y] == 1:
raise InvalidStartPointException
if self._maze[goal_x][goal_y] == 1:
raise InvalidDestinationException
came_from = self._a_star_search(start, goal)
path = self._reconstruct_path(came_from, start, goal)
return path
def detect(self, image) -> StartingZone:
corner_positions = [
Position(x, y) for x, y in STARTING_ZONE_CORNERS_POSITION
]
center_position = Position(STARTING_ZONE_CENTER_POSITION[0],
STARTING_ZONE_CENTER_POSITION[1])
return StartingZone(corner_positions, center_position)
def test_givenPathWithPosition_whenAddAdjacentPosition_thenPositionIsAddedToPath(
self, ):
path = Path([Position(1, 2)])
a_position = Position(1, 1)
path.add(a_position)
self.assertEqual(a_position, path[1])
def calculate_distance_between_two_positions(first_position: Position,
second_position: Position):
first_x, first_y = first_position.to_tuple()
second_x, second_y = second_position.to_tuple()
distance = math.sqrt((first_x - second_x)**2 + (first_y - second_y)**2)
return Distance(distance)
def test_givenPathWithTwoPositions_whenGetSecondItem_thenSecondPositionIsReturned(
self, ):
first_position = Position(1, 1)
second_position = Position(1, 2)
path = Path([first_position, second_position])
actual_position = path[1]
self.assertEqual(second_position, actual_position)
def test_givenPathWithTwoPositions_whenGettingLengthOfPath_thenReturnTwo(
self):
first_position = Position(1, 1)
second_position = Position(1, 2)
path = Path([first_position, second_position])
path_length = len(path)
self.assertEqual(2, path_length)
def find_path_to_command_panel_zone(self, robot_position: Position) -> Path:
position = self._game_table.get_puck_zone_center()
position = Position(
position.get_x_coordinate() - 45, position.get_y_coordinate()
)
return (
self._shortest_path_algorithm.find_shortest_path_with_cartesian_coordinates(
robot_position, position
)
)
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenSingleLinePath_whenCreateMovements_thenSingleMovementIsCreated(
self):
a_path = Path(
[Position(1, 0),
Position(1, 1),
Position(1, 2),
Position(1, 4)])
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
self.assertEqual(1, len(movements))
def calculate_angle_between_positions(
first_position: Position,
second_position: Position) -> Orientation:
first_x, first_y = first_position.to_tuple()
second_x, second_y = second_position.to_tuple()
angle = -math.atan2(second_y - first_y, second_x - first_x)
if angle < 0:
angle += 2 * math.pi
return Orientation.from_radian(angle)
def test_givenAnImageWithTwoObstacles_whenDetectObstacle_thenReturnCorrectObstaclePosition(
self, ):
expected_first_obstacle_position, expected_second_obstacle_position = [
Position(1100, 757),
Position(787, 311),
]
actual_obstacle_position = self.obstacle_detector.detect(self.AN_IMAGE)
self.assertEqual(expected_first_obstacle_position,
actual_obstacle_position[0])
self.assertEqual(expected_second_obstacle_position,
actual_obstacle_position[1])
def _get_robot_orientation_in_degree(self,
robot_marker_corner) -> Orientation:
marker_upper_left_corner = Position(
int(robot_marker_corner[0][0][0]),
int(robot_marker_corner[0][0][1]),
)
marker_bottom_left_corner = Position(
int(robot_marker_corner[0][3][0]),
int(robot_marker_corner[0][3][1]),
)
return self.calculate_angle_between_positions(
marker_bottom_left_corner, marker_upper_left_corner)
def _position_is_not_adjacent(self, position: Position):
if len(self) < 1:
return False
path_last_position = self[-1]
are_positions_adjacent_in_x = (
abs(position.get_x_coordinate() -
path_last_position.get_x_coordinate()) == 1)
are_positions_adjacent_in_y = (
abs(position.get_y_coordinate() -
path_last_position.get_y_coordinate()) == 1)
return not are_positions_adjacent_in_x != are_positions_adjacent_in_y
def test_givenAnImage_whenDetect_thenReturnCorrectStartingZone(self):
an_image = cv2.imread(self.AN_IMAGE)
expected_coordinates = [
Position(195, 814),
Position(626, 814),
Position(195, 378),
Position(626, 378),
]
expected_starting_zone_center = Position(508, 785)
expected_starting_zone = StartingZone(
expected_coordinates, expected_starting_zone_center
)
actual_starting_zone = self.detector.detect(an_image)
self.assertEqual(expected_starting_zone, actual_starting_zone)
def test_givenOrientationCloseToCardinalOrientationSouth_whenCreateMovements_thenReturnMovementWithRightDirection(
self, ):
a_close_orientation = Orientation(91)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
expected_direction = Direction.FORWARD
movement = self.movement_factory.create_movements(
a_path, a_close_orientation)[0]
self.assertEqual(movement.get_direction(), expected_direction)
def test_givenOrientationFarFromCardinalOrientation_whenCreateMovements_thenRaiseInvalidOrientationException(
self, ):
a_far_orientation = Orientation(188)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
creating_movements = lambda: self.movement_factory.create_movements(
a_path, a_far_orientation)
with self.assertRaises(InvalidOrientationException):
creating_movements()
def test_givenEmptyMazeAndRobotRadius_whenAddObstacle_thenRobotRadiusIsTakenIntoAccountWhenAddingObstacle(
self, ):
expected_maze = Maze(
np.array([
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
]))
obstacle_position = Position(13, 9)
actual_maze = self.maze_factory.create_from_shape((17, 20, 0))
actual_maze.add_obstacle(obstacle_position, self.A_ROBOT_RADIUS,
self.AN_OBSTACLE_RADIUS)
self.assertEqual(expected_maze, actual_maze)
def _reconstruct_path(
self,
came_from: Dict[GridLocation, GridLocation],
start: GridLocation,
goal: GridLocation,
) -> Path:
current: GridLocation = goal
path: List[Position] = []
while current != start:
current_y_coordinate, current_x_coordinate = current
path.append(Position(current_x_coordinate, current_y_coordinate))
current = came_from[current]
start_y_coordinate, start_x_coordinate = start
path.append(Position(start_x_coordinate, start_y_coordinate))
path.reverse()
return Path(path)
def find_path_to_ohmmeter(self, robot_position: Position) -> Path:
return (
self._shortest_path_algorithm.find_shortest_path_with_cartesian_coordinates(
robot_position,
Position(DEFAULT_OHMMETER_POSITION[0], DEFAULT_OHMMETER_POSITION[1]),
)
)
def test_givenStraightPathWithFivePoints_whenCreateMovements_thenASingleMovementWithDistanceFiveIsCreated(
self, ):
a_path = Path([
Position(1, 0),
Position(1, 1),
Position(1, 2),
Position(1, 4),
Position(1, 5),
])
expected_distance = Distance(distance=5)
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
movement_distance = movements[0].get_distance()
self.assertEqual(expected_distance, movement_distance)
class VisionWorker:
last_known_pose = RobotPose(
Position(STARTING_ZONE_CENTER_POSITION[0],
STARTING_ZONE_CENTER_POSITION[1]),
CardinalOrientation.WEST.value,
)
def __init__(self, vision_service: VisionService):
self._vision_service = vision_service
def run(self):
while True:
self.update_vision_state(self._vision_service)
time.sleep(0.1)
@staticmethod
def update_vision_state(vision_service: VisionService):
try:
table_image, robot_pose = vision_service.get_vision_state()
GameState.get_instance().set_table_image(table_image)
GameState.get_instance().set_robot_pose(robot_pose)
VisionWorker.last_known_pose = robot_pose
except RobotNotFoundException:
GameState.get_instance().set_robot_pose(
VisionWorker.last_known_pose)
def test_givenEmptyPath_whenAdd_thenPositionIsAddedToPath(self):
path = Path([])
a_position = Position(1, 1)
path.add(a_position)
self.assertEqual(a_position, path[0])
def test_givenAnImageRight_side_WhenDetect_ThenReturnXEqualZero(self, ):
expected_position: Position = Position(0, 27)
actual_position = self._command_panel_detector.detect_upper_left_corner(
self.A_RIGHT_SIDE_IMAGE_OF_COMMAND_PANEL)
self.assertEqual(actual_position, expected_position)
def test_givenAnImage_WhenDetect_ThenReturnTopLeftPanelPosition(self, ):
expected_position: Position = Position(429, 37)
actual_position = self._command_panel_detector.detect_upper_left_corner(
self.AN_IMAGE_WITH_COMMAND_PANEL)
self.assertEqual(actual_position, expected_position)
def _create_vision_service(self):
starting_zone_corners_detector = HardcodedStartingZoneDetector()
obstacle_detector = OpenCvObstacleDetector(
DICT_4X4_50,
OBSTACLE_ARUCO_MARKER_ID,
CAMERA_MATRIX,
DISTORTION_COEFFICIENTS,
OBSTACLE_ARUCO_MARKER_SIZE,
OBSTACLE_HEIGHT,
)
image_calibrator = OpenCvCalibrator(CALIBRATION_FILE_PATH)
maze_factory = MazeFactory(ROBOT_RADIUS, OBSTACLE_RADIUS)
table_detector = OpenCvTableDetector()
self._world_camera = self._create_world_camera()
robot_detector = OpenCvRobotDetector(
DICT_4X4_50,
ROBOT_ARUCO_MARKER_ID,
ROBOT_ARUCO_MARKER_SIZE,
CAMERA_MATRIX,
DISTORTION_COEFFICIENTS,
ROBOT_HEIGHT,
)
puck_zone_center_position = Position(DEFAULT_PUCK_ZONE_POSITION[0],
DEFAULT_PUCK_ZONE_POSITION[1])
return VisionService(
starting_zone_corners_detector,
obstacle_detector,
image_calibrator,
table_detector,
self._world_camera,
maze_factory,
robot_detector,
TemplateMatchingPuckDetector(),
puck_zone_center_position,
)
