def markers(args, must_print=True):
global nao_motion
data.IP = args['--ip']
data.PORT = int(args['--port'])
next_img_func = get_nao_image
try:
if args['--no-robot']:
next_img_func = get_webcam_image
except KeyError:
pass
end_reached = False
nao_motion = MotionController()
nao_motion.stand()
while not end_reached:
img = next_img_func(int(args['--cam-no']), res=2)
detected_markers = detect_markers(img)
for m in detected_markers:
m.draw_contour(img)
cv2.putText(img, str(m.id), tuple(int(p) for p in m.center),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 3)
if must_print:
print "ID: ", str(m.id), "\n", "Contours: ", str(m.contours.ravel().reshape(-1, 2).tolist())
print
else:
end_reached = True
print
if not args['--no-image']:
cv2.imshow('Markers', img)
if cv2.waitKey(1) == 27:
print "Esc pressed : exit"
close_camera()
return 0
def coordinates(args):
data.IP = args['--ip']
data.PORT = int(args['--port'])
global nao_motion
nao_motion = MotionController()
camera_position_func = nao_motion.getCameraBottomPositionFromTorso
if int(args['--cam-no']) == 0:
camera_position_func = nao_motion.getCameraTopPositionFromTorso
myc4 = Connect4Handler(get_nao_image, cam_no=int(args['--cam-no']))
tries = int(args['--min-detections'])
if args['--board']:
dist = float(args['--dist'])
sloped = args['--sloped']
while True:
try:
coords = myc4.getUpperHoleCoordinatesUsingFrontHoles(dist, sloped, int(args['--hole']),
camera_position_func(),
camera_matrix=data.CAM_MATRIX,
camera_dist=data.CAM_DISTORSION,
tries=tries, debug=not(args['--no-image']))
# We want the coordinates of the center of the hole, not the hand ideal position
coords[2] -= 0.12
coords[5] -= 0.505
coords[1] -= 0.028
coords[0] += 0.25
print coords
except FrontHolesGridNotFoundException:
print
if not(args['--no-image']):
if cv2.waitKey(50) == 27:
print "Esc pressed : exit"
close_camera()
return 0
else:
nao_motion.stand()
while True:
try:
coords = myc4.getUpperHoleCoordinatesUsingMarkers(int(args['--hole']),
camera_position_func(),
data.CAM_MATRIX, data.CAM_DISTORSION,
res=640,
debug=False)
if not args['--no-image']:
markers(args, must_print=False)
# We want the coordinates of the center of the hole, not the hand ideal position
coords[2] -= 0.12
coords[5] -= 0.505
coords[1] -= 0.028
coords[0] += 0.01
print coords
sleep(0.5)
except NotEnoughLandmarksException:
pass
if cv2.waitKey(50) == 27:
print "Esc pressed : exit"
close_camera()
return 0
return 0
def ik(args):
global broker, nao_motion, nao_video
data.IP = args['--ip']
data.PORT = int(args['--port'])
broker = ALBroker("myBroker", "0.0.0.0", 0, data.IP, data.PORT)
nao_motion = MotionController()
nao_video = VideoController()
nao_motion.stand()
nao_tts = ALProxy("ALTextToSpeech", data.IP, data.PORT)
try:
loop = LogicalLoop(nao_motion=nao_motion, nao_video=nao_video, nao_tts=nao_tts,
ppA=float(args['--ppA']), cA=float(args['--cA']), rA=float(args['--rA']),
min_detections=1, wait_disc_func=lambda: None)
if not args['--no-grab']:
wait_for_disc()
else:
nao_motion.motion_proxy.closeHand("LHand")
nao_motion.setLeftArmRaised(secure=True)
loop.inverseKinematicsConvergence(int(args['--hole']))
nao_motion.crouch()
except KeyboardInterrupt:
print "Keyboard interrupt"
broker.shutdown()
return 0
