def _find_destination_center(self, image: np.ndarray,
destination: int) -> CameraCoordinate:
zone_height, zone_width = image.shape
coordinates = []
for i in range(4):
destination_width = int(zone_width / 4)
destination_start = i * destination_width
destination_end = destination_start + destination_width
destination_roi = image[:, destination_start:destination_end]
_, contours, _ = cv2.findContours(destination_roi, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
contours = [
c for c in contours
if OpenCvDestinationFinder._does_contour_fit_destination_dot(c)
]
if len(contours) == 0:
break
destination_coordinate = CameraCoordinate(-1, -1)
distance_to_center = float("inf")
roi_center_x = destination_width / 2
roi_center_y = zone_height / 2
destination_found = False
for contour in contours:
try:
contour_center = OpenCvDestinationFinder._compute_contour_center(
contour)
current_distance = sqrt(
(roi_center_x - contour_center.x)**2 +
(roi_center_y - contour_center.y)**2)
if current_distance < distance_to_center:
distance_to_center = current_distance
destination_coordinate = contour_center
destination_found = True
except ValueError:
pass
if self._debug_display.debug_active():
image_copy = cv2.cvtColor(destination_roi, cv2.COLOR_GRAY2BGR)
cv2.drawContours(image_copy, contours, -1, (0, 0, 255), 3)
cv2.circle(
image_copy,
(destination_coordinate.x, destination_coordinate.y), 3,
(0, 255, 0), 3)
cv2.imshow('_find_destination_center {}'.format(i), image_copy)
cv2.waitKey(1)
if not destination_found:
break
coordinates.append(
CameraCoordinate(destination_coordinate.x + destination_start,
destination_coordinate.y))
if len(coordinates) != 4:
raise CouldNotFindDestinationError
return coordinates[destination]
def setUp(self) -> None:
self.triangle = Item(Color.Red, Shape.Triangle, CameraCoordinate(0, 0))
self.red = self.triangle
self.square = Item(Color.Green, Shape.Square, CameraCoordinate(0, 0))
self.green = self.square
self.pentagon = Item(Color.Blue, Shape.Pentagon, CameraCoordinate(0, 0))
self.blue = self.pentagon
self.circle = Item(Color.Yellow, Shape.Circle, CameraCoordinate(0, 0))
self.yellow = self.circle
def _compute_coordinate(contour: np.ndarray, zone_x: int,
zone_y: int) -> CameraCoordinate:
moments = cv2.moments(contour)
try:
x = int(moments['m10'] / moments['m00']) + zone_x
y = int(moments['m01'] / moments['m00']) + zone_y
except ZeroDivisionError:
raise CouldNotFindItemsError
return CameraCoordinate(x, y)
def _process(self, image: np.ndarray) -> None:
grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
grey = OpenCvDestinationFinder._cut_robot(grey)
grey, zone_x, zone_y = self._cut_zone(grey)
coordinate = self._find_destination_center(grey,
self._destination_to_find)
self._destination_coordinate = CameraCoordinate(
coordinate.x + zone_x, coordinate.y + zone_y)
def convert_pixel_to_real(
self, coordinate_pixel: CameraCoordinate) -> CameraCoordinate:
pixel_vector = np.array([coordinate_pixel.x, coordinate_pixel.y,
1]).transpose()
real_vector = self._camera_matrix_inverse.dot(pixel_vector)
real_vector = np.multiply(real_vector,
camera_height_form_table_mm).transpose()
return CameraCoordinate(real_vector[0], real_vector[1])
def find_drop_position(self, objective: Objective) -> CameraCoordinate:
self._sight_service.look_down()
self._vision_service.update()
try:
destination_position = self._vision_service.find_destination_position(objective.destination)
except CouldNotFindDestinationError as e:
self._vision_service.save_image()
raise e
finally:
self._sight_service.look_ahead()
return CameraCoordinate(destination_position.x / 10, destination_position.y / 10)
def from_item_relative_position(item: ItemRelativePosition) -> Item:
if item.item.color == visionColor.Red:
color = cortexColor.Red
elif item.item.color == visionColor.Green:
color = cortexColor.Green
elif item.item.color == visionColor.Blue:
color = cortexColor.Blue
else:
color = cortexColor.Yellow
if item.item.shape == visionShape.Triangle:
shape = cortexShape.Triangle
elif item.item.shape == visionShape.Square:
shape = cortexShape.Square
elif item.item.shape == visionShape.Pentagon:
shape = cortexShape.Pentagon
else:
shape = cortexShape.Circle
coordinate = CameraCoordinate(item.camera_coordinate.x / 10, item.camera_coordinate.y / 10)
return Item(color, shape, coordinate)
def from_robot(self, coord: RobotCoordinate) -> CameraCoordinate:
homogeneous_coordinate = array([coord.x, coord.y, 1])
transformed_coordinate = self._transform_matrix @ homogeneous_coordinate
return CameraCoordinate(transformed_coordinate[0],
transformed_coordinate[1])
def test_whenGettingCount_thenIsRight(self) -> None:
booth = Booth(Address(CameraCoordinate(1, 2)))
for i in [self.square, self.square, self.square, self.square, self.triangle, self.pentagon]:
booth.add_item(i)
self.assertEqual(6, booth.get_count())
def test_givenMajoritySquare_whenGettingShape_thenSquare(self) -> None:
booth = Booth(Address(CameraCoordinate(1, 2)))
for i in [self.square, self.square, self.square, self.square, self.triangle, self.pentagon]:
booth.add_item(i)
self.assertEqual(Shape.Square, booth.get_shape())
def _compute_coordinate(contour: np.ndarray, zone_x: int, zone_y: int) -> CameraCoordinate:
moments = cv2.moments(contour)
x = int(moments['m10'] / moments['m00']) + zone_x
y = int(moments['m01'] / moments['m00']) + zone_y
return CameraCoordinate(x, y)
def __init__(self) -> None:
self._destination_to_find = 0
self._destination_coordinate = CameraCoordinate(0, 0)
self._debug_display = OpenCvDebugDisplay()
def _compute_contour_center(contour: np.ndarray) -> CameraCoordinate:
moments = cv2.moments(contour)
x = int(moments['m10'] / moments['m00'])
y = int(moments['m01'] / moments['m00'])
return CameraCoordinate(x, y)
class DexterityCortex:
prehensor_position = CameraCoordinate(2.5, 1.4)
def __init__(self, mobility_service: MobilityService,
dexterity_service: DexterityService,
direction_cortex: DirectionCortex,
visual_cortex: IVisualCortex,
item_chooser: IItemChooser) -> None:
self._mobility_service = mobility_service
self._dexterity_service = dexterity_service
self._direction_cortex = direction_cortex
self._visual_cortex = visual_cortex
self._item_chooser = item_chooser
def grab(self, objective: Objective) -> None:
found_item = False
items_in_source = []
while not found_item:
try:
items_in_source = self._find_matching_items(objective)
except NoItemMatched:
continue
except CouldNotFindItemsError:
continue
found_item = True
items_left = [item for item in items_in_source]
while len(items_left) == len(
items_in_source): # TODO check if specific item is left
self._dexterity_service.let_go()
chosen_item = items_in_source[0]
path_to_item = self._path_to_position(chosen_item.position)
self._mobility_service.drive(path_to_item)
sleep(1)
self._dexterity_service.grab()
sleep(1)
path_from_item = self._path_from_position(chosen_item.position)
self._mobility_service.drive(path_from_item)
try:
items_left = self._find_matching_items(objective, False)
except NoItemMatched:
items_left = []
except CouldNotFindItemsError:
items_left = []
def _find_matching_items(self,
objective: Objective,
move: bool = True) -> List[Item]:
if move:
self._mobility_service.operate(
[TranslateOperation(Angle(pi), Distance(5))])
self._direction_cortex.reach_source()
self._mobility_service.operate(
[TranslateOperation(Angle(pi / 2), Distance(1))])
items = self._visual_cortex.find_items()
return self._item_chooser.choose_from(objective, items)
def drop(self, objective: Objective) -> None:
found_destination = False
position = None
while not found_destination:
try:
position = self._find_destination(objective)
except CouldNotFindDestinationError:
print("CouldNotFindDestinationError")
continue
found_destination = True
path_to_position = self._path_to_position(position)
self._mobility_service.drive(path_to_position)
self._dexterity_service.let_go()
path_from_position = self._path_from_position(position)
self._mobility_service.drive(path_from_position)
def _find_destination(self, objective: Objective) -> CameraCoordinate:
self._mobility_service.operate(
[TranslateOperation(Angle(pi), Distance(5))])
self._direction_cortex.reach_goal()
self._mobility_service.operate(
[TranslateOperation(Angle(pi / 2), Distance(1))])
return self._visual_cortex.find_drop_position(objective)
def _path_to_position(self, position: CameraCoordinate) -> CameraPath:
movements = [CameraMovement(self.prehensor_position, position)]
return CameraPath(movements)
def _path_from_position(self, position: CameraCoordinate) -> CameraPath:
movements = [CameraMovement(position, self.prehensor_position)]
return CameraPath(movements)