53 lines
1.2 KiB
Python
Executable File
53 lines
1.2 KiB
Python
Executable File
#! /usr/bin/env python
|
|
"""A small module for hand controls service.
|
|
|
|
ROS Node: `hand_ler`
|
|
Provides `hands` service.
|
|
"""
|
|
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):
|
|
"""Open hands in parallel.
|
|
|
|
The robot's hand function blocks and only accepts one hand. This is a
|
|
thread-based hackaround.
|
|
|
|
"""
|
|
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):
|
|
"""Hand service routine."""
|
|
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()
|