-
Notifications
You must be signed in to change notification settings - Fork 5
/
nao_nocv_2_0.py
1470 lines (1229 loc) · 54 KB
/
nao_nocv_2_0.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
## Nao functions version 2.0
## change log:
## 1.02: Added class "Region"
## 1.02: Resolution stuff.
## 1.03: Detect() now returns an object of the class "Region()"
## 1.04: Added Aldebarans face detection NaoFaceLocation().
## 1.05: Added the gesture() function and EyeLED() function.
## 1.06: Now able to look for the correct haarcascade file within the pythonpath
## 1.07: Changed Track() function to better support different frame rates
## 1.08: Added ALTrack function
## 1.09: Added second gesture in Gesture()
## 1.10: Added InitPose
## 1.11: Added Move
## 1.12: Added Crouch
## 1.13: Removed Gesture(), instead use the gesture lib. Changed comments for Move()
## 1.14: Added Play() function for playing sound files
## 1.15: Added Record() function
## 1.16: Added WalkTo function
## 1.17: Added PlaySine function
## 1.18: Added function FindFace()
## 1.19: Added RunMovement() (19-09-2011 - Turin)
## 1.20: Added Stiffen() for stiffening the joints
## 1.21: Added RunLed() for running led scripts
## 1.22: GetAvailableLEDPatterns() and GetAvailableGestures() added.
## 1.23: speechProxy added
## 1.24: File existence check added in RunMovement, RunLed, RunSpeech
## 1.25: Fixed remove = remove.reverse() returning None error
## 1.26: Added InitSpeech() and DetectSpeech()
## 1.27: GetAvailableDialogs() added.
## 1.28: Added LoadDialog()
## 1.29: Changed searchpaths of RunLED, RunMovement and RunSpeech to include /led, /gestures and /tts subfolders, respectively.
## 1.30: Added possibility of sending port number to InitProxy
## 1.31: Added better error handling in several functions and made posting of text optional.
## 1.32: RunLED changed to read files with ; as delimiter and to deal with multi-line led-files
## 1.33: LoadDialog() reads files with ; as delimiter
## 1.34: Added functions MoveHead() to move nao's head and GetYaw() to request the yaw of nao's head
## 1.35: Added functions SetTTSVolume() and GetTTSVolume() for checking and controlling the volume of the Text to Speech
## 1.36: Added functions SetMusicVolume() and GetMusicVolume() for checking and controlling the volume of the Music
## 1.37: Updated FindFace to include arbitrary offset and gain. Default changed to up -0.2.
## 1.38: Speed of GetImage() improved. Removed dependency on Python Image Library
## 1.39: Removed "from naoqi import xxx" statements.
## 1.40: Added ALRobotPosture proxy, GoToPosture and proper InitPose() and Crouch(); InitProxy rewritten
## 1.41: Added Landmark detection, Sound localization and Sound detection
## 1.42: Added GetGyro, GetAccel, GetTorsoAngle, and GetFootSensors
### nao_nocv.py
## 1.0 removed dependencies on OpenCV and Image libraries. InitVideo and GetImage modified, but broken.
## 1.1 incorporated updates from nao.py version 1.38
## 1.37: Updated FindFace to include arbitrary offset and gain. Default changed to up -0.2.
## 1.38: Speed of GetImage() improved. Removed dependency on Python Image Library
## 1.39: Removed "from naoqi import xxx" statements.
## 1.2 updated to match nao.py version 1.40
## 1.40: Added ALRobotPosture proxy, GoToPosture and proper InitPose() and Crouch(); InitProxy rewritten;
## 1.3 updated to match nao.py version 1.41
# 1.41: Added Landmark detection, Sound localization and Sound detection
## 2.0 updated to naoqi version 2.x
# 1.42: Added GetGyro, GetAccel, GetTorsoAngle, and GetFootSensors
#import cv
from time import time
from time import sleep
#import Image
import random
import math
import sys
import os
import csv
import numpy as np
import naoqi
from collections import deque
__naoqi_version__='2.1'
__nao_module_name__ ="Nao Library"
gftt_list = list() # initialize good features to track for opencv
fast = 0 # initiliaze face detection state for opencv
time_q = deque([1,1,1,1,1,1,1,1,1,1])
old_time = time()
time_old_track = time()
#font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.5, 0.5, 0.0, 1)
## Find the *.xml file for face detection.
list_path = sys.path
for i in range (0,len(list_path)):
if os.path.exists(list_path[i]+"/haarcascade_frontalface_alt2.xml"):
break
#cascade_front = cv.Load(list_path[i]+"/haarcascade_frontalface_alt2.xml")
interpol_time=0.3
start_mov_t = time()
weights = list()
existence = list()
id_pose = None
alface_subscribed = False
xtargetold = 0
ytargetold = 0
class ResolutionCamera:
def __init__(self):
self.low = 0
self.medium = 1
self.high = 2
self.very_high=3
self.res_160x120 = 0 #kQQVGA
self.res_320x240 = 1 #kQVGA
self.res_640x480 = 2 #kVGA
self.res_1280x960 = 3 #k4VGA
self.resolutionar = [160,120],[320,240],[640,480],[1280,960]
self.framerate=30
camera_resolution = ResolutionCamera()
class Region:
def __init__(self):
self.x = 0
self.y = 0
self.width = 0
self.height = 0
def Say(text, POST=True):
global tts
#print text
try:
#volume=GetTTSVolume()
SetTTSVolume(0.99)
if POST:
tts.post.say(text)
else:
tts.say(text)
#SetTTSVolume(volume)
except NameError:
print ('ALTextToSpeech proxy undefined. Are you running a simulated naoqi?')
def HeadTouch():
head_touch = memoryProxy.getData("Device/SubDeviceList/Head/Touch/Front/Sensor/Value", 0)
return head_touch
#################################################################################
## Use this function, InitProxy, to initialise the proxy. As an argument give up
## the Ip of Nao
#################################################################################
def ConnectProxy(proxy_name, IP, PORT):
theProxy=None
try:
theProxy = naoqi.ALProxy(proxy_name, IP, PORT)
sleep(0.01)
except RuntimeError as e:
print ("Error when creating ", proxy_name ," proxy:")
print (str(e))
return theProxy
def InitProxy(IP="marvin.local", proxy=[0], PORT = 9559):
"""proxy: (list) 1->TTS, 2->audio, 3->motion, 4->memory, 5->face, 6->video, 7->LED's, 8->Track, 9->Speech, 10->Audioplayer, 11->VisionToolbox"""
global audioProxy
global motionProxy
global memoryProxy
global cameraProxy
global faceProxy
global ledProxy
global tts
global trackfaceProxy
global playProxy
global videoProxy
global asr
global speechProxy # same as asr for backward compatibility
global sonarProxy
global postureProxy
global landmarkProxy
global trackerProxy
global soundProxy
global soundLocalizationProxy
global ALModuleList
global proxyDict
ALModuleList=["ALTextToSpeech","ALAudioDevice","ALMotion","ALMemory","ALFaceDetection","ALVideoDevice","ALLeds","ALFaceTracker","ALSpeechRecognition","ALAudioPlayer","ALVideoRecorder","ALSonar","ALRobotPosture","ALLandMarkDetection","ALTracker","ALSoundDetection","ALAudioSourceLocalization"]
proxyDict={}
proxyDict=proxyDict.fromkeys(ALModuleList,None)
#proxyList=[None]*(len(ALModuleList))
# check if list is empty
if len(proxy)==0:
proxy=range(1, len(ALModuleList)+1)
else:
#if not check whether it contains a 0
if 0 in proxy:
proxy=range(1, len(ALModuleList)+1)
for i in proxy:
proxyDict[ALModuleList[i-1]]=ConnectProxy(ALModuleList[i-1],IP, PORT)
#define globals
tts=proxyDict["ALTextToSpeech"]
audioProxy=proxyDict["ALAudioDevice"]
motionProxy=proxyDict["ALMotion"]
memoryProxy=proxyDict["ALMemory"]
faceProxy=proxyDict["ALFaceDetection"]
cameraProxy=proxyDict["ALVideoDevice"]
ledProxy=proxyDict["ALLeds"]
trackfaceProxy=proxyDict["ALFaceTracker"]
asr=proxyDict["ALSpeechRecognition"]
speechProxy=asr # for backward compatibility
playProxy=proxyDict["ALAudioPlayer"]
videoProxy=proxyDict["ALVideoRecorder"]
sonarProxy=proxyDict["ALSonar"]
postureProxy=proxyDict["ALRobotPosture"]
landmarkProxy=proxyDict["ALLandMarkDetection"]
soundProxy=proxyDict["ALSoundDetection"]
soundLocalizationProxy=proxyDict["ALAudioSourceLocalization"]
trackerProxy=proxyDict["ALTracker"]
def InitSonar(flag=True):
#period = 100
#precision = 0.1
if flag:
#sonarProxy.subscribe("test4", period , precision )
sonarProxy.subscribe("test4" )
else:
try:
sonarProxy.unsubscribe("test4" )
flag=False
except:
print ("Sonar already unsubscribed")
flag=False
return flag
#################################################################################
## Use this function, CloseProxy, to close the proxy. As an argument give up
## the Ip of Nao
#################################################################################
def CloseProxy(proxy=[0]):
"""proxy: (list) 1->TTS, 2->audio, 3->motion, 4->memory, 5->face, 6->video, 7->LED's, 8->Track, 9->Speech, 10->Audioplayer, 11->VisionToolbox"""
global ALModuleList
global proxyDict
# check if list is empty
if len(proxy)==0:
proxy=range(1, len(ALModuleList)+1)
else:
#if not check whether it contains a 0
if 0 in proxy:
proxy=range(1, len(ALModuleList)+1)
for i in proxy:
try:
proxyDict[ALModuleList[i-1]].exit()
sleep(0.1)
#print "Proxy ALTextToSpeech established"
except RuntimeError as e:
print ("Error when deleting ", ALModuleList[i-1], " TextToSpeech proxy:")
print (str(e))
#redefine globals (one or more are set to None)
tts=proxyDict["ALTextToSpeech"]
audioProxy=proxyDict["ALAudioDevice"]
motionProxy=proxyDict["ALMotion"]
memoryProxy=proxyDict["ALMemory"]
faceProxy=proxyDict["ALFaceDetection"]
cameraProxy=proxyDict["ALVideoDevice"]
ledProxy=proxyDict["ALLeds"]
trackfaceProxy=proxyDict["ALFaceTracker"]
asr=proxyDict["ALSpeechRecognition"]
speechProxy=asr # for backward compatibility
playProxy=proxyDict["ALAudioPlayer"]
videoProxy=proxyDict["ALVideoRecorder"]
sonarProxy=proxyDict["ALSonar"]
postureProxy=proxyDict["ALRobotPosture"]
landmarkProxy=proxyDict["ALLandMarkDetection"]
################################################################################
## nao.ALFacePosition() subscribes to faceProxy and returns location of face.
## It uses the embedded functions of Aldebaran face detection. If you want to
## change the period, you will have to first unsubscribe using switch = False.
## It returns [face_location,detected]. detected = whether a face has been seen.
################################################################################
def ALFacePosition(switch = True, period = 100):
global alface_subscribed
if alface_subscribed == False:
faceProxy.subscribe("Test_Face", period, 0.0)
alface_subscribed = True
location_face = memoryProxy.getData("FaceDetected")
if switch == False:
faceProxy.unsubscribe("Test_Face")
alface_subscribed == False
#print " location face: " , location_face
if location_face==None:
location_face=[]
if len(location_face) >= 2: # Changed with respect to old naoqi versions
return [-location_face[1][0][0][1],location_face[1][0][0][2]], True
else:
return [], False
def DetectFace(switch = True, period = 100):
facePosition, detected = ALFacePosition(switch, period)
timestamp=time()
return detected, timestamp, facePosition # for consistency with DetectSound DetectLandmark etc
###############################################################################
## EyesLED() can change the color of the leds. The color parameter sets
## the color in RGB values.
## The standard color is off, [0,0,0]. The interpolation time defines the time
## in seconds it will take to fully switch to the new color.
###############################################################################
def EyeLED(color=[0,0,0],interpol_time = 0, POST=True):
sGroup = "FaceLeds"
try:
if POST:
ledProxy.post.fadeRGB(sGroup, 256*256*color[0] + 256*color[1] + color[2],interpol_time)
else:
ledProxy.fadeRGB(sGroup, 256*256*color[0] + 256*color[1] + color[2],interpol_time)
except NameError:
print ('ALLeds proxy undefined.')
###############################################################################
## This function returns the available gestures located in the gesture dir.
###############################################################################
def GetAvailableGestures():
"""Returns available gestures in a list"""
list_path = sys.path
found = 0
for i in range (0,len(list_path)):
if os.path.exists(list_path[i]+"/gestures"):
found = 1
break
if found == 0:
print ("Could not find /gestures directory!")
raise IOError
return None
remove = []
list_gestures = os.listdir(list_path[i]+"/gestures")
for i in range(len(list_gestures)):
list_gestures[i] = "/gestures/"+list_gestures[i]
if not list_gestures[i].endswith(".py") and not list_gestures[i].endswith(".ges"):
remove.append(i)
## remove non py files
remove.reverse()
for i in range(len(remove)):
list_gestures.pop(remove[i])
return list_gestures
###############################################################################
## This function returns the available gestures located in the gesture dir.
###############################################################################
def GetAvailableLEDPatterns():
"""Returns available gestures in a list"""
list_path = sys.path
found = 0
for i in range (0,len(list_path)):
if os.path.exists(list_path[i]+"/led"):
found = 1
break
if found == 0:
print ("Could not find /led directory!")
raise IOError
return None
list_led = os.listdir(list_path[i]+"/led")
remove = []
for i in range(len(list_led)):
list_led[i] = "/led/"+list_led[i]
if not list_led[i].endswith(".csv") and not list_led[i].endswith(".led"):
remove.append(i)
## remove non csv files
remove.reverse()
for i in remove:
list_led.pop(i)
return list_led
###############################################################################
## This function returns the available dialogs located in the dialogs dir.
###############################################################################
def GetAvailableDialogs():
"""Returns available dialogs in a list"""
list_path = sys.path
found = 0
for i in range (0,len(list_path)):
if os.path.exists(list_path[i]+"/dialogs"):
found = 1
break
if found == 0:
print ("Could not find /dialogs directory!")
raise IOError
return None
list_dlg = os.listdir(list_path[i]+"/dialogs")
remove = []
for i in range(len(list_dlg)):
list_dlg[i] = "/dialogs/"+list_dlg[i]
if not list_dlg[i].endswith(".csv") and not list_dlg[i].endswith(".dlg"):
remove.append(i)
## remove non csv files
remove.reverse()
for i in remove:
list_dlg.pop(i)
return list_dlg
#########################################################################
## Loads a dialog csv file and converts its logic and questions/messages
## to dictionaires for use in a smach state machine
#########################################################################
def LoadDialog(file_name):
""" Give the filename of the dialog in the /dialogs folder. Extension should be .csv or .dlg."""
list_path = sys.path
filefound=False
for i in range (0,len(list_path)):
if os.path.exists(list_path[i]+"/dialogs/"+file_name):
filefound=True
break
if not filefound:
print ("Dialog file "+str(file_name)+" not found in PYTHONPATH")
return
file_load = open(list_path[i]+"/dialogs/"+file_name)
#read all rows of CSV file (assumes delimiter is ';')
csv_reader = csv.reader(file_load, delimiter=';')
return csv_reader
################################################################################
## nao.InitVideo() initialises the cv image and sets the variables on Nao.
## It allows you to give up the resolution. But first execute nao.InitProxy()
################################################################################
def InitVideo(resolution_id):
global key
global nameId
global cameraProxy
global nao_img
res = camera_resolution.resolutionar[resolution_id]
framerate=camera_resolution.framerate
kALColorSpace=0 #BGR: 11, RGB: 13
try:
nameId = cameraProxy.subscribe("python_GVM2"+str(random.random()*10), resolution_id, kALColorSpace, framerate) #0, 0, 10
except NameError:
print ('ALVideoDevice proxy undefined. Are you running a simulated naoqi?')
return None
nao_img = np.zeros((res[0], res[1],3),dtype=np.uint8)
return nao_img
#################################################################################
## nao.GetImage() gets the image from Nao. You will fist need to execute
## nao.Initvideo()
#################################################################################
def GetImage():
global nao_img
gotimage = False
count = 0
while not gotimage and count < 10:
try:
img =cameraProxy.getImageRemote(nameId)
nao_img=img[6]
# pi=Image.frombuffer("L",(img[0],img[1]),img[6])
gotimage =True
except NameError:
print ('ALVideoDevice proxy undefined. Are you running a simulated naoqi?')
break
except:
count = count + 1
print("problems with video buffer!! Did you initialize nao.InitVideo() the video first?")
# cv.SetData(cv_im, pi.tostring())
# cv.Flip(cv_im,cv_im,0)
# key = cv.WaitKey(10)
return nao_img
################################################################################
## Initializes the track function it stiffens the joints, gathers the IDPose
################################################################################
def InitTrack():
global xtargetold
global ytargetold
xtargetold = 0
ytargetold = 0
# Stiffening the head joints
motionProxy.stiffnessInterpolation('HeadYaw', 1.0, 1.0)
motionProxy.stiffnessInterpolation('HeadPitch', 1.0, 1.0)
interpol_time = 0.5
names = ["HeadYaw","HeadPitch"]
################################################################################
## Releasing stiffness of the head joints
################################################################################
def EndTrack():
motionProxy.stiffnessInterpolation('HeadYaw', 0.0, 1.0)
motionProxy.stiffnessInterpolation('HeadPitch', 0.0, 1.0)
################################################################################
## If the tracking function is initialised you can let nao follow a point in
## the camera stream the boolean "detected" specifies whether the target
## was detected. "frametime" is the time between frames.
################################################################################
def Track(target_loc, detected, speed = 5, min_move = 0.04):
"""
target_loc = the location Nao's head should move to in radians
detected = is the head detected, If False target_loc is not used and speed of movement gradually decreases
(optional) speed = the speed of the movement
(optional) min_move = the minimal angle of difference between the target_loc and current location for movements to occur.
"""
global xtargetold
global ytargetold
global time_old_track
global id_pose
global interpol_time
global start_mov_t
interpol_time = 1.0/speed
xtarget = target_loc[0]
ytarget = target_loc[1]
try:
frametime = time() - time_old_track
time_old_track = time()
except:
print ("Not able to determine frame rate. Guessing...")
frametime = 0.15
if detected == False:
xtarget = xtargetold-xtargetold*(frametime)
ytarget = ytargetold-ytargetold*(frametime)
xtargetold = xtarget
ytargetold = ytarget
if ((xtarget > min_move or xtarget < -min_move) or (ytarget > min_move or ytarget < -min_move)):
names = ["HeadYaw","HeadPitch"]
try:
id_pose
except NameError:
id_pose = None
if id_pose != None:
#motionProxy.stop(id_pose)
pass
try:
id_pose = motionProxy.post.angleInterpolation(names, [-xtarget/2.5,ytarget/2.5] , interpol_time, False)
except RuntimeError:
print ("Kan hoofd niet draaien")
start_mov_t = time()
################################################################################
## Is used to see if Nao's head is moving.
################################################################################
def MovingHead():
time_mov = time()-start_mov_t
if time_mov > 2*interpol_time:
return False
else:
return True
return
###############################################################################
## !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
## !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
################################################################################
# Track Face
################################################################################
def ALTrack(switch=1):
"""Turn head tracking on or off. Or get status = 2"""
## if switch == 1:
## InitTrack()
## trackfaceProxy.startTracker()
## elif switch == 0:
## trackfaceProxy.stopTracker()
## #EndTrack()
## else:
## return trackfaceProxy.isActive()
Tracker(switch)
def Tracker(switch=1,targetName="Landmark", targetParam=0.1):
"""Turn tracker on or off. Or get status = 2"""
##Target Parameters Comment
##RedBall diameter of ball (meter) Used to compute the distance between robot and ball.
##Face width of face (meter) Used to compute the distance between robot and face.
##LandMark [size, [LandMarkId, ...]] size is used to compute the distance between robot and LandMark. LandMarkId to specify the LandMark to track.
##LandMarks [[size, [LandMarkId, ...]], [size, [LandMarkId, ...]]] Same parameters as LandMark. One array by LandMark.
##People [peopleId, ...] Used to track a specific people.
##Sound [distance, confidence] distance is used to estimate sound position and confidence to filter sound location.
if switch == 1:
InitTrack()
# Add target to track.
trackerProxy.registerTarget(size, LandMarkId)
# Then, start tracker.
trackerProxy.track(size)
elif switch == 0:
trackerProxy.stopTracker()
trackerProxy.unregisterAllTargets()
#EndTrack()
else:
return trackfaceProxy.isActive()
##############################################################################
## Go to one of the predefined postures
#############################################################################
def GoToPosture(thePosture, speed=0.5):
# "StandInit"
# "SitRelax"
# "StandZero"
# "LyingBelly"
# "Sit"
# "LyingBack"
# "Stand"
# "Crouch"
postureProxy.goToPosture(thePosture, speed)
def version(string):
n=string.find('.') # find first period
a=string[:n+1] # cut out the main version number plus period
b=string[n+1:].replace('.','') #remove any trailing periods
return float(a+b) # convert to float
##############################################################################
## Put's Noa into its Initpose. Only use when standing or in crouch position.
#############################################################################
def InitPose(time_pos=0.5, speed=0.8):
"""Nao will move to initpose."""
Move(0.0,0.0,0.0) # stop moving
sleep(0.1)
# set stiffness
motionProxy.stiffnessInterpolation('Body',1.0, time_pos)
GoToPosture("Stand", speed)
#sleep(0.5)
#IP.initPose(motionProxy)
# numJoints = len(motionProxy.getJointNames('Body'))
# allAngles = [0.0,0.0, # head
# 1.39, 0.34, -1.39, -1.04, 0.0, 0.0, # left arm
# 0.0, 0.0, -0.43, 0.69, -0.34, 0.0, # left leg
# 0.0, 0.0, -0.43, 0.69, -0.34, 0.0, # right leg
# 1.39, -0.34, 1.39, 1.04, 0.0, 0.0] # right arm
# #printnumJoints
# if (numJoints == 26):
# angles = allAngles
# elif (numJoints == 22): # no hands (e.g. simulator)
# angles = allAngles[0:6] + allAngles[8:24]
# else:
# print "error in Init Pose"
# try:
# motionProxy.post.angleInterpolation('Body', angles, 1.5, True);
# except RuntimeError,e:
# print "An error has been caught"
# print e
def Stiffen(stiffness = True, int_time=1):
"""Make Nao stiffen its joints (Can be True or False)"""
motionProxy.stiffnessInterpolation('Body',int(stiffness), int_time)
def StiffenUpperBody(stiffness = True, int_time=0.1):
"""Make Nao stiffen its joints (Can be True or False)"""
motionProxy.setStiffnesses('HeadPitch',int(stiffness), int_time)
motionProxy.setStiffnesses('HeadYaw',int(stiffness), int_time)
motionProxy.setStiffnesses('LElbowRoll',int(stiffness), int_time)
motionProxy.setStiffnesses('LElbowYaw',int(stiffness), int_time)
motionProxy.setStiffnesses('LHand',int(stiffness), int_time)
motionProxy.setStiffnesses('LShoulderPitch',int(stiffness), int_time)
motionProxy.setStiffnesses('LShoulderRoll',int(stiffness), int_time)
motionProxy.setStiffnesses('LWristYaw',int(stiffness), int_time)
motionProxy.setStiffnesses('RElbowRoll',int(stiffness), int_time)
motionProxy.setStiffnesses('RElbowYaw',int(stiffness), int_time)
motionProxy.setStiffnesses('RHand',int(stiffness), int_time)
motionProxy.setStiffnesses('RShoulderPitch',int(stiffness), int_time)
motionProxy.setStiffnesses('RShoulderRoll',int(stiffness), int_time)
motionProxy.setStiffnesses('RWristYaw',int(stiffness), int_time)
################################################################################
## Nao crouches and loosens it's joints.
###############################################################################
def Crouch(speed=0.8):
Move(0.0,0.0,0.0) # stop moving
sleep(0.1)
GoToPosture("Crouch", speed)
motionProxy.stiffnessInterpolation('Body',0, 0.5)
##def Crouch():
## """Make Nao to crouch pose."""
##
## # get the robot config
## robotConfig = motionProxy.getRobotConfig()
##
## #for i in range(len(robotConfig[0])):
## # print robotConfig[0][i], ": ", robotConfig[1][i]
##
## # "Model Type" : "naoH25", "naoH21", "naoT14" or "naoT2".
## # "Head Version" : "VERSION_32" or "VERSION_33" or "VERSION_40".
## # "Body Version" : "VERSION_32" or "VERSION_33" or "VERSION_40".
## # "Laser" : True or False.
## # "Legs" : True or False.
## # "Arms" : True or False.
## # "Extended Arms": True or False.
## # "Hands" : True or False.
## # "Arm Version" : "VERSION_32" or "VERSION_33" or "VERSION_40".
## # Number of Legs : 0 or 2
## # Number of Arms : 0 or 2
## # Number of Hands: 0 or 2
## if robotConfig[1][0]=="naoH25" or robotConfig[1][0]=="naoH21":
## pass
## else:
## print "Wrong robot type: cannot crouch without arms and legs"
## return
## if robotConfig[1][8]=="VERSION_32":
## allAngles = [0.0,0.0, # head
## 1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm
## -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg
## -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg
## 1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm
## elif robotConfig[1][8]=="VERSION_33":
## #Modified for robot version V33
## allAngles = [0.0,0.0, # head
## 1.545, 0.2, -1.56, -0.5, 0.0, 0.0, # left arm
## -0.319, 0.037, -0.695, 2.11, -1.189, -0.026, # left leg
## -0.319, 0.037, -0.695, 2.11, -1.189, -0.026, # right leg
## 1.545, -0.2, 1.56, 0.5, 0.0, 0.0] # right arm
## else:
## #Modified for robot version V4.0
## allAngles = [0.0,0.0, # head
## 1.53, 0.15, -1.56, -0.5, 0.0, 0.0, # left arm
## -0.30, 0.05, -0.75, 2.11, -1.19, -0.04, # left leg
## -0.30, 0.05, -0.75, 2.11, -1.19, -0.04, # right leg
## 1.53, -0.15, 1.56, 0.5, 0.0, 0.0] # right arm
##
## numJoints = len(motionProxy.getJointNames('Body'))
## if (numJoints == 26):
## angles = allAngles
## elif (numJoints == 22): # no hands (e.g. simulator)
## angles = allAngles[0:6] + allAngles[8:24]
## else:
## print "error in numJoints"
##
## try:
## motionProxy.angleInterpolation('Body', angles, 1.5, True);
##
## except RuntimeError,e:
## print "An error has been caught"
## print e
##
## motionProxy.stiffnessInterpolation('Body',0, 0.5)
##################################################################################
## Allows Nao to move in a certain direction with a certain speed.
################################################################################
def Move(dx=0, dy=0, dtheta=0, freq=1):
""""
dx = forward speed, dtheta = rotational speed,
dy = sidewards speed, freq = step frequency.
Allows Nao to move in a certain direction
with a certain speed.
"""
if version(__naoqi_version__)<2:
motionProxy.setWalkTargetVelocity(dx, dy, dtheta, freq)
else:
motionProxy.moveToward(dx,dy,dtheta)
def ReadSonar():
"""Read the left and right Sonar Values"""
SonarLeft = "Device/SubDeviceList/US/Left/Sensor/Value"
SonarRight = "Device/SubDeviceList/US/Right/Sensor/Value"
SL=memoryProxy.getData(SonarLeft,0) # read sonar left value from memory
SR=memoryProxy.getData(SonarRight ,0) # read sonar right value from memory
return SL, SR
def GetGyro():
"""Get the Gyrometers Values"""
GyrX = memoryProxy.getData("Device/SubDeviceList/InertialSensor/GyrX/Sensor/Value")
GyrY = memoryProxy.getData("Device/SubDeviceList/InertialSensor/GyrY/Sensor/Value")
return GyrX, GyrY
def GetAccel():
"""Get the Accelerometers Values"""
AccX = memoryProxy.getData("Device/SubDeviceList/InertialSensor/AccX/Sensor/Value")
AccY = memoryProxy.getData("Device/SubDeviceList/InertialSensor/AccY/Sensor/Value")
AccZ = memoryProxy.getData("Device/SubDeviceList/InertialSensor/AccZ/Sensor/Value")
return AccX, AccY, AccZ
def GetTorsoAngle():
"""Get the Computed Torso Angle in radians"""
TorsoAngleX = memoryProxy.getData("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value")
TorsoAngleY = memoryProxy.getData("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value")
return TorsoAngleX, TorsoAngleY
def GetFootSensors():
"""Get the foot sensor values"""
# Get The Left Foot Force Sensor Values
LFsrFL = memoryProxy.getData("Device/SubDeviceList/LFoot/FSR/FrontLeft/Sensor/Value")
LFsrFR = memoryProxy.getData("Device/SubDeviceList/LFoot/FSR/FrontRight/Sensor/Value")
LFsrBL = memoryProxy.getData("Device/SubDeviceList/LFoot/FSR/RearLeft/Sensor/Value")
LFsrBR = memoryProxy.getData("Device/SubDeviceList/LFoot/FSR/RearRight/Sensor/Value")
LF=[LFsrFL, LFsrFR, LFsrBL, LFsrBR]
# Get The Right Foot Force Sensor Values
RFsrFL = memoryProxy.getData("Device/SubDeviceList/RFoot/FSR/FrontLeft/Sensor/Value")
RFsrFR = memoryProxy.getData("Device/SubDeviceList/RFoot/FSR/FrontRight/Sensor/Value")
RFsrBL = memoryProxy.getData("Device/SubDeviceList/RFoot/FSR/RearLeft/Sensor/Value")
RFsrBR = memoryProxy.getData("Device/SubDeviceList/RFoot/FSR/RearRight/Sensor/Value")
RF=[RFsrFL, RFsrFR, RFsrBL, RFsrBR]
return LF, RF
##################################################################################
## Allows Nao to move dx meters forward, dy meters sidways with final orientation of dtheta
################################################################################
def Walk(dx=0,dy=0,dtheta=0,post=False):
""""
dx = forward meters, dtheta = final angle,
dy = sidewards meters
Allows Nao to move in a certain direction.
"""
if version(__naoqi_version__)<2:
if post==False:
motionProxy.walkTo(dx, dy, dtheta)
else:
motionProxy.post.walkTo(dx, dy, dtheta)
else:
if post==False:
motionProxy.moveTo(dx, dy, dtheta)
else:
motionProxy.post.moveTo(dx, dy, dtheta)
##################################################################################
## Moves nao head yaw and pitch of the provided values yaw_val and pitch_val
################################################################################
def MoveHead(yaw_val=0, pitch_val=0, isAbsolute=True, post=True, timeLists= [[1],[1]]):
names = ["HeadYaw", "HeadPitch"]
angleLists = [[yaw_val], [pitch_val]]
if post==False:
motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)
else:
motionProxy.post.angleInterpolation(names, angleLists, timeLists, isAbsolute)
def GetYaw():
names = "HeadYaw"
useSensors = True
HeadYaw = motionProxy.getAngles(names, useSensors)
return HeadYaw
def GetPitch():
names = "HeadPitch"
useSensors = True
HeadPitch = motionProxy.getAngles(names, useSensors)
return HeadPitch
def GetYawPitch():
names = ["HeadYaw", "HeadPitch"]
useSensors = True
HeadYawPitch = motionProxy.getAngles(names, useSensors)
return HeadYawPitch
##################################################################################
## Allows Nao to play a sinusoidal wave of frequency in Hertz p1, Volume gain 0-100 p2, Stereo Pan set to either {-1,0,+1} p3 , duration in seconds
################################################################################
def PlaySine(p1,p2,p3,duration):
global audioProxy
try:
audioProxy.playSine(p1,p2,p3,duration)
except NameError:
print ('ALAudioDevice proxy undefined. Are you running a simulated naoqi?')
###################
#stop music
###########################
def StopPlay():
playProxy.stopAll()
######################################################
# Use this class and it's Play() function to play a wav or mp3 file on Nao.
# The files should be uploaded via ftp. Go to ftp://username:password@nao's_ip
# And upload them it's initial directory, which is /home/nao/ .
# id_music
######################################################
def Play(file_name):
"""Plays a audio file on Nao, it runs the file from the /home/nao/ directory"""
file_name = "/home/nao/"+file_name
id_music=playProxy.post.playFile(file_name)
return id_music
###########
# Pause
########################
def Pause(id_music):
playProxy.post.pause(id_music)
########################
#playFileFromPosition
##############################
def playFileFromPosition(file_name, position):
file_name = "/home/nao/"+file_name
id_music=playProxy.post.playFileFromPosition(file_name, position)
return id_music
##########################
#Set Volume TTS
##################################
def SetTTSVolume(volume):
tts.setVolume(volume)
##########################
#Get Volume TTS
##################################
def GetTTSVolume():
vol=tts.getVolume()
return vol
##########################
#Set Volume Music
##################################
def SetMusicVolume(id_music,volume):
playProxy.setVolume(id_music,volume)
##########################
#Get Volume Music
##################################
def GetMusicVolume():
vol=playProxy.getMasterVolume()
return vol
###############################################################################
## This will record a file located in the /home/nao/naoqi/share/naoqi/vision
## Directory on Nao
###############################################################################
def Record(file_name, fps = 3.0, resolution = 0):
"""
file_name without extension. fps, should be higher than 3.0.
resolution shoud be between 0 and 2.
Saved in /home/nao/naoqi/share/naoqi/vision