image_R2D2 = cv2.flip(
cv2.imread("images/R2-D2_s.png", cv2.IMREAD_COLOR), -1)
image_BB8 = cv2.flip(cv2.imread("images/BB8_s.png", cv2.IMREAD_COLOR),
-1)
our, other = (image_R2D2, image_BB8) if not Cfg.bb8 else (image_BB8,
image_R2D2)
capture = None if not Cfg.use_image else cv2.flip(
cv2.imread(Cfg.use_image, cv2.IMREAD_COLOR), -1)
# 2.
print("Press ESC to quit")
while cv2.waitKey(1000) != 27:
if not Cfg.use_image:
foundOther, coordinatesOther = robot.detectImage(other)
foundOur, coordinatesOur = robot.detectImage(our)
else:
foundOther, coordinatesOther = match_images(other, capture)
foundOur, coordinatesOur = match_images(our, capture)
if foundOur and foundOther:
if coordinatesOur[0] > coordinatesOther[
0]: # the camera is inverted
print('Our is at the left')
else:
print('Our is at the right')
else:
print('not found', foundOur, foundOther)
finally:
# robot.rotate(np.deg2rad(-90))
#
# robot.advance(robot.getObstacleDistance() - GRID * 1.5)
# dist = robot.updateOdOnWall(ANG=30)
#
# robot.onMarker(y=GRID * 8 - dist, th=np.deg2rad(90), now=True)
# position looking at the images
go(0.5, 6)
lookAt(0.5, 7)
# detect image
periodic = Periodic(1)
rotateLeft = 1
while periodic():
foundOur, coordinatesOur = robot.detectImage(IMAGE_OUR)
foundOther, coordinatesOther = robot.detectImage(IMAGE_OTHER)
if foundOur and foundOther:
leftExit = coordinatesOur[0] > coordinatesOther[
0] # the camera is inverted
break
else:
robot.setSpeed(0, np.deg2rad(7.5) * rotateLeft)
rotateLeft = 1 - rotateLeft
# Go to image wall and updateTh
go(0.5, 6.75)
dist = robot.updateOdOnWall(45)
if dist > GRID / 2:
robot.advance(dist - GRID / 2)