41 lines
907 B
Python
Executable File
41 lines
907 B
Python
Executable File
#! /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()
|