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,
)
def _create_puck_alignment_corrector(self, ) -> PuckAlignmentCorrector:
puck_correctly_placed_position = Position(
PUCK_ALIGNMENT_X_CENTER_POSITION, PUCK_ALIGNMENT_Y_CENTER_POSITION)
return PuckAlignmentCorrector(
puck_correctly_placed_position,
PUCK_ALIGNMENT_HORIZONTAL_THRESHOLD,
PUCK_ALIGNMENT_UP_THRESHOLD,
OpenCvPuckDetector(),
TemplateMatchingPuckDetector(),
)
def _setup_vision_service(self):
self._vision_service._calibrator = MagicMock()
self._vision_service._puck_detector = TemplateMatchingPuckDetector()
self.image_paths = self._find_files_in_directory(
"../resources/camera-config/all-color")
for image_path in self.image_paths:
self.images.append(cv2.imread(image_path))
number_of_colors_to_detect = len(Color) - 1
self._vision_service._calibrator.calibrate.side_effect = [
item for sublist in [[elem] * number_of_colors_to_detect
for elem in self.images] for item in sublist
]
)
from domain.Color import Color
from domain.Position import Position
from domain.vision.exception.RobotNotFoundException import RobotNotFoundException
from domain.RobotPose import RobotPose
from domain.vision.exception.ObstacleNotFoundException import ObstacleNotFoundException
from infra.camera.OpenCvCalibrator import OpenCvCalibrator
from infra.camera.OpenCvWorldCamera import OpenCvWorldCamera
from infra.vision.OpenCvObstacleDetector import OpenCvObstacleDetector
from infra.vision.OpenCvRobotDetector import OpenCvRobotDetector
from infra.vision.TemplateMatchingPuckDetector import TemplateMatchingPuckDetector
if __name__ == "__main__":
calibrator = OpenCvCalibrator(CALIBRATION_FILE_PATH)
camera = OpenCvWorldCamera(LAPTOP_CAMERA_INDEX, calibrator)
detector = TemplateMatchingPuckDetector()
robot_detector = OpenCvRobotDetector(
DICT_4X4_50,
ROBOT_ARUCO_MARKER_ID,
ROBOT_ARUCO_MARKER_SIZE,
CAMERA_MATRIX,
DISTORTION_COEFFICIENTS,
ROBOT_HEIGHT,
)
obstacle_detector = OpenCvObstacleDetector(
DICT_4X4_50,
OBSTACLE_ARUCO_MARKER_ID,
CAMERA_MATRIX,
DISTORTION_COEFFICIENTS,
OBSTACLE_ARUCO_MARKER_SIZE,
OBSTACLE_HEIGHT,
from config.config import ROBOT_RADIUS, OBSTACLE_RADIUS, STARTING_ZONE_CENTER_POSITION
from context.StationContext import StationContext
from domain.Color import Color
from domain.Position import Position
from domain.Puck import Puck
from domain.StartingZoneCorner import StartingZoneCorner
from domain.pathfinding.AStarShortestPathAlgorithm import AStarShortestPathAlgorithm
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)
def setUp(self) -> None:
self.template_matching_puck_detector = TemplateMatchingPuckDetector()
class TestTemplateMatchingPuckDetector(TestCase):
AN_IMAGE_PATH = (
os.path.dirname(os.path.abspath(__file__))
+ "/../../../resources/camera-config/image_0.jpg"
)
AN_IMAGE = cv2.imread(AN_IMAGE_PATH)
def setUp(self) -> None:
self.template_matching_puck_detector = TemplateMatchingPuckDetector()
def test_givenColorGrey_whenDetect_thenReturnRightPositionForGreyPuck(self):
expected_position = Position(1181, 192)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.GREY
)
self.assertEqual(actual_position, expected_position)
def test_givenColorRed_whenDetect_thenReturnRightPositionForRedPuck(self):
expected_position = Position(1184, 335)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.RED
)
self.assertEqual(actual_position, expected_position)
def test_givenColorGreen_whenDetect_thenReturnRightPositionForGreenPuck(self):
expected_position = Position(1078, 182)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.GREEN
)
self.assertEqual(actual_position, expected_position)
def test_givenColorYellow_whenDetect_thenReturnRightPositionForYellowPuck(self):
expected_position = Position(967, 632)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.YELLOW
)
self.assertEqual(actual_position, expected_position)
def test_givenColorOrange_whenDetect_thenReturnRightPositionForOrangePuck(self):
expected_position = Position(888, 636)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.ORANGE
)
self.assertEqual(actual_position, expected_position)
def test_givenColorBlue_whenDetect_thenReturnRightPositionForBluePuck(self):
expected_position = Position(1072, 626)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.BLUE
)
self.assertEqual(actual_position, expected_position)
def test_givenColorPurple_whenDetect_thenReturnRightPositionForPurplePuck(self):
expected_position = Position(1155, 625)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.PURPLE
)
self.assertEqual(actual_position, expected_position)
def test_givenColorWhite_whenDetect_thenReturnRightPositionForWhitePuck(self):
expected_position = Position(902, 175)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.WHITE
)
self.assertEqual(actual_position, expected_position)
def test_givenColorBrown_whenDetect_thenReturnRightPositionForBrownPuck(self):
expected_position = Position(985, 180)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.BROWN
)
self.assertEqual(actual_position, expected_position)
def test_givenColorBlack_whenDetect_thenReturnRightPositionForBlackPuck(self):
expected_position = Position(1175, 489)
actual_position = self.template_matching_puck_detector.detect(
self.AN_IMAGE, Color.BLACK
)
self.assertEqual(actual_position, expected_position)
def test_givenAnImage_whenDetectAllColors_thenColorsAllHaveDifferentPositions(self):
possible_colors = []
for color in Color:
if color is not Color.NONE:
possible_colors.append(color)
color_positions = [
self.template_matching_puck_detector.detect(self.AN_IMAGE, color)
for color in possible_colors
]
different_positions = set(color_positions)
self.assertEqual(len(different_positions), 10)