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, )