did hands

script/hand_ler.py

@@ -0,0 +1,40 @@
+#! /usr/bin/env python
+import os
+
+import rospy
+from naoqi import ALProxy
+from threading import Thread
+
+from teleoperation.srv import Hands, HandsResponse
+
+
+mp = None
+
+
+def do_in_parallel(func):
+    tl = Thread(target=func, args=('LHand',))
+    tr = Thread(target=func, args=('RHand',))
+    tl.start()
+    tr.start()
+    while tr.is_alive() and tl.is_alive():
+        rospy.Rate(10).sleep()
+
+
+def handle_hands(request):
+    if request.target == 'open':
+        do_in_parallel(mp.openHand)
+        return HandsResponse('opened')
+    elif request.target == 'close':
+        do_in_parallel(mp.closeHand)
+        return HandsResponse('closed')
+    else:
+        return HandsResponse('nope')
+
+
+if __name__ == '__main__':
+    rospy.init_node('hand_ler')
+    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+    mp.wakeUp()
+    hands = rospy.Service('hands', Hands, handle_hands)
+    rospy.spin()
+    mp.rest()